text:
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

// Qt
#include <QLabel>
#include <QTimer>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QMessageBox>
#include <QString>
#include <QApplication>
#include <QFont>
#include <QFileDialog>
#include <QTextEdit>
// ROS
#include <ros/ros.h>
#include <ros/package.h>  // for getting file path for loadng images
// SA
#include "header_widget.h"  // title and instructions
#include "start_screen_widget.h"
// C
#include <fstream>  // for reading in urdf
#include <streambuf>
// Boost
#include <boost/algorithm/string.hpp>  // for trimming whitespace from user input
#include <boost/filesystem.hpp>        // for reading folders/files
#include <boost/algorithm/string.hpp>  // for string find and replace in paths
// MoveIt
#include <moveit/rdf_loader/rdf_loader.h>

namespace moveit_setup_assistant
{
// Boost file system
namespace fs = boost::filesystem;

// ******************************************************************************************
// Start screen user interface for MoveIt Configuration Assistant
// ******************************************************************************************
StartScreenWidget::StartScreenWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
  : SetupScreenWidget(parent), config_data_(config_data)
{
  // Basic widget container
  QVBoxLayout* layout = new QVBoxLayout(this);

  // Horizontal layout splitter
  QHBoxLayout* hlayout = new QHBoxLayout();
  // Left side of screen
  QVBoxLayout* left_layout = new QVBoxLayout();
  // Right side of screen
  QVBoxLayout* right_layout = new QVBoxLayout();

  // Right Image Area ----------------------------------------------
  right_image_ = new QImage();
  right_image_label_ = new QLabel(this);
  std::string image_path = "./resources/MoveIt_Setup_Asst_xSm.png";
  if (chdir(config_data_->setup_assistant_path_.c_str()) != 0)
  {
    ROS_ERROR("FAILED TO CHANGE PACKAGE TO moveit_setup_assistant");
  }

  if (right_image_->load(image_path.c_str()))
  {
    right_image_label_->setPixmap(QPixmap::fromImage(*right_image_));
    right_image_label_->setMinimumHeight(384);  // size of right_image_label_
    // right_image_label_->setMinimumWidth(450);
  }
  else
  {
    ROS_ERROR_STREAM("FAILED TO LOAD " << image_path);
  }

  logo_image_ = new QImage();
  logo_image_label_ = new QLabel(this);
  image_path = "./resources/moveit_logo.png";

  if (logo_image_->load(image_path.c_str()))
  {
    logo_image_label_->setPixmap(QPixmap::fromImage(logo_image_->scaledToHeight(50)));
    logo_image_label_->setMinimumWidth(96);
  }
  else
  {
    ROS_ERROR_STREAM("FAILED TO LOAD " << image_path);
  }

  right_layout->addWidget(logo_image_label_);
  right_layout->setAlignment(logo_image_label_, Qt::AlignLeft | Qt::AlignTop);
  right_layout->addWidget(right_image_label_);
  right_layout->setAlignment(right_image_label_, Qt::AlignRight | Qt::AlignTop);

  // Top Label Area ---------------------------------------------------
  HeaderWidget* header = new HeaderWidget(
      "MoveIt Setup Assistant", "Welcome to the MoveIt Setup Assistant! These tools will assist you in creating a "
                                "MoveIt configuration package that is required to run MoveIt. This includes generating "
                                "a Semantic Robot Description Format (SRDF) file, kinematics configuration file and "
                                "OMPL planning configuration file. It also involves creating launch files for move "
                                "groups, OMPL planner, planning contexts and the planning warehouse.",
      this);
  layout->addWidget(header);

  // Select Mode Area -------------------------------------------------
  select_mode_ = new SelectModeWidget(this);
  connect(select_mode_->btn_new_, SIGNAL(clicked()), this, SLOT(showNewOptions()));
  connect(select_mode_->btn_exist_, SIGNAL(clicked()), this, SLOT(showExistingOptions()));
  left_layout->addWidget(select_mode_);

  // Path Box Area ----------------------------------------------------

  // Stack Path Dialog
  stack_path_ =
      new LoadPathArgsWidget("Load MoveIt Configuration Package Path",
                             "Specify the package name or path of an existing MoveIt configuration package to be "
                             "edited for your robot. Example package name: <i>pr2_moveit_config</i>",
                             "xacro arguments", this, true);  // directory
  stack_path_->hide();                                        // user needs to select option before this is shown
  stack_path_->setArgs("--inorder ");
  connect(stack_path_, SIGNAL(pathChanged(QString)), this, SLOT(onPackagePathChanged(QString)));
  left_layout->addWidget(stack_path_);

  // URDF File Dialog
  urdf_file_ = new LoadPathArgsWidget("Load a URDF or COLLADA Robot Model",
                                      "Specify the location of an existing Universal Robot Description Format or "
                                      "COLLADA file for "
                                      "your robot. The robot model will be loaded to the parameter server for you.",
                                      "xacro arguments", this, false, true);  // no directory, load only
  urdf_file_->hide();  // user needs to select option before this is shown
  urdf_file_->setArgs("--inorder ");
  connect(urdf_file_, SIGNAL(pathChanged(QString)), this, SLOT(onUrdfPathChanged(QString)));
  left_layout->addWidget(urdf_file_);

  // Load settings box ---------------------------------------------
  QHBoxLayout* load_files_layout = new QHBoxLayout();

  progress_bar_ = new QProgressBar(this);
  progress_bar_->setMaximum(100);
  progress_bar_->setMinimum(0);
  progress_bar_->hide();
  load_files_layout->addWidget(progress_bar_);

  btn_load_ = new QPushButton("&Load Files", this);
  btn_load_->setMinimumWidth(180);
  btn_load_->setMinimumHeight(40);
  btn_load_->hide();
  load_files_layout->addWidget(btn_load_);
  load_files_layout->setAlignment(btn_load_, Qt::AlignRight);
  connect(btn_load_, SIGNAL(clicked()), this, SLOT(loadFilesClick()));

  // Next step instructions
  next_label_ = new QLabel(this);
  QFont next_label_font(QFont().defaultFamily(), 11, QFont::Bold);
  next_label_->setFont(next_label_font);
  // next_label_->setWordWrap(true);
  next_label_->setText("Success! Use the left navigation pane to continue.");
  //  next_label_->setSizePolicy( QSizePolicy::Expanding, QSizePolicy::Preferred );
  next_label_->hide();  // only show once the files have been loaded.

  // Final Layout Setup ---------------------------------------------
  // Alignment
  layout->setAlignment(Qt::AlignTop);
  hlayout->setAlignment(Qt::AlignTop);
  left_layout->setAlignment(Qt::AlignTop);
  right_layout->setAlignment(Qt::AlignTop);

  // Stretch
  left_layout->setSpacing(10);

  // Attach Layouts
  hlayout->addLayout(left_layout);
  hlayout->addLayout(right_layout);
  layout->addLayout(hlayout);

  // Verticle Spacer
  QWidget* vspacer = new QWidget(this);
  vspacer->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
  layout->addWidget(vspacer);

  // Attach bottom layout
  layout->addWidget(next_label_);
  layout->setAlignment(next_label_, Qt::AlignRight);
  layout->addLayout(load_files_layout);

  this->setLayout(layout);
  //  this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);

  // Debug mode: auto load the configuration file by clicking button after a timeout
  if (config_data_->debug_)
  {
    // select_mode_->btn_exist_->click();

    QTimer* update_timer = new QTimer(this);
    update_timer->setSingleShot(true);  // only run once
    connect(update_timer, SIGNAL(timeout()), btn_load_, SLOT(click()));
    update_timer->start(100);
  }
}

// ******************************************************************************************
// Destructor
// ******************************************************************************************
StartScreenWidget::~StartScreenWidget()
{
  delete right_image_;  // does not have a parent passed to it
  delete logo_image_;
}

// ******************************************************************************************
// Show options for creating a new configuration package
// ******************************************************************************************
void StartScreenWidget::showNewOptions()
{
  // Do GUI stuff
  select_mode_->btn_exist_->setChecked(false);
  select_mode_->btn_new_->setChecked(true);
  urdf_file_->show();
  stack_path_->hide();
  btn_load_->show();

  // Remember choice
  create_new_package_ = true;
}

// ******************************************************************************************
// Show options for editing an existing configuration package
// ******************************************************************************************
void StartScreenWidget::showExistingOptions()
{
  // Do GUI stuff
  select_mode_->btn_exist_->setChecked(true);
  select_mode_->btn_new_->setChecked(false);
  urdf_file_->hide();
  stack_path_->show();
  btn_load_->show();

  // Remember choice
  create_new_package_ = false;
}

// ******************************************************************************************
// Load files to parameter server - CLICK
// ******************************************************************************************
void StartScreenWidget::loadFilesClick()
{
  // Disable start screen GUI components from being changed
  urdf_file_->setDisabled(true);
  // srdf_file_->setDisabled(true);
  stack_path_->setDisabled(true);
  select_mode_->setDisabled(true);
  btn_load_->setDisabled(true);
  progress_bar_->show();

  bool result;

  // Decide if this is a new config package, or loading an old one
  if (create_new_package_)
  {
    result = loadNewFiles();
    // Load 3d_sensors config file
    load3DSensorsFile();
  }
  else
  {
    result = loadExistingFiles();
  }

  // Check if there was a failure loading files
  if (!result)
  {
    // Renable components
    urdf_file_->setDisabled(false);
    // srdf_file_->setDisabled(false);
    stack_path_->setDisabled(false);
    select_mode_->setDisabled(false);
    btn_load_->setDisabled(false);
    progress_bar_->hide();
  }
  else
  {
    // Hide the logo image so that other screens can resize the rviz thing properly
    right_image_label_->hide();
    logo_image_label_->hide();
  }
}

void StartScreenWidget::onPackagePathChanged(const QString& path)
{
  if (!loadPackageSettings(false))
    return;
  // set xacro args from loaded settings
  stack_path_->setArgs(QString::fromStdString(config_data_->xacro_args_));
}

void StartScreenWidget::onUrdfPathChanged(const QString& path)
{
  urdf_file_->setArgsEnabled(rdf_loader::RDFLoader::isXacroFile(path.toStdString()));
}

bool StartScreenWidget::loadPackageSettings(bool show_warnings)
{
  // Get the package path
  std::string package_path_input = stack_path_->getPath();
  // Check that input is provided
  if (package_path_input.empty())
  {
    if (show_warnings)
      QMessageBox::warning(this, "Error Loading Files", "Please specify a configuration package path to load.");
    return false;
  }

  // check that the folder exists
  if (!config_data_->setPackagePath(package_path_input))
  {
    if (show_warnings)
      QMessageBox::critical(this, "Error Loading Files", "The specified path is not a directory or is not accessable");
    return false;
  }

  std::string setup_assistant_path;

  // Check if the old package is a setup assistant package. If it is not, quit
  if (!config_data_->getSetupAssistantYAMLPath(setup_assistant_path))
  {
    if (show_warnings)
      QMessageBox::warning(
          this, "Incorrect Directory/Package",
          QString("The chosen package location exists but was not created using MoveIt Setup Assistant. "
                  "If this is a mistake, provide the missing file: ")
              .append(setup_assistant_path.c_str()));
    return false;
  }

  // Get setup assistant data
  if (!config_data_->inputSetupAssistantYAML(setup_assistant_path))
  {
    if (show_warnings)
      QMessageBox::warning(this, "Setup Assistant File Error",
                           QString("Unable to correctly parse the setup assistant configuration file: ")
                               .append(setup_assistant_path.c_str()));
    return false;
  }
  return true;
}

// ******************************************************************************************
// Load exisiting package files
// ******************************************************************************************
bool StartScreenWidget::loadExistingFiles()
{
  // Progress Indicator
  progress_bar_->setValue(10);
  QApplication::processEvents();

  if (!loadPackageSettings(true))
    return false;

  // Progress Indicator
  progress_bar_->setValue(30);
  QApplication::processEvents();

  // Get the URDF path using the loaded .setup_assistant data and check it
  if (!createFullURDFPath())
    return false;  // error occured

  // use xacro args from GUI
  config_data_->xacro_args_ = stack_path_->getArgs().toStdString();

  // Load the URDF
  if (!loadURDFFile(config_data_->urdf_path_, config_data_->xacro_args_))
    return false;  // error occured

  // Get the SRDF path using the loaded .setup_assistant data and check it
  if (!createFullSRDFPath(config_data_->config_pkg_path_))
    return false;  // error occured

  // Progress Indicator
  progress_bar_->setValue(50);
  QApplication::processEvents();

  // Load the SRDF
  if (!loadSRDFFile(config_data_->srdf_path_))
    return false;  // error occured

  // Progress Indicator
  progress_bar_->setValue(60);
  QApplication::processEvents();

  // Load the allowed collision matrix
  config_data_->loadAllowedCollisionMatrix();

  // Load kinematics yaml file if available --------------------------------------------------
  fs::path kinematics_yaml_path = config_data_->config_pkg_path_;
  kinematics_yaml_path /= "config/kinematics.yaml";

  if (!config_data_->inputKinematicsYAML(kinematics_yaml_path.make_preferred().native().c_str()))
  {
    QMessageBox::warning(this, "No Kinematic YAML File",
                         QString("Failed to parse kinematics yaml file. This file is not critical but any previous "
                                 "kinematic solver settings have been lost. To re-populate this file edit each "
                                 "existing planning group and choose a solver, then save each change. \n\nFile error "
                                 "at location ")
                             .append(kinematics_yaml_path.make_preferred().native().c_str()));
  }

  // Load 3d_sensors config file
  load3DSensorsFile();

  // Load ros controllers yaml file if available-----------------------------------------------
  fs::path ros_controllers_yaml_path = config_data_->config_pkg_path_;
  ros_controllers_yaml_path /= "config/ros_controllers.yaml";
  config_data_->inputROSControllersYAML(ros_controllers_yaml_path.make_preferred().native().c_str());

  fs::path ompl_yaml_path = config_data_->config_pkg_path_;
  ompl_yaml_path /= "config/ompl_planning.yaml";
  config_data_->inputOMPLYAML(ompl_yaml_path.make_preferred().native().c_str());

  // DONE LOADING --------------------------------------------------------------------------

  // Call a function that enables navigation
  Q_EMIT readyToProgress();

  // Progress Indicator
  progress_bar_->setValue(70);
  QApplication::processEvents();

  // Load Rviz
  Q_EMIT loadRviz();

  // Progress Indicator
  progress_bar_->setValue(100);
  QApplication::processEvents();

  next_label_->show();  // only show once the files have been loaded

  ROS_INFO("Loading Setup Assistant Complete");
  return true;  // success!
}

// ******************************************************************************************
// Load chosen files for creating new package
// ******************************************************************************************
bool StartScreenWidget::loadNewFiles()
{
  // Get URDF file path
  config_data_->urdf_path_ = urdf_file_->getPath();

  // Check that box is filled out
  if (config_data_->urdf_path_.empty())
  {
    QMessageBox::warning(this, "Error Loading Files", "No robot model file specefied");
    return false;
  }

  // Check that this file exits
  if (!fs::is_regular_file(config_data_->urdf_path_))
  {
    QMessageBox::warning(this, "Error Loading Files",
                         QString("Unable to locate the URDF file: ").append(config_data_->urdf_path_.c_str()));
    return false;
  }

  // Attempt to get the ROS package from the path
  if (!extractPackageNameFromPath())
  {
    return false;  // An error occurred
  }

  // Progress Indicator
  progress_bar_->setValue(20);
  QApplication::processEvents();

  // use xacro args from GUI
  config_data_->xacro_args_ = urdf_file_->getArgs().toStdString();

  // Load the URDF to the parameter server and check that it is correct format
  if (!loadURDFFile(config_data_->urdf_path_, config_data_->xacro_args_))
    return false;  // error occurred

  // Progress Indicator
  progress_bar_->setValue(50);
  QApplication::processEvents();

  // Create blank SRDF file
  const std::string blank_srdf = "<?xml version='1.0'?><robot name='" + config_data_->urdf_model_->getName() + "'></"
                                                                                "robot>";

  // Load a blank SRDF file to the parameter server
  if (!setSRDFFile(blank_srdf))
  {
    QMessageBox::warning(this, "Error Loading Files", "Failure loading blank SRDF file.");
    return false;
  }

  // Progress Indicator
  progress_bar_->setValue(60);
  QApplication::processEvents();

  // DONE LOADING --------------------------------------------------------------------------

  // Call a function that enables navigation
  Q_EMIT readyToProgress();

  // Progress Indicator
  progress_bar_->setValue(70);
  QApplication::processEvents();

  // Load Rviz
  Q_EMIT loadRviz();

  // Progress Indicator
  progress_bar_->setValue(100);
  QApplication::processEvents();

  next_label_->show();  // only show once the files have been loaded

  ROS_INFO("Loading Setup Assistant Complete");
  return true;  // success!
}

// ******************************************************************************************
// Load URDF File to Parameter Server
// ******************************************************************************************
bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args)
{
  const std::vector<std::string> xacro_args_ = { xacro_args };

  if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, xacro_args_))
  {
    QMessageBox::warning(this, "Error Loading Files",
                         QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str()));
    return false;
  }

  if (config_data_->urdf_string_.empty() && rdf_loader::RDFLoader::isXacroFile(urdf_file_path))
  {
    QMessageBox::warning(this, "Error Loading Files", "Running xacro failed.\nPlease check console for errors.");
    return false;
  }

  // Verify that file is in correct format / not an XACRO by loading into robot model
  if (!config_data_->urdf_model_->initString(config_data_->urdf_string_))
  {
    QMessageBox::warning(this, "Error Loading Files", "URDF/COLLADA file is not a valid robot model.");
    return false;
  }
  config_data_->urdf_from_xacro_ = rdf_loader::RDFLoader::isXacroFile(urdf_file_path);

  ROS_INFO_STREAM("Loaded " << config_data_->urdf_model_->getName() << " robot model.");

  // Load the robot model to the parameter server
  ros::NodeHandle nh;
  int steps = 0;
  while (!nh.ok() && steps < 4)
  {
    ROS_WARN("Waiting for node handle");
    sleep(1);
    steps++;
    ros::spinOnce();
  }

  ROS_INFO("Setting Param Server with Robot Description");
  // ROS_WARN("Ignore the following error message 'Failed to contact master'. This is a known issue.");
  nh.setParam("/robot_description", config_data_->urdf_string_);

  return true;
}

// ******************************************************************************************
// Load SRDF File to Parameter Server
// ******************************************************************************************
bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path)
{
  const std::vector<std::string> xacro_args;

  std::string srdf_string;
  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, xacro_args))
  {
    QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str()));
    return false;
  }

  // Put on param server
  return setSRDFFile(srdf_string);
}

// ******************************************************************************************
// Put SRDF File on Parameter Server
// ******************************************************************************************
bool StartScreenWidget::setSRDFFile(const std::string& srdf_string)
{
  // Verify that file is in correct format / not an XACRO by loading into robot model
  if (!config_data_->srdf_->initString(*config_data_->urdf_model_, srdf_string))
  {
    QMessageBox::warning(this, "Error Loading Files", "SRDF file not a valid semantic robot description model.");
    return false;
  }
  ROS_INFO_STREAM("Robot semantic model successfully loaded.");

  // Load to param server
  ros::NodeHandle nh;
  int steps = 0;
  while (!nh.ok() && steps < 4)
  {
    ROS_WARN("Waiting for node handle");
    sleep(1);
    steps++;
    ros::spinOnce();
  }

  ROS_INFO("Setting Param Server with Robot Semantic Description");
  nh.setParam("/robot_description_semantic", srdf_string);

  return true;
}

// ******************************************************************************************
// Extract the package/stack name and relative path to urdf from an absolute path name
// Input:  cofig_data_->urdf_path_
// Output: cofig_data_->urdf_pkg_name_
//         cofig_data_->urdf_pkg_relative_path_
// ******************************************************************************************
bool StartScreenWidget::extractPackageNameFromPath()
{
  // Get the path to urdf, save filename
  fs::path urdf_path = config_data_->urdf_path_;
  fs::path urdf_directory = urdf_path;
  urdf_directory.remove_filename();

  fs::path sub_path;         // holds the directory less one folder
  fs::path relative_path;    // holds the path after the sub_path
  std::string package_name;  // result

  // Paths for testing if files exist
  fs::path package_path;

  std::vector<std::string> path_parts;  // holds each folder name in vector

  // Copy path into vector of parts
  for (fs::path::iterator it = urdf_directory.begin(); it != urdf_directory.end(); ++it)
    path_parts.push_back(it->native());

  bool packageFound = false;

  // reduce the generated directoy path's folder count by 1 each loop
  for (int segment_length = path_parts.size(); segment_length > 0; --segment_length)
  {
    // Reset the sub_path
    sub_path.clear();

    // Create a subpath based on the outer loops length
    for (int segment_count = 0; segment_count < segment_length; ++segment_count)
    {
      sub_path /= path_parts[segment_count];

      // decide if we should remember this directory name because it is topmost, in case it is the package/stack name
      if (segment_count == segment_length - 1)
      {
        package_name = path_parts[segment_count];
      }
    }

    // check if this directory has a package.xml
    package_path = sub_path;
    package_path /= "package.xml";
    ROS_DEBUG_STREAM("Checking for " << package_path.make_preferred().native());

    // Check if the files exist
    if (fs::is_regular_file(package_path) || fs::is_regular_file(sub_path / "manifest.xml"))
    {
      // now generate the relative path
      for (size_t relative_count = segment_length; relative_count < path_parts.size(); ++relative_count)
        relative_path /= path_parts[relative_count];

      // add the URDF filename at end of relative path
      relative_path /= urdf_path.filename();

      // end the search
      segment_length = 0;
      packageFound = true;
      break;
    }
  }

  // Assign data to moveit_config_data
  if (!packageFound)
  {
    // No package name found, we must be outside ROS
    config_data_->urdf_pkg_name_ = "";
    config_data_->urdf_pkg_relative_path_ = config_data_->urdf_path_;  // just the absolute path
  }
  else
  {
    // Check that ROS can find the package
    const std::string robot_desc_pkg_path = ros::package::getPath(config_data_->urdf_pkg_name_) + "/";

    if (robot_desc_pkg_path.empty())
    {
      QMessageBox::warning(this, "Package Not Found In ROS Workspace",
                           QString("ROS was unable to find the package name '")
                               .append(config_data_->urdf_pkg_name_.c_str())
                               .append("' within the ROS workspace. This may cause issues later."));
    }

    // Success
    config_data_->urdf_pkg_name_ = package_name;
    config_data_->urdf_pkg_relative_path_ = relative_path.make_preferred().native();
  }

  ROS_DEBUG_STREAM("URDF Package Name: " << config_data_->urdf_pkg_name_);
  ROS_DEBUG_STREAM("URDF Package Path: " << config_data_->urdf_pkg_relative_path_);

  return true;  // success
}

// ******************************************************************************************
// Make the full URDF path using the loaded .setup_assistant data
// ******************************************************************************************
bool StartScreenWidget::createFullURDFPath()
{
  if (!config_data_->createFullURDFPath())
  {
    if (config_data_->urdf_path_.empty())  // no path could be resolved
    {
      QMessageBox::warning(this, "Error Loading Files", QString("ROS was unable to find the package name '")
                                                            .append(config_data_->urdf_pkg_name_.c_str())
                                                            .append("'. Verify this package is inside your ROS "
                                                                    "workspace and is a proper ROS package."));
    }
    else
    {
      QMessageBox::warning(this, "Error Loading Files",
                           QString("Unable to locate the URDF file in package. Expected File: \n")
                               .append(config_data_->urdf_path_.c_str()));
    }
    return false;
  }

  // Check if a package name was provided
  if (config_data_->urdf_pkg_name_.empty())
  {
    ROS_WARN("The URDF path is absolute to the filesystem and not relative to a ROS package/stack");
  }

  return true;  // success
}

// ******************************************************************************************
// Make the full SRDF path using the loaded .setup_assistant data
// ******************************************************************************************
bool StartScreenWidget::createFullSRDFPath(const std::string& package_path)
{
  if (!config_data_->createFullSRDFPath(package_path))
  {
    QMessageBox::warning(this, "Error Loading Files",
                         QString("Unable to locate the SRDF file: ").append(config_data_->srdf_path_.c_str()));
    return false;
  }

  return true;  // success
}

// ******************************************************************************************
// Loads sensors_3d yaml file
// ******************************************************************************************
bool StartScreenWidget::load3DSensorsFile()
{
  // Loads sensors_3d yaml file if available --------------------------------------------------
  fs::path sensors_3d_yaml_path = config_data_->config_pkg_path_;
  sensors_3d_yaml_path /= "config/sensors_3d.yaml";

  // If config was not available, load default configuration
  if (!fs::is_regular_file(sensors_3d_yaml_path))
  {
    sensors_3d_yaml_path = "resources/default_config/sensors_3d.yaml";
    return config_data_->input3DSensorsYAML(sensors_3d_yaml_path.make_preferred().native().c_str());
  }
  else
  {
    fs::path default_sensors_3d_yaml_path = "resources/default_config/sensors_3d.yaml";
    return config_data_->input3DSensorsYAML(default_sensors_3d_yaml_path.make_preferred().native().c_str(),
                                            sensors_3d_yaml_path.make_preferred().native().c_str());
  }
}

// ******************************************************************************************
// ******************************************************************************************
// Class for selecting which mode
// ******************************************************************************************
// ******************************************************************************************

// ******************************************************************************************
// Create the widget
// ******************************************************************************************
SelectModeWidget::SelectModeWidget(QWidget* parent) : QFrame(parent)
{
  // Set frame graphics
  setFrameShape(QFrame::StyledPanel);
  setFrameShadow(QFrame::Raised);
  setLineWidth(1);
  setMidLineWidth(0);

  // Basic widget container
  QVBoxLayout* layout = new QVBoxLayout(this);

  // Horizontal layout splitter
  QHBoxLayout* hlayout = new QHBoxLayout();

  // Widget Title
  QLabel* widget_title = new QLabel(this);
  widget_title->setText("Choose mode:");
  QFont widget_title_font(QFont().defaultFamily(), 12, QFont::Bold);
  widget_title->setFont(widget_title_font);
  layout->addWidget(widget_title);
  layout->setAlignment(widget_title, Qt::AlignTop);

  // Widget Instructions
  QTextEdit* widget_instructions = new QTextEdit(this);
  widget_instructions->setText("All settings for MoveIt are stored in a Moveit configuration package. Here you have "
                               "the option to create a new configuration package, or load an existing one. Note: any "
                               "changes to a MoveIt configuration package outside this setup assistant will likely be "
                               "overwritten by this tool.");
  widget_instructions->setWordWrapMode(QTextOption::WrapAtWordBoundaryOrAnywhere);
  // widget_instructions->setMinimumWidth(1);
  layout->addWidget(widget_instructions);
  layout->setAlignment(widget_instructions, Qt::AlignTop);

  // New Button
  btn_new_ = new QPushButton(this);
  btn_new_->setText("Create &New MoveIt\nConfiguration Package");
  hlayout->addWidget(btn_new_);

  // Exist Button
  btn_exist_ = new QPushButton(this);
  btn_exist_->setText("&Edit Existing MoveIt\nConfiguration Package");
  btn_exist_->setCheckable(true);
  hlayout->addWidget(btn_exist_);

  // Add horizontal layer to verticle layer
  layout->addLayout(hlayout);
  setLayout(layout);
  btn_new_->setCheckable(true);
}

}  // namespace

max_stars_repo_name: raghavendersahdev/moveit

max_issues_repo_name: raghavendersahdev/moveit

max_forks_repo_name: raghavendersahdev/moveit


text:
#include <iostream>
#include <armadillo>
#include <fstream>
#include <string>
#include <vector>
#include <math.h>
#include <iomanip>

using namespace std;
using namespace arma;

struct GMS
{
    int G,M,S;

    GMS(int,int,int);
    double gdec();
    double rad();
};

struct topoPoint
{
    double Hz,Ze,Di;
    vec3 XYZ;
    bool rad;

    topoPoint(double,double,double,bool);
    void cart();
    void translate(vec3 tvec);
    void Rotate(mat R);

};

GMS::GMS (int g,int m,int s)
{
    G=g;
    M=m;
    S=s;
}

double GMS::gdec ()
{
    return double(G)+ (double(M)/60) + (double(S)/3600);
}

double GMS::rad()
{
    double radd = gdec()  * (datum::pi/180);
    return radd;
}

struct SMMTleverARM
{
//to handle with the base from the file
    vector<vector<string>> data;
    vector<vector<GMS>>  hor,zen;
    vector<vector<double>> dists;

//the arbitrary-frame surveyed points
    vector<vector<topoPoint>> points;

//the planes of the process (coefficients a,b,c,d)
    vec4 Hplan,Vplan;
    vec4 CEin,CEout,CDin,CDout;
//aux1 is parallel to Hplan

//matrices containing points
    mat ptsH,ptsV; //at the sides of the IMU
    mat ptsCEin,ptsCEout,ptsCDin,ptsCDout; //points on the parallel circular patterns, on the lens of the cameras
    mat ptsOnCEin,ptsOnCEout,ptsOnCDin,ptsOnCDout; //adjusted points

//rotation matrices
    mat Rot1 = eye<mat>(3,3);

//the principal vectors, to define the cannonical system
    vec3 u,v,w; //u for x,v for y,w for z

//the axis direction of each camera system, in the same convention of the IMU
    vec3 xE,yE,zE,xD,yD,zD;

//the axis of each camera system, with the z axis pointing backwards
    vec3 Xe,Ye,Ze,Xd,Yd,Zd;

//the difference between the antena top and the center of phase
    vec3 PhC = {0,0,-0.0071};

//the lever-arm vectors
    vec3 antLA,LcamLA,RcamLA;

//temporary, and auxiliar vectors
    vec3 tempE,tempD,aux1e,aux1d,aux_E0,aux_D0;

//the boresight matrices
    mat Rimu_LC = eye(3,3),Rimu_RC = eye(3,3),Rimu_LC2 = eye(3,3),Rimu_RC2 = eye(3,3),Rimu_LC3 = eye(3,3),Rimu_RC3 = eye(3,3);

//center of the IMU, the mark on the H plane, the mark on the v plane
    vec3 imuC,Hm,Vm;

    double planesAngle;

    SMMTleverARM (string fileName);

    vector<vector<string>> readLAFile(string filename);

//the member functions
    void horiz();
    void zenit();
    void distanc();
    void avgOBS();
    void pointsPrint(string filename);
    void translateAll(vec3 tvec);
    void rotAll (mat R);
    void report();

};

vec3 onPlanePoint(vec4 plCoef,double x,double y);

//all the function propotypes:
double pdpiH(double,double);
double pdpiV(double,double);
void TptMat (mat *M,vec Tvec);
void RptMat (mat *M,mat R);

vec4 plane3points(vec3 u,vec3 v,vec3 w);

mat topoPointsMat (vector<topoPoint> points,uword first,uword last);

vec4 parallelPlane(vec4 plane1,vec3 pointOnPlane);

vec3 ProjPtOrtPlane(vec3 pt2Proj,vec3 plNorm,vec3 ptOnPlane);

vec4 leasqPlane2(vector<topoPoint> pointList,mat *ptOnPlane,string repNam,uword ijpt=0)
{
    vec4 res,Xo;
    mat data,P,Pinf,A,B,M,mvcXa,mvcLa,mvcV,mvcW;
    vec Lb,W,X,Xa,K,V,La;
    double vp;

    if (pointList.size() >= 3)
    {
        Xo = plane3points(pointList.at(0).XYZ,pointList.at(1).XYZ,pointList.at(2).XYZ);
    }

    Pinf = eye(3,3)*10000;

    if (pointList.size() < 3)
    {
        res = {0,0,1,0};
    }
    else if (pointList.size() == 3)
    {
        res = Xo;
    }
    else
    {
        ofstream out(repNam);

        //vetor Lb
        data = ones(pointList.size(),3);

        for (uword i0 = 0; i0 < pointList.size(); i0++)
        {
            data.row(i0) = pointList.at(i0).XYZ.t();
        }

        Lb = vectorise(data.t());

        //matriz dos pesos
        P = eye(Lb.n_elem,Lb.n_elem);
        if (ijpt != 0)
        {
            //em caso de que haja um ponto a ser injuncionado
//        P.submat(ijpt*3-2,ijpt*3-2,ijpt*3,ijpt*3) = Pinf;
            P.submat(ijpt*3-3,ijpt*3-3,ijpt*3-1,ijpt*3-1) = Pinf;
        }
        //matriz jacobiana A (parametros)

        A = ones<mat>(pointList.size(),4);

        for (uword i = 0; i < pointList.size(); i++)
        {
            A(i,0)=data(i,0);
            A(i,1)=data(i,1);
            A(i,2)=data(i,2);
        }

        //matriz jacobiana B (observacoes)

        B = zeros<mat>(pointList.size(),Lb.n_elem);

        uword j = 0;
        for (uword i2 = 0; i2 < pointList.size(); i2++)
        {
            B(i2,0+j)=Xo(0);
            B(i2,1+j)=Xo(1);
            B(i2,2+j)=Xo(2);

            j += 3;
        }

        W = zeros<vec>(pointList.size());

        for (uword i3 = 0; i3 < pointList.size(); i3++)
        {
            W(i3) = dot(A.row(i3),Xo);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        K = -M.i()*(A*X+W);

        V = P.i() * B.t() * K;

        La = Lb+V;

        *ptOnPlane = trans(reshape(La,3,pointList.size()));

        vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

        mvcXa = vp * inv(A.t()*M.i()*A);

        mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

        mvcV  = vp*P.i()-mvcLa;

        mvcW  = vp*M;

        out.precision(30);

        res = Xa;

//        Xo.raw_print(out,"Xo");
//        X.raw_print(out,"X");
//        Xa.raw_print(out,"Xa:");
//        res.raw_print(out," b ");

//        out <<"Xo+X: "<<endl<<std::setprecision(30)<< Xo+X<<endl;
        //normalizing the plane equations
        res.rows(0,2) = normalise(Xa.rows(0,2));
        res(3) = - dot(normalise(Xa.rows(0,2)),La.rows(0,2));

//            res.print(out," a ");

        data.print(out,"dados de entrada: ");
        out<<endl;

        res.print(out,"coeficientes do plano estimado: ");
        out<<endl;

        V.print(out,"resíduos: ");
        out<<endl;

        La.print(out,"La:");
        out<<endl;

        out << "var. posteriori: " << vp << endl<<endl;

        mvcXa.print(out,"MVC dos parametros ajustados");
        out<<endl;

        mvcLa.print(out,"MVC das observações ajustadas");
        out<<endl;

    }
//cout<< res<<endl;
    return res;
}

void leasq2ParallelPlanes(vec4 *plane1,vec4 *plane2,mat points1,mat points2,mat *ptOnPlane1,mat *ptOnPlane2,string repNam,uword n=5,double e=.000001)
{
    vec5 Xo,X,Xa;
    mat data,P,A,B,M,mvcXa,mvcLa,mvcV,mvcW,ptsOnPlanes;
    vec Lb,W,K,V,La,temp5,temp6;
    double vp;
    vec4 temp1,temp2,temp3,temp4;
    temp1.ones();
    temp2.zeros();
    uword iter;

    ofstream out(repNam);

    Xo.rows(0,3) = plane3points(trans(points1.row(0)),trans(points1.row(1)),trans(points1.row(2)) );
    Xo(4) = - dot(Xo.rows(0,2),points2.row(0));

    //vetor Lb

    data = join_vert(points1,points2);

    Lb = vectorise(data.t());

    //matriz dos pesos
    P = eye(Lb.n_elem,Lb.n_elem);

    //matriz jacobiana A (parametros)

    A = zeros<mat>(data.n_rows,Xo.n_elem);
    B = zeros<mat>(data.n_rows,Lb.n_elem);
    W = zeros<vec>(data.n_rows);

    A.submat(0,0,data.n_rows-1,data.n_cols-1) = data;

    for (uword i = 0; i < data.n_rows; i++)
    {
        if (i < points1.n_rows)
        {
            A(i,3) = 1;
        }
        else
        {
            A(i,4) = 1;
        }
    }

    for(uword it = 0; it < n; it ++)
    {

        uword j = 0;
        for (uword i2 = 0; i2 < data.n_rows; i2++)
        {
            B(i2,0+j)=Xo(0);
            B(i2,1+j)=Xo(1);
            B(i2,2+j)=Xo(2);

            j += 3;

            temp1.rows(0,2) = trans( data.row(i2) );
            temp2.rows(0,2) = Xo.rows(0,2);

            if(i2 < points1.n_rows)
            {
                temp2(3)=Xo(3);
            }
            else
            {
                temp2(3)=Xo(4);
            }

            W(i2) = dot(temp1,temp2);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        Xo = Xa;

        iter = it;

        if (arma::max(arma::abs(X)) < e)
        {
            break;
        }

    }

    K = -M.i()*(A*X+W);

    V = P.i() * B.t() * K;

    La = Lb+V;

    ptsOnPlanes = trans(reshape(La,data.n_cols,data.n_rows));

    *ptOnPlane1 = ptsOnPlanes.rows(0,points1.n_rows-1);

    *ptOnPlane2 = ptsOnPlanes.rows(points1.n_rows,data.n_rows-1);

    temp3.rows(0,2) =   normalise(Xa.rows(0,2));
    temp3(3)= - dot(La.rows(0,2),normalise(Xa.rows(0,2)));

    temp4 = temp3;
    temp4(3)= - dot(La.rows(La.n_elem-3,La.n_elem-1),normalise(Xa.rows(0,2)));

    *plane1 = temp3;
    *plane2 = temp4;

    vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

    mvcXa = vp * inv(A.t()*M.i()*A);

    mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

    mvcV  = vp*P.i()-mvcLa;

    mvcW  = vp*M;

    out.precision(20);

    points1.raw_print(out,"dados de entrada (primeiro plano): ");
    out<<endl;

    points2.raw_print(out,"dados de entrada (segundo plano): ");
    out<<endl;

    out << iter <<" iterações necessárias" <<endl<<endl;

    Xa.raw_print(out," vetor Xa (pré-normalização)");
    out<<endl;

    temp3.raw_print(out,"coeficientes do primeiro plano estimado: ");
    out<<endl;

    temp4.raw_print(out,"coeficientes do segundo plano estimado: ");
    out<<endl;

    V.raw_print(out,"resíduos: ");
    out<<endl;

    ptsOnPlanes.raw_print(out,"La:");
    out<<endl;

    out << "var. posteriori: " << vp << endl<<endl;

    mvcXa.raw_print(out,"MVC dos parametros ajustados");
    out<<endl;

    mvcLa.raw_print(out,"MVC das observações ajustadas");
    out<<endl;

}

struct Sphere
{
    vec3 center;
    double r;
    mat onSpherePts;

    Sphere(mat points,string repNam,uword n=20,double e=.000001);
};

Sphere::Sphere(mat data,string repNam,uword n,double e)
{
    vec4 Xo,X,Xa;
    mat P,A,B,M,mvcXa,mvcLa,mvcV,mvcW,ptsOnPlanes;
    vec Lb,W,K,V,La,temp5,temp6,aux;
    double vp;
    vec4 temp1,temp2,temp3,temp4;
    temp1.ones();
    temp2.zeros();
    uword iter;

    ofstream out(repNam);

    aux=zeros(data.n_rows);

    for (uword i = 0; i < aux.n_rows; i++)
    {
        aux(i) = norm(data.row(i) - mean(data));
    }

    Xo.rows(1,3) = trans(mean(data));
    Xo(0) = mean(aux);

    //vetor Lb

    Lb = vectorise(data.t());

    //matriz dos pesos
    P = eye(Lb.n_elem,Lb.n_elem);

    //matriz jacobiana A (parametros)

    A = zeros<mat>(data.n_rows,Xo.n_elem);
    B = zeros<mat>(data.n_rows,Lb.n_elem);
    W = zeros<vec>(data.n_rows);

    for(uword it = 0; it < n; it ++)
    {

        uword j = 0;
        for (uword i2 = 0; i2 < data.n_rows; i2++)
        {
            A(i2,0) = - 2 * Xo(0);

            A(i2,1) = 2 * (Xo(1) - data(i2,0));
            A(i2,2) = 2 * (Xo(2) - data(i2,1));
            A(i2,3) = 2 * (Xo(3) - data(i2,2));

            B(i2,j)  = - 2 * (Xo(1) - data(i2,0));
            B(i2,1+j)= - 2 * (Xo(2) - data(i2,1));
            B(i2,2+j)= - 2 * (Xo(3) - data(i2,2));

            j += 3;

            W(i2) = std::pow((data(i2,0) - Xo(1)),2) + std::pow((data(i2,1) - Xo(2)),2) +
                    std::pow((data(i2,2) - Xo(3)),2) - std::pow(Xo(0),2);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        Xo = Xa;

        iter = it;

        if (arma::max(arma::abs(X)) < e)
        {
            break;
        }

    }

    K = -M.i()*(A*X+W);

    V = P.i() * B.t() * K;

    La = Lb+V;

    onSpherePts = trans(reshape(La,data.n_cols,data.n_rows));

    vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

    mvcXa = vp * inv(A.t()*M.i()*A);

    mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

    mvcV  = vp*P.i()-mvcLa;

    mvcW  = vp*M;

    out.precision(20);

    data.raw_print(out,"dados de entrada :");
    out<<endl;

    out << iter <<" iterações necessárias" <<endl<<endl;

    Xa.raw_print(out,"parametros ajustados (r,XC,YC,ZC)");
    out<<endl;

    V.raw_print(out,"resíduos: ");
    out<<endl;

    La.raw_print(out,"La:");
    out<<endl;

    out << "var. posteriori: " << vp << endl<<endl;

    mvcXa.raw_print(out,"MVC dos parametros ajustados");
    out<<endl;

    mvcLa.raw_print(out,"MVC das observações ajustadas");
    out<<endl;

    //assignment to the object variables
    r = Xo(0);
    center = Xo.rows(1,3);
}

vector<vector<string>> SMMTleverARM::readLAFile(string filename)
{
//to read a file of the raw data of the Lever Arm surveying
    ifstream in(filename);
    vector<vector<string>> res;
    vector<string> A,B,C,D,E;
//to control
    int state = 0;
    string line;

    while (std::getline(in,line))
    {
        if (line.find("COD:[A") != -1)
        {
            state = 1;
            continue;
        }
        if (line.find("COD:[B") != -1)
        {
            state = 2;
            continue;
        }
        if (line.find("COD:[C") != -1)
        {
            state =3 ;
            continue;
        }
        if (line.find("COD:[D") != -1)
        {
            state =4 ;
            continue;
        }
        if (line.find("COD:[E") != -1)
        {
            state =5 ;
            continue;
        }

        if (state == 1)
        {
            A.push_back(line);
            //cout << stoi(line.substr(23,3))<<endl;
        }
        if (state == 2)
        {
            B.push_back(line);
        }
        if (state == 3)
        {
            C.push_back(line);
        }
        if (state ==4 )
        {
            D.push_back(line);
        }
        if (state == 5)
        {
            E.push_back(line);
        }
    }

    res.push_back(A);
    res.push_back(B);
    res.push_back(C);
    res.push_back(D);
    res.push_back(E);
    //cout<<A.size()<<" "<< B.size()<<" "<<C.size()<<" "<<D.size()<<" "<<E.size()<<endl;
    //cout<<res.size();

    return res;
}

SMMTleverARM::SMMTleverARM (string fileName)
{
// the constructor, all the modifying functions must have their calls here.
    data = readLAFile(fileName);
    horiz();
    zenit();
    distanc();
    avgOBS();

//the two planes
    Vplan = leasqPlane2(points.at(1),&ptsV,"plano_vert.txt",2);
    Hplan = leasqPlane2(points.at(2),&ptsH,"plano_hor.txt",2);
//the angle between them
    planesAngle = std::acos(dot(Vplan.rows(0,2),Hplan.rows(0,2))) * (180/datum::pi);

//the marks on the IMU surface
    Hm = trans(ptsH.row(2));
    Vm = trans(ptsV.row(2));

//the normal of the Hplan, is also the third direction (w),
//but at first, is needed some test
    w = Hplan.rows(0,2);
    if(w(2)<0) //the z component, needs to be positive, due to the z axis of the total station
    {
        w *= -1;
    }

//the aux1 plane
//aux1.rows(0,2) = w;
//aux1(3) = - dot(w,Vm);

//IMU center with the intersection of the plane (aux1) and the line (point Hm and direction w)
//imuC = Hm + (dot(w,(Vm-Hm)))* w; //do not delete the old implementation
    imuC = ProjPtOrtPlane(Hm,w,Vm);

//the first principal direction (u)
    u = normalise(Vm - imuC);

//the second principal direction(v)
    v = normalise(cross(w,u));

//first, translating the origin to the calculated center of the IMU
    TptMat(&ptsH,-imuC);
    TptMat(&ptsV,-imuC);
    translateAll(-imuC);
    pointsPrint("points1.txt");

//composing the first rotation matrice
    Rot1.col(0)=u;
    Rot1.col(1)=v;
    Rot1.col(2)=w;
    Rot1 = Rot1.t();

//rotating all to the imu BF
    RptMat(&ptsH,Rot1);
    RptMat(&ptsV,Rot1);
    rotAll(Rot1);
    pointsPrint("points2.txt");

//imuC += -imuC;

//creating matrices with the points, from topoPoints;
    ptsCDin  = topoPointsMat(points.at(3),0,4);
    ptsCDout = topoPointsMat(points.at(3),5,9);
    ptsCEin  = topoPointsMat(points.at(4),0,4);
    ptsCEout = topoPointsMat(points.at(4),5,9);

//the pair of parallel planes, one for each camera
    leasq2ParallelPlanes(&CDin,&CDout,ptsCDin,ptsCDout,&ptsOnCDin,&ptsOnCDout,"RcamPlanes.txt");
    leasq2ParallelPlanes(&CEin,&CEout,ptsCEin,ptsCEout,&ptsOnCEin,&ptsOnCEout,"LcamPlanes.txt");

//creating a sphere for each camera, to measure the center
    Sphere Rsphere(join_vert(ptsOnCDin,ptsOnCDout),"Rsphere.txt");
    Sphere Lsphere(join_vert(ptsOnCEin,ptsOnCEout),"Lsphere.txt");

//Y of each camera direction (with same convention)
//due to the IMU bf and the cameras position, the y component needs to be positive
// to point forward the camera axis

    yE = normalise(CEin.rows(0,2));
    if (yE(1) < 0 )
    {
        yE *= -1;
    }

    yD = normalise(CDin.rows(0,2));
    if (yD(1) < 0 )
    {
        yD *= -1;
    }

//yD.print();cout<<endl;
//yE.print();cout<<endl;

//we'll need points perfectly on the plane, and to have this guarantee:
//each one are obtained by the plane equation, with z=f(x,y)
    aux1d = onPlanePoint(CDin,ptsOnCDin(ptsOnCDin.n_rows-1,0),ptsOnCDin(ptsOnCDin.n_rows-1,1));
    aux1e = onPlanePoint(CEin,ptsOnCEin(ptsOnCEin.n_rows-1,0),ptsOnCEin(ptsOnCEin.n_rows-1,1));

    aux_D0 = onPlanePoint(CDin,ptsOnCDin(0,0),ptsOnCDin(0,1));
    aux_E0 = onPlanePoint(CEin,ptsOnCEin(0,0),ptsOnCEin(0,1));

//projecting the center of spheres on the inner plane of the rings on the camera lenses
    tempD = ProjPtOrtPlane(Rsphere.center,yD,aux1d);
    tempE = ProjPtOrtPlane(Lsphere.center,yE,aux1e);

//calculating the z direction, for each camera
    zE = normalise(aux_E0 - tempE);
    zD = normalise(aux_D0 - tempD);

//and, finally the x direction
    xE = normalise(cross(yE,zE));
    xD = normalise(cross(yD,zD));

    vec3 Xe2,Ye2,Ze2,Xd2,Yd2,Zd2;

//////formerly:
//////we can describe the axes in the camera conventional convention (z axis pointing from the CP to the focal plane)
////Xe = xE;Ye = zE;Ze = -yE;
////Xd = xD;Yd = zD;Zd = -yD;

//we can describe the axes in the camera modern convention (z axis pointing from the principle point to CP)
    Xe = xE;
    Ye = -zE;
    Ze = yE;

    Xd = xD;
    Yd = -zD;
    Zd = yD;

    //conventional convention
    Xe2 = xE;Ye2 = zE;Ze2 = -yE;
    Xd2 = xD;Yd2 = zD;Zd2 = -yD;

//with the axis, we can have the boresight matrices:
    //first with the same convention of the IMU:
    Rimu_LC2.row(0) = trans(xE);
    Rimu_LC2.row(1) = trans(yE);
    Rimu_LC2.row(2) = trans(zE);

    Rimu_RC2.row(0) = trans(xD);
    Rimu_RC2.row(1) = trans(yD);
    Rimu_RC2.row(2) = trans(zD);

    //with the camera modern convention
    Rimu_LC.row(0) = trans(Xe);
    Rimu_LC.row(1) = trans(Ye);
    Rimu_LC.row(2) = trans(Ze);

    Rimu_RC.row(0) = trans(Xd);
    Rimu_RC.row(1) = trans(Yd);
    Rimu_RC.row(2) = trans(Zd);

    //then with the camera conventional convention
    Rimu_LC3.row(0) = trans(Xe2);
    Rimu_LC3.row(1) = trans(Ye2);
    Rimu_LC3.row(2) = trans(Ze2);

    Rimu_RC3.row(0) = trans(Xd2);
    Rimu_RC3.row(1) = trans(Yd2);
    Rimu_RC3.row(2) = trans(Zd2);

//now, the lever-arm

    //antenna lever-arm: just the point at the antena top, and the offset from there to the PhC
    antLA = (points.at(0).at(0).XYZ) + PhC;

    //the lever-arm of the two cameras:
    RcamLA = Rsphere.center;
    LcamLA = Lsphere.center;

    report();
}

void SMMTleverARM::avgOBS()
{
    vector<topoPoint> temp;

    double hA,zA,dA=0,hB,zB,dB,hC,zC,dC,hD,zD,dD,hE,zE,dE,hF,zF,dF,hG,zG,dG,hH,zH,dH,hI,zI,dI,hJ,zJ,dJ;

//std::setprecision(8);
    for (unsigned int i = 0; i < hor.size(); i++)
    {
        if (i == 0)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][1].gdec()) +
                  pdpiH(hor[i][2].gdec(),hor[i][3].gdec()) +
                  pdpiH(hor[i][4].gdec(),hor[i][5].gdec())) / 3;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][1].gdec()) +
                  pdpiV(zen[i][2].gdec(),zen[i][3].gdec()) +
                  pdpiV(zen[i][4].gdec(),zen[i][5].gdec())) / 3;

            for (unsigned int j = 0; j < dists[i].size(); j++)
            {
                dA += dists[i][j]/6;
            }

            topoPoint ANT(hA,zA,dA,false);
            temp.push_back(ANT);

            //cout <<hA<<" "<<zA<<" "<<dA<<endl;
            //cout <<hA<<" "<<zA<<" "<<dA<<endl;
        }
        if (i == 1)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][5].gdec()) +
                  pdpiH(hor[i][10].gdec(),hor[i][15].gdec())) / 2;

            hB = (pdpiH(hor[i][1].gdec(),hor[i][6].gdec()) +
                  pdpiH(hor[i][11].gdec(),hor[i][16].gdec())) / 2;

            hC = (pdpiH(hor[i][2].gdec(),hor[i][7].gdec()) +
                  pdpiH(hor[i][12].gdec(),hor[i][17].gdec())) / 2;

            hD = (pdpiH(hor[i][3].gdec(),hor[i][8].gdec()) +
                  pdpiH(hor[i][13].gdec(),hor[i][18].gdec())) / 2;

            hE = (pdpiH(hor[i][4].gdec(),hor[i][9].gdec()) +
                  pdpiH(hor[i][14].gdec(),hor[i][19].gdec())) / 2;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][5].gdec()) +
                  pdpiV(zen[i][10].gdec(),zen[i][15].gdec())) / 2;

            zB = (pdpiV(zen[i][1].gdec(),zen[i][6].gdec()) +
                  pdpiV(zen[i][11].gdec(),zen[i][16].gdec())) / 2;

            zC = (pdpiV(zen[i][2].gdec(),zen[i][7].gdec()) +
                  pdpiV(zen[i][12].gdec(),zen[i][17].gdec())) / 2;

            zD = (pdpiV(zen[i][3].gdec(),zen[i][8].gdec()) +
                  pdpiV(zen[i][13].gdec(),zen[i][18].gdec())) / 2;

            zE = (pdpiV(zen[i][4].gdec(),zen[i][9].gdec()) +
                  pdpiV(zen[i][14].gdec(),zen[i][19].gdec())) / 2;

            dA = (dists[i][0]+dists[i][5]+dists[i][10]+dists[i][15]) / 4;

            dB = (dists[i][1]+dists[i][6]+dists[i][11]+dists[i][16]) / 4;

            dC = (dists[i][2]+dists[i][7]+dists[i][12]+dists[i][17]) / 4;

            dD = (dists[i][3]+dists[i][8]+dists[i][13]+dists[i][18]) / 4;

            dE = (dists[i][4]+dists[i][9]+dists[i][14]+dists[i][19]) / 4;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);

//                                cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 2)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][5].gdec()) +
                  pdpiH(hor[i][10].gdec(),hor[i][15].gdec())) / 2;

            hB = (pdpiH(hor[i][1].gdec(),hor[i][6].gdec()) +
                  pdpiH(hor[i][11].gdec(),hor[i][16].gdec())) / 2;

            hC = (pdpiH(hor[i][2].gdec(),hor[i][7].gdec()) +
                  pdpiH(hor[i][12].gdec(),hor[i][17].gdec())) / 2;

            hD = (pdpiH(hor[i][3].gdec(),hor[i][8].gdec()) +
                  pdpiH(hor[i][13].gdec(),hor[i][18].gdec())) / 2;

            hE = (pdpiH(hor[i][4].gdec(),hor[i][9].gdec()) +
                  pdpiH(hor[i][14].gdec(),hor[i][19].gdec())) / 2;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][5].gdec()) +
                  pdpiV(zen[i][10].gdec(),zen[i][15].gdec())) / 2;

            zB = (pdpiV(zen[i][1].gdec(),zen[i][6].gdec()) +
                  pdpiV(zen[i][11].gdec(),zen[i][16].gdec())) / 2;

            zC = (pdpiV(zen[i][2].gdec(),zen[i][7].gdec()) +
                  pdpiV(zen[i][12].gdec(),zen[i][17].gdec())) / 2;

            zD = (pdpiV(zen[i][3].gdec(),zen[i][8].gdec()) +
                  pdpiV(zen[i][13].gdec(),zen[i][18].gdec())) / 2;

            zE = (pdpiV(zen[i][4].gdec(),zen[i][9].gdec()) +
                  pdpiV(zen[i][14].gdec(),zen[i][19].gdec())) / 2;

            dA = (dists[i][0]+dists[i][5]+dists[i][10]+dists[i][15]) / 4;

            dB = (dists[i][1]+dists[i][6]+dists[i][11]+dists[i][16]) / 4;

            dC = (dists[i][2]+dists[i][7]+dists[i][12]+dists[i][17]) / 4;

            dD = (dists[i][3]+dists[i][8]+dists[i][13]+dists[i][18]) / 4;

            dE = (dists[i][4]+dists[i][9]+dists[i][14]+dists[i][19]) / 4;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);

//                                cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 3)
        {
            topoPoint EXTRA(hor[i][0].gdec(),zen[i][0].gdec(),dists[i][0],false);

            hA = pdpiH(hor[i][1].gdec(),hor[i][11].gdec());
            hB = pdpiH(hor[i][2].gdec(),hor[i][12].gdec());
            hC = pdpiH(hor[i][3].gdec(),hor[i][13].gdec());
            hD = pdpiH(hor[i][4].gdec(),hor[i][14].gdec());
            hE = pdpiH(hor[i][5].gdec(),hor[i][15].gdec());
            hF = pdpiH(hor[i][6].gdec(),hor[i][16].gdec());
            hG = pdpiH(hor[i][7].gdec(),hor[i][17].gdec());
            hH = pdpiH(hor[i][8].gdec(),hor[i][18].gdec());
            hI = pdpiH(hor[i][9].gdec(),hor[i][19].gdec());
            hJ = pdpiH(hor[i][10].gdec(),hor[i][20].gdec());

            zA = pdpiV(zen[i][1].gdec(),zen[i][11].gdec());
            zB = pdpiV(zen[i][2].gdec(),zen[i][12].gdec());
            zC = pdpiV(zen[i][3].gdec(),zen[i][13].gdec());
            zD = pdpiV(zen[i][4].gdec(),zen[i][14].gdec());
            zE = pdpiV(zen[i][5].gdec(),zen[i][15].gdec());
            zF = pdpiV(zen[i][6].gdec(),zen[i][16].gdec());
            zG = pdpiV(zen[i][7].gdec(),zen[i][17].gdec());
            zH = pdpiV(zen[i][8].gdec(),zen[i][18].gdec());
            zI = pdpiV(zen[i][9].gdec(),zen[i][19].gdec());
            zJ = pdpiV(zen[i][10].gdec(),zen[i][20].gdec());

            dA = (dists[i][1]+dists[i][11]) / 2;
            dB = (dists[i][2]+dists[i][12]) / 2;
            dC = (dists[i][3]+dists[i][13]) / 2;
            dD = (dists[i][4]+dists[i][14]) / 2;
            dE = (dists[i][5]+dists[i][15]) / 2;
            dF = (dists[i][6]+dists[i][16]) / 2;
            dG = (dists[i][7]+dists[i][17]) / 2;
            dH = (dists[i][8]+dists[i][18]) / 2;
            dI = (dists[i][9]+dists[i][19]) / 2;
            dJ = (dists[i][10]+dists[i][20]) / 2;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);
            topoPoint F(hF,zF,dF,false);
            temp.push_back(F);
            topoPoint G(hG,zG,dG,false);
            temp.push_back(G);
            topoPoint H(hH,zH,dH,false);
            temp.push_back(H);
            topoPoint I(hI,zI,dI,false);
            temp.push_back(I);
            topoPoint J(hJ,zJ,dJ,false);
            temp.push_back(J);

            temp.push_back(EXTRA);

//                               cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 4)
        {
            topoPoint EXTRA(hor[i][0].gdec(),zen[i][0].gdec(),dists[i][0],false);

            hA = pdpiH(hor[i][1].gdec(),hor[i][11].gdec());
            hB = pdpiH(hor[i][2].gdec(),hor[i][12].gdec());
            hC = pdpiH(hor[i][3].gdec(),hor[i][13].gdec());
            hD = pdpiH(hor[i][4].gdec(),hor[i][14].gdec());
            hE = pdpiH(hor[i][5].gdec(),hor[i][15].gdec());
            hF = pdpiH(hor[i][6].gdec(),hor[i][16].gdec());
            hG = pdpiH(hor[i][7].gdec(),hor[i][17].gdec());
            hH = pdpiH(hor[i][8].gdec(),hor[i][18].gdec());
            hI = pdpiH(hor[i][9].gdec(),hor[i][19].gdec());
            hJ = pdpiH(hor[i][10].gdec(),hor[i][20].gdec());

            zA = pdpiV(zen[i][1].gdec(),zen[i][11].gdec());
            zB = pdpiV(zen[i][2].gdec(),zen[i][12].gdec());
            zC = pdpiV(zen[i][3].gdec(),zen[i][13].gdec());
            zD = pdpiV(zen[i][4].gdec(),zen[i][14].gdec());
            zE = pdpiV(zen[i][5].gdec(),zen[i][15].gdec());
            zF = pdpiV(zen[i][6].gdec(),zen[i][16].gdec());
            zG = pdpiV(zen[i][7].gdec(),zen[i][17].gdec());
            zH = pdpiV(zen[i][8].gdec(),zen[i][18].gdec());
            zI = pdpiV(zen[i][9].gdec(),zen[i][19].gdec());
            zJ = pdpiV(zen[i][10].gdec(),zen[i][20].gdec());

            dA = (dists[i][1]+dists[i][11]) / 2;
            dB = (dists[i][2]+dists[i][12]) / 2;
            dC = (dists[i][3]+dists[i][13]) / 2;
            dD = (dists[i][4]+dists[i][14]) / 2;
            dE = (dists[i][5]+dists[i][15]) / 2;
            dF = (dists[i][6]+dists[i][16]) / 2;
            dG = (dists[i][7]+dists[i][17]) / 2;
            dH = (dists[i][8]+dists[i][18]) / 2;
            dI = (dists[i][9]+dists[i][19]) / 2;
            dJ = (dists[i][10]+dists[i][20]) / 2;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);
            topoPoint F(hF,zF,dF,false);
            temp.push_back(F);
            topoPoint G(hG,zG,dG,false);
            temp.push_back(G);
            topoPoint H(hH,zH,dH,false);
            temp.push_back(H);
            topoPoint I(hI,zI,dI,false);
            temp.push_back(I);
            topoPoint J(hJ,zJ,dJ,false);
            temp.push_back(J);

            temp.push_back(EXTRA);

//                               cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }

        points.push_back(temp);
        temp.clear();
    }

    pointsPrint("points0.txt");
}

void SMMTleverARM::pointsPrint(string filename)
{
    ofstream outFile(filename);

    for (unsigned ii = 0; ii < points.size(); ii++)
    {
        for (unsigned jj = 0; jj < points[ii].size(); jj++)
        {
            outFile << points[ii][jj].XYZ(0) <<" ";
            outFile << points[ii][jj].XYZ(1) <<" ";
            outFile << points[ii][jj].XYZ(2) << endl;
        }
    }

    outFile.close();
}

void SMMTleverARM::horiz()
{
    vector<GMS> temp;
    int g,m,s;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            g = stoi(data[i][j].substr(23,3));
            m = stoi(data[i][j].substr(27,2));
            s = stoi(data[i][j].substr(29,2));
            GMS temp2(g,m,s);

            temp.push_back(temp2);
        }
        hor.push_back(temp);
        temp.clear();
    }

}

void SMMTleverARM::zenit()
{
    vector<GMS> temp;
    int g,m,s;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            g = stoi(data[i][j].substr(35,3));
            m = stoi(data[i][j].substr(39,2));
            s = stoi(data[i][j].substr(41,2));
            GMS temp2(g,m,s);
//            cout<<temp2.gdec()<<endl;

            temp.push_back(temp2);
        }
        zen.push_back(temp);
        temp.clear();
    }

}

void SMMTleverARM::distanc()
{
    vector<double> temp;
    double temp2;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            temp2 = stod(data[i][j].substr(47,10));
//            cout<<temp2<<endl;

            temp.push_back(temp2);
        }
        dists.push_back(temp);
        temp.clear();
    }

}

topoPoint::topoPoint(double h,double z,double di,bool isRad)
{
    Hz=h;
    Ze=z;
    Di=di;
    rad = isRad;
    cart();
}

void topoPoint::cart()
{
    double X,Y,Z;

    if (rad)
    {
        X = std::sin(Ze) * std::sin(Hz) * Di;
        Y = std::sin(Ze) * std::cos(Hz) * Di;
        Z = std::cos(Ze) * Di;
    }
    else
    {
        X = std::sin(Ze*(datum::pi/180)) * std::sin(Hz*(datum::pi/180)) * Di;
        Y = std::sin(Ze*(datum::pi/180)) * std::cos(Hz*(datum::pi/180)) * Di;
        Z = std::cos(Ze*(datum::pi/180)) * Di;
    }

    vec3 temp = {X,Y,Z};

    XYZ = temp;
}

double pdpiH(double h1,double h2)
{
    if (h1 > h2)
    {
        return (h1+h2+180)/2;
    }
    else
    {
        return (h1+h2-180)/2;
    }
}

double pdpiV(double v1,double v2)
{
    if (v1 < v2)
    {
        return (v1-v2+360)/2;
    }
    else
    {
        return (v2-v1+360)/2;
    }
}

vec4 plane3points(vec3 u,vec3 v,vec3 w)
{
//the function returns a 4-vector with the plane equation coefficients
//assuming the form  ax + by + cz + d = 0
    vec4 res;
    vec3 p1,p2,n;
    vec d;
    p1 = normalise(v - u);
    p2 = normalise(w - u);
    n  = normalise(cross(p1,p2));
    d = - dot(n,u);

    res = join_vert(n,d);

//cout << res <<endl;
    return res;
}

void topoPoint::translate(vec3 tvec)
{
    XYZ += tvec;
}

void topoPoint::Rotate(mat R)
{
    XYZ = R * XYZ;
}

void TptMat (mat *M,vec Tvec)
{
    mat temp = *M;

    if(temp.n_cols == Tvec.n_elem)
    {
        for (uword i = 0; i < temp.n_rows; i++)
        {
            temp.row(i) = trans(trans(temp.row(i)) + Tvec );
        }
    }
    else
    {
        cout << "nada realizado, tamanho de colunas da matriz diferente do numero de elementos do vetor dado"<<endl;
    }

    *M = temp;
}

void RptMat (mat *M,mat R)
{
    mat temp = *M;

    if( (temp.n_cols == R.n_cols) & (R.n_cols == R.n_rows) )
    {
        for (uword i = 0; i < temp.n_rows; i++)
        {
            temp.row(i) = trans(R*trans(temp.row(i)));
        }
    }
    else
    {
        cout << "nada realizado, tamanho de colunas da matriz diferente do numero de colunas da matriz dada"<<endl;
        cout<<"ou a matriz fornecida não é quadrada"<<endl;
    }

    *M = temp;
}

void SMMTleverARM::translateAll(vec3 tvec)
{
    for (unsigned int i = 0; i < points.size(); i++)
    {
        for (unsigned int j = 0; j < points[i].size(); j++)
        {
            points[i][j].translate(tvec);
        }
    }
}

void SMMTleverARM::rotAll (mat R)
{
    for (unsigned int i = 0; i < points.size(); i++)
    {
        for (unsigned int j = 0; j < points[i].size(); j++)
        {
            points[i][j].Rotate(R);
        }
    }
}

mat topoPointsMat (vector<topoPoint> points,uword first,uword last)
{
    mat res;

    uword nlin = last - first + 1;

    res.zeros(nlin,3);

    for (uword i = first; i < last+1; i++)
    {
        res.row(i-first) = trans(points.at(i).XYZ);
//        cout << i <<endl;
    }

//    cout<<res<<endl;

    return res;
}

vec4 parallelPlane(vec4 plane1,vec3 pointOnPlane)
{
    vec4 res;
    vec3 n = normalise(plane1.rows(0,2));

    res.rows(0,2) = n;
    res(3) = - dot(n,pointOnPlane);

    return res;
}

vec3 ProjPtOrtPlane(vec3 pt2Proj,vec3 plNorm,vec3 ptOnPlane)
{
    return pt2Proj + (dot(normalise(plNorm),ptOnPlane-pt2Proj)) * normalise(plNorm);
}

vec3 onPlanePoint(vec4 plCoef,double x,double y)
{
    vec3 res;

    res(0) = x;
    res(1) = y;
    res(2) = - (plCoef(0)*x+plCoef(1)*y+plCoef(3))/plCoef(2);

    return res;
}

void SMMTleverARM::report()
{
    ofstream out("report.txt");
    out.precision(8);

    out << "Relatório de Saída do Processamento da ";
    out<< "determinação dos parâmetros de orientação relativa"   <<endl;
    out<<"do Sistema de Mapeamento Móvel Terrestre do LAPE"<<endl<<endl<<endl;

    out << "Lever Arm (no body-frame da IMU) --------------------------------"<<endl<<endl;

    out<< "IMU -> Antena"<<endl;
    out << "X(m): " <<antLA(0)<< " | Y(m): " <<antLA(1)<< " | Z(m): " <<antLA(2)<<endl<<endl;

    out<< "IMU -> Câmera Esquerda"<<endl;
    out << "X(m): " <<LcamLA(0)<< " | Y(m): " <<LcamLA(1)<< " | Z(m): " <<LcamLA(2)<<endl<<endl;

    out<< "IMU -> Câmera Direita"<<endl;
    out << "X(m): " <<RcamLA(0)<< " | Y(m): " <<RcamLA(1)<< " | Z(m): " <<RcamLA(2)<<endl<<endl<<endl;

    out<<"Distancias:"<<endl;
    out<<"Entre câmeras: "<<arma::norm(LcamLA-RcamLA)<<endl;
    out<<"IMU - Antena: "<<arma::norm(antLA)<<endl;
    out<<"IMU - Cam Esq. : "<<arma::norm(LcamLA)<<endl;
    out<<"IMU - Cam Dir. : "<<arma::norm(RcamLA)<<endl;

    out<< "Matrizes do Boresight (cossenos diretores): "<<endl<<endl;
    Rimu_LC.raw_print(out,"IMU (BF) -> Câmera Esquerda (BF)");
    out<<endl;
    Rimu_RC.raw_print(out,"IMU (BF) -> Câmera Direita (BF)");
    out<<endl<<endl;

    out<< "Matrizes do Boresight (cossenos diretores, mesma convenção da IMU): "<<endl<<endl;
    Rimu_LC2.raw_print(out,"IMU (BF) -> Câmera Esquerda (BF)");
    out<<endl;
    Rimu_RC2.raw_print(out,"IMU (BF) -> Câmera Direita (BF)");
    out<<endl<<endl;

    out<<"no último caso, os angulos formados com os eixos do BF da IMU: "<<endl<<endl;

    out<<"IMU (BF) -> Câmera Esquerda (BF)"<<endl<<arma::acos(Rimu_LC2)*(180/datum::pi)<<endl<<endl;
    out<<"IMU (BF) -> Câmera Direita (BF)"<<endl<<arma::acos(Rimu_RC2)*(180/datum::pi)<<endl;

    out<<endl<<"testes de consistência dos resultados"<<endl;
    out<<"as matrizes de rotação deverão, ao ser multiplicadas por sua transposta, serem iguais a matriz identidade"<<endl<<endl;

    out<<"Rtopo_imu * Rtopo_imu.t()"<<endl<<Rot1 * Rot1.t()<<endl;

    out<<"Rimu_LC * Rimu_LC.t()"<<endl<<Rimu_LC * Rimu_LC.t()<<endl;
    out<<"Rimu_RC * Rimu_RC.t()"<<endl<<Rimu_RC * Rimu_RC.t()<<endl;

    out<<"Rimu_LC2 * Rimu_LC2.t()"<<endl<<Rimu_LC2 * Rimu_LC2.t()<<endl;
    out<<"Rimu_RC2 * Rimu_RC2.t()"<<endl<<Rimu_RC2 * Rimu_RC2.t()<<endl;

}

ext: hpp

max_stars_repo_name: kauevestena/smmt

max_issues_repo_name: kauevestena/smmt

max_forks_repo_name: kauevestena/smmt


text:
#include <memory>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "block_system.hpp"
#include "bsgs.hpp"
#include "dbg.hpp"
#include "perm.hpp"
#include "perm_group.hpp"
#include "perm_set.hpp"

namespace mpsym
{

namespace internal
{

std::vector<PermGroup> PermGroup::wreath_decomposition() const
{
  DBG(DEBUG) << "Finding wreath product decomposition for";
  DBG(DEBUG) << *this;

  for (BlockSystem const &block_system : BlockSystem::non_trivial(*this)) {
    DBG(TRACE) << "Considering block system:";
    DBG(TRACE) << block_system;

    // determine block permuter subgroup
    PermGroup block_permuter(block_system.size(),
                             block_system.block_permuter(generators()));

    DBG(TRACE) << "Block permuter is:";
    DBG(TRACE) << block_permuter;

    // determine block stabilizer subgroups
    auto stabilizers(wreath_decomp_find_stabilizers(
      block_system, block_permuter));

    if (stabilizers.empty())
      continue;

    // check if a monomorphism can be found heuristically
    auto block_permuter_image(wreath_decomp_construct_block_permuter_image(
      block_system, block_permuter));

    bool found_monomorphism(wreath_decomp_reconstruct_block_permuter(
      block_system, block_permuter, block_permuter_image));

    if (!found_monomorphism)
      break;

    // construct the wreath decomposition
    std::vector<PermGroup> decomposition(block_system.size() + 1u);

    decomposition[0] = PermGroup(degree(), block_permuter_image);
    for (unsigned i = 0u; i < block_system.size(); ++i)
      decomposition[i + 1u] = stabilizers[i];

    DBG(DEBUG) << "=> Found wreath product decomposition:";
#ifndef NDEBUG
    for (PermGroup const &pg : decomposition)
      DBG(DEBUG) << pg;
#endif

    return decomposition;
  }

  DBG(DEBUG) << "=> No wreath product decomposition found";
  return {};
}

std::vector<PermGroup> PermGroup::wreath_decomp_find_stabilizers(
  BlockSystem const &block_system,
  PermGroup const &block_permuter) const
{
  using boost::multiprecision::pow;

  std::vector<PermGroup> stabilizers(block_system.size());

  auto create_stabilizer = [&](unsigned i) {
    auto block(block_system[i]);

    // find stabilizer subgroup generators
    auto stabilizer_generators(
      BlockSystem::block_stabilizers(generators(), block));

    // restrict stabilizer subgroup generators to block
    PermSet stabilizer_generators_restricted;
    for (Perm const &gen : stabilizer_generators) {
      auto gen_restricted(gen.restricted(block.begin(), block.end()));

      if (!gen_restricted.id())
        stabilizer_generators_restricted.insert(gen_restricted);
    }

    // construct stabilizer subgroup
    stabilizers[i] = PermGroup(degree(), stabilizer_generators_restricted);

    DBG(TRACE) << "Block stabilizer of " << block_system[i] << ":";
    DBG(TRACE) << stabilizers[i];
  };

  // determine stabilizer subgroup of first block
  create_stabilizer(0);

  // skip blocksystem if order equality not fullfilled
  BSGS::order_type expected_order = pow(
    stabilizers[0].order(), block_system.size()) * block_permuter.order();

  if (_order != expected_order) {
    DBG(TRACE) << "Group order equality not satisfied";
    return {};
  }

  // determine stabilizers subgroups of remaining blocks
  for (unsigned i = 1u; i < block_system.size(); ++i)
    create_stabilizer(i);

  return stabilizers;
}

PermSet PermGroup::wreath_decomp_construct_block_permuter_image(
  BlockSystem const &block_system,
  PermGroup const &block_permuter) const
{
  PermSet block_permuter_image;

  for (Perm const &gen : block_permuter.generators()) {
    std::vector<unsigned> perm(degree());

    for (unsigned i = 0u; i < block_system.size(); ++i) {
      auto block(block_system[i]);

      for (auto j = 0u; j < block.size(); ++j)
        perm[block[j] - 1u] = block_system[gen[i + 1u] - 1u][j];
    }

    block_permuter_image.insert(Perm(perm));
  }

  DBG(TRACE) << "Heuristic monomorphism image generators:";
  DBG(TRACE) << block_permuter_image;

  return block_permuter_image;
}

bool PermGroup::wreath_decomp_reconstruct_block_permuter(
  BlockSystem const &block_system,
  PermGroup const &block_permuter,
  PermSet const &block_permuter_image) const
{
  bool found_monomorphism = true;

  PermSet block_permuter_reconstruction;

  for (Perm const &gen : block_permuter_image) {
    std::vector<unsigned> perm(block_system.size());

    for (unsigned i = 0u; i < block_system.size(); ++i)
      perm[i] = block_system.block_index(gen[block_system[i][0]]) + 1u;

    Perm reconstructed_gen(perm);

    if (!block_permuter.contains_element(reconstructed_gen)) {
      found_monomorphism = false;
      break;
    }

    block_permuter_reconstruction.insert(reconstructed_gen);
  }

  DBG(TRACE) << "Block permuter reconstruction yields generators:";
  DBG(TRACE) << block_permuter_reconstruction;

  if (found_monomorphism) {
    if (PermGroup(block_system.size(), block_permuter_reconstruction).order()
        != block_permuter.order())
      found_monomorphism = false;
  }

  if (!found_monomorphism) {
    DBG(WARN) << "Wreath decomposition exists but was not found by heuristic";
  }

  return found_monomorphism;
}

} // namespace internal

} // namespace mpsym

ext: cpp


text: #include <boost/multi_index/ordered_index.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <main.h>
#include <iostream>
#include <GL/glew.h>
#include <Eigen/OpenGLSupport>
#include <GL/glut.h>
using namespace Eigen;

#define VERIFY_MATRIX(CODE,REF) { \
    glLoadIdentity(); \
    CODE; \
    Matrix<float,4,4,ColMajor> m; m.setZero(); \
    glGet(GL_MODELVIEW_MATRIX, m); \
    if(!(REF).cast<float>().isApprox(m)) { \
      std::cerr << "Expected:\n" << ((REF).cast<float>()) << "\n" << "got\n" << m << "\n\n"; \
    } \
    VERIFY_IS_APPROX((REF).cast<float>(), m); \
  }

#define VERIFY_UNIFORM(SUFFIX,NAME,TYPE) { \
    TYPE value; value.setRandom(); \
    TYPE data; \
    int loc = glGetUniformLocation(prg_id, #NAME); \
    VERIFY((loc!=-1) && "uniform not found"); \
    glUniform(loc,value); \
    EIGEN_CAT(glGetUniform,SUFFIX)(prg_id,loc,data.data()); \
    if(!value.isApprox(data)) { \
      std::cerr << "Expected:\n" << value << "\n" << "got\n" << data << "\n\n"; \
    } \
    VERIFY_IS_APPROX(value, data); \
  }

#define VERIFY_UNIFORMi(NAME,TYPE) { \
    TYPE value = TYPE::Random().eval().cast<float>().cast<TYPE::Scalar>(); \
    TYPE data; \
    int loc = glGetUniformLocation(prg_id, #NAME); \
    VERIFY((loc!=-1) && "uniform not found"); \
    glUniform(loc,value); \
    glGetUniformiv(prg_id,loc,(GLint*)data.data()); \
    if(!value.isApprox(data)) { \
      std::cerr << "Expected:\n" << value << "\n" << "got\n" << data << "\n\n"; \
    } \
    VERIFY_IS_APPROX(value, data); \
  }

void printInfoLog(GLuint objectID)
{
    int infologLength, charsWritten;
    GLchar *infoLog;
    glGetProgramiv(objectID,GL_INFO_LOG_LENGTH, &infologLength);
    if(infologLength > 0)
    {
        infoLog = new GLchar[infologLength];
        glGetProgramInfoLog(objectID, infologLength, &charsWritten, infoLog);
        if (charsWritten>0)
          std::cerr << "Shader info : \n" << infoLog << std::endl;
        delete[] infoLog;
    }
}

GLint createShader(const char* vtx, const char* frg)
{
  GLint prg_id = glCreateProgram();
  GLint vtx_id = glCreateShader(GL_VERTEX_SHADER);
  GLint frg_id = glCreateShader(GL_FRAGMENT_SHADER);
  GLint ok;

  glShaderSource(vtx_id, 1, &vtx, 0);
  glCompileShader(vtx_id);
  glGetShaderiv(vtx_id,GL_COMPILE_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "vtx compilation failed\n";
  }

  glShaderSource(frg_id, 1, &frg, 0);
  glCompileShader(frg_id);
  glGetShaderiv(frg_id,GL_COMPILE_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "frg compilation failed\n";
  }

  glAttachShader(prg_id, vtx_id);
  glAttachShader(prg_id, frg_id);
  glLinkProgram(prg_id);
  glGetProgramiv(prg_id,GL_LINK_STATUS,&ok);
  if(!ok)
  {
    std::cerr << "linking failed\n";
  }
  printInfoLog(prg_id);

  glUseProgram(prg_id);
  return prg_id;
}

void test_openglsupport()
{
  int argc = 0;
  glutInit(&argc, 0);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowPosition (0,0);
  glutInitWindowSize(10, 10);

  if(glutCreateWindow("Eigen") <= 0)
  {
    std::cerr << "Error: Unable to create GLUT Window.\n";
    exit(1);
  }

  glewExperimental = GL_TRUE;
  if(glewInit() != GLEW_OK)
  {
    std::cerr << "Warning: Failed to initialize GLEW\n";
  }

  Vector3f v3f;
  Matrix3f rot;
  glBegin(GL_POINTS);

  glVertex(v3f);
  glVertex(2*v3f+v3f);
  glVertex(rot*v3f);

  glEnd();

  // 4x4 matrices
  Matrix4f mf44; mf44.setRandom();
  VERIFY_MATRIX(glLoadMatrix(mf44), mf44);
  VERIFY_MATRIX(glMultMatrix(mf44), mf44);
  Matrix4d md44; md44.setRandom();
  VERIFY_MATRIX(glLoadMatrix(md44), md44);
  VERIFY_MATRIX(glMultMatrix(md44), md44);

  // Quaternion
  Quaterniond qd(AngleAxisd(internal::random<double>(), Vector3d::Random()));
  VERIFY_MATRIX(glRotate(qd), Projective3d(qd).matrix());

  Quaternionf qf(AngleAxisf(internal::random<double>(), Vector3f::Random()));
  VERIFY_MATRIX(glRotate(qf), Projective3f(qf).matrix());

  // 3D Transform
  Transform<float,3,AffineCompact> acf3; acf3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(acf3), Projective3f(acf3).matrix());
  VERIFY_MATRIX(glMultMatrix(acf3), Projective3f(acf3).matrix());

  Transform<float,3,Affine> af3(acf3);
  VERIFY_MATRIX(glLoadMatrix(af3), Projective3f(af3).matrix());
  VERIFY_MATRIX(glMultMatrix(af3), Projective3f(af3).matrix());

  Transform<float,3,Projective> pf3; pf3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(pf3), Projective3f(pf3).matrix());
  VERIFY_MATRIX(glMultMatrix(pf3), Projective3f(pf3).matrix());

  Transform<double,3,AffineCompact> acd3; acd3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(acd3), Projective3d(acd3).matrix());
  VERIFY_MATRIX(glMultMatrix(acd3), Projective3d(acd3).matrix());

  Transform<double,3,Affine> ad3(acd3);
  VERIFY_MATRIX(glLoadMatrix(ad3), Projective3d(ad3).matrix());
  VERIFY_MATRIX(glMultMatrix(ad3), Projective3d(ad3).matrix());

  Transform<double,3,Projective> pd3; pd3.matrix().setRandom();
  VERIFY_MATRIX(glLoadMatrix(pd3), Projective3d(pd3).matrix());
  VERIFY_MATRIX(glMultMatrix(pd3), Projective3d(pd3).matrix());

  // translations (2D and 3D)
  {
    Vector2f vf2; vf2.setRandom(); Vector3f vf23; vf23 << vf2, 0;
    VERIFY_MATRIX(glTranslate(vf2), Projective3f(Translation3f(vf23)).matrix());
    Vector2d vd2; vd2.setRandom(); Vector3d vd23; vd23 << vd2, 0;
    VERIFY_MATRIX(glTranslate(vd2), Projective3d(Translation3d(vd23)).matrix());

    Vector3f vf3; vf3.setRandom();
    VERIFY_MATRIX(glTranslate(vf3), Projective3f(Translation3f(vf3)).matrix());
    Vector3d vd3; vd3.setRandom();
    VERIFY_MATRIX(glTranslate(vd3), Projective3d(Translation3d(vd3)).matrix());

    Translation<float,3> tf3; tf3.vector().setRandom();
    VERIFY_MATRIX(glTranslate(tf3), Projective3f(tf3).matrix());

    Translation<double,3> td3;  td3.vector().setRandom();
    VERIFY_MATRIX(glTranslate(td3), Projective3d(td3).matrix());
  }

  // scaling (2D and 3D)
  {
    Vector2f vf2; vf2.setRandom(); Vector3f vf23; vf23 << vf2, 1;
    VERIFY_MATRIX(glScale(vf2), Projective3f(Scaling(vf23)).matrix());
    Vector2d vd2; vd2.setRandom(); Vector3d vd23; vd23 << vd2, 1;
    VERIFY_MATRIX(glScale(vd2), Projective3d(Scaling(vd23)).matrix());

    Vector3f vf3; vf3.setRandom();
    VERIFY_MATRIX(glScale(vf3), Projective3f(Scaling(vf3)).matrix());
    Vector3d vd3; vd3.setRandom();
    VERIFY_MATRIX(glScale(vd3), Projective3d(Scaling(vd3)).matrix());

    UniformScaling<float> usf(internal::random<float>());
    VERIFY_MATRIX(glScale(usf), Projective3f(usf).matrix());

    UniformScaling<double> usd(internal::random<double>());
    VERIFY_MATRIX(glScale(usd), Projective3d(usd).matrix());
  }

  // uniform
  {
    const char* vtx = "void main(void) { gl_Position = gl_Vertex; }\n";

    if(GLEW_VERSION_2_0)
    {
      #ifdef GL_VERSION_2_0
      const char* frg = ""
        "uniform vec2 v2f;\n"
        "uniform vec3 v3f;\n"
        "uniform vec4 v4f;\n"
        "uniform ivec2 v2i;\n"
        "uniform ivec3 v3i;\n"
        "uniform ivec4 v4i;\n"
        "uniform mat2 m2f;\n"
        "uniform mat3 m3f;\n"
        "uniform mat4 m4f;\n"
        "void main(void) { gl_FragColor = vec4(v2f[0]+v3f[0]+v4f[0])+vec4(v2i[0]+v3i[0]+v4i[0])+vec4(m2f[0][0]+m3f[0][0]+m4f[0][0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      VERIFY_UNIFORM(fv,v2f, Vector2f);
      VERIFY_UNIFORM(fv,v3f, Vector3f);
      VERIFY_UNIFORM(fv,v4f, Vector4f);
      VERIFY_UNIFORMi(v2i, Vector2i);
      VERIFY_UNIFORMi(v3i, Vector3i);
      VERIFY_UNIFORMi(v4i, Vector4i);
      VERIFY_UNIFORM(fv,m2f, Matrix2f);
      VERIFY_UNIFORM(fv,m3f, Matrix3f);
      VERIFY_UNIFORM(fv,m4f, Matrix4f);
      #endif
    }
    else
      std::cerr << "Warning: opengl 2.0 was not tested\n";

    if(GLEW_VERSION_2_1)
    {
      #ifdef GL_VERSION_2_1
      const char* frg = "#version 120\n"
        "uniform mat2x3 m23f;\n"
        "uniform mat3x2 m32f;\n"
        "uniform mat2x4 m24f;\n"
        "uniform mat4x2 m42f;\n"
        "uniform mat3x4 m34f;\n"
        "uniform mat4x3 m43f;\n"
        "void main(void) { gl_FragColor = vec4(m23f[0][0]+m32f[0][0]+m24f[0][0]+m42f[0][0]+m34f[0][0]+m43f[0][0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Matrix<float,2,3> Matrix23f;
      typedef Matrix<float,3,2> Matrix32f;
      typedef Matrix<float,2,4> Matrix24f;
      typedef Matrix<float,4,2> Matrix42f;
      typedef Matrix<float,3,4> Matrix34f;
      typedef Matrix<float,4,3> Matrix43f;

      VERIFY_UNIFORM(fv,m23f, Matrix23f);
      VERIFY_UNIFORM(fv,m32f, Matrix32f);
      VERIFY_UNIFORM(fv,m24f, Matrix24f);
      VERIFY_UNIFORM(fv,m42f, Matrix42f);
      VERIFY_UNIFORM(fv,m34f, Matrix34f);
      VERIFY_UNIFORM(fv,m43f, Matrix43f);
      #endif
    }
    else
      std::cerr << "Warning: opengl 2.1 was not tested\n";

    if(GLEW_VERSION_3_0)
    {
      #ifdef GL_VERSION_3_0
      const char* frg = "#version 150\n"
        "uniform uvec2 v2ui;\n"
        "uniform uvec3 v3ui;\n"
        "uniform uvec4 v4ui;\n"
        "out vec4 data;\n"
        "void main(void) { data = vec4(v2ui[0]+v3ui[0]+v4ui[0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Matrix<unsigned int,2,1> Vector2ui;
      typedef Matrix<unsigned int,3,1> Vector3ui;
      typedef Matrix<unsigned int,4,1> Vector4ui;

      VERIFY_UNIFORMi(v2ui, Vector2ui);
      VERIFY_UNIFORMi(v3ui, Vector3ui);
      VERIFY_UNIFORMi(v4ui, Vector4ui);
      #endif
    }
    else
      std::cerr << "Warning: opengl 3.0 was not tested\n";

    #ifdef GLEW_ARB_gpu_shader_fp64
    if(GLEW_ARB_gpu_shader_fp64)
    {
      #ifdef GL_ARB_gpu_shader_fp64
      const char* frg = "#version 150\n"
        "uniform dvec2 v2d;\n"
        "uniform dvec3 v3d;\n"
        "uniform dvec4 v4d;\n"
        "out vec4 data;\n"
        "void main(void) { data = vec4(v2d[0]+v3d[0]+v4d[0]); }\n";

      GLint prg_id = createShader(vtx,frg);

      typedef Vector2d Vector2d;
      typedef Vector3d Vector3d;
      typedef Vector4d Vector4d;

      VERIFY_UNIFORM(dv,v2d, Vector2d);
      VERIFY_UNIFORM(dv,v3d, Vector3d);
      VERIFY_UNIFORM(dv,v4d, Vector4d);
      #endif
    }
    else
      std::cerr << "Warning: GLEW_ARB_gpu_shader_fp64 was not tested\n";
    #else
      std::cerr << "Warning: GLEW_ARB_gpu_shader_fp64 was not tested\n";
    #endif
  }

}


text:
#include <boost/bind.hpp>
#include <boost/phoenix/object/construct.hpp>
#include <boost/spirit/home/x3.hpp>
#include <boost/spirit/include/lex.hpp>

#include <functional>

#include <fmt/core.h>

#include "url_parser.hpp"

parsed_url parse(std::string url) noexcept {

    parsed_url desiderata;

    using boost::spirit::lex::_start;
    using boost::spirit::lex::_end;

    if( boost::spirit::x3::phrase_parse(url.begin(),
                                        url.end(),
                                        (-((+boost::spirit::x3::alpha) >> "://")
                                                                   [ ([&](auto &ctx) mutable{ auto str = boost::optional<std::string>(boost::spirit::x3::_attr(ctx)); desiderata.protocol = str.has_value() ? str.value() : std::string();}) ])
                                                    >> (+(boost::spirit::x3::char_ - (boost::spirit::x3::char_("[/?]"))))[([&desiderata](auto &ctx) mutable {
                                                                                auto str = boost::optional<std::string>(boost::spirit::x3::_attr(ctx));

                                                                                if(! str.has_value()) {
                                                                                desiderata.host = "";
                                                                                desiderata.port = "";
                                                                                return;
                                                                                }

                                                                                const std::string authority_text = str.value();

                                                                                auto colon_itr = std::find(authority_text.begin(), authority_text.end(), ':');

                                                                                if(colon_itr == authority_text.end()) {
                                                                                desiderata.host = authority_text;
                                                                                desiderata.port = desiderata.protocol == "http" ? "80" : "443";
                                                                                return;
                                                                                }

                                                                                auto const host = std::string(authority_text.begin(), colon_itr);
                                                                                auto const port = std::string(colon_itr+1, authority_text.end());

                                                                                desiderata.host = host;
                                                                                desiderata.port = std::to_string(std::atoi(port.c_str()));

                                                                                })]
                                                    >> -(
                                                        (+(boost::spirit::x3::char_ - boost::spirit::x3::char_("?")))[ ([&](auto &ctx) mutable {auto str = boost::optional<std::string>(boost::spirit::x3::_attr(ctx)); desiderata.path = str.has_value()? str.value() : std::string();})]
                                                    )
                                                    >> -(
                                                        (+(boost::spirit::x3::char_ - boost::spirit::x3::char_("#")))[([&](auto &ctx){auto str = boost::optional<std::string>(boost::spirit::x3::_attr(ctx)); desiderata.query = str.has_value() ? str.value() : std::string();})]
                                                    )
                                         >> -((+(boost::spirit::x3::char_))[([&]([[maybe_unused]] auto &ctx){auto str = boost::optional<std::string>(boost::spirit::x3::_attr(ctx)); desiderata.fragment = str.has_value() ? str.value() : std::string();})])
                                        ,
                                        boost::spirit::x3::space)) {
        // If parse was successful
        desiderata.total_path = desiderata.path + desiderata.query + desiderata.fragment;
        return desiderata;
    }

    fmt::print("Failed to parse: {}\n", url);

    return desiderata;
}


text:
// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/string.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

// By default, this is disabled
#ifdef BOOST_HANA_CONFIG_ENABLE_STRING_UDL

    constexpr auto str = "Hello world!"_s;
    BOOST_HANA_CONSTANT_CHECK(str == hana::string_c<'H', 'e', 'l', 'l', 'o', ' ',
                                                    'w', 'o', 'r', 'l', 'd', '!'>);

    BOOST_HANA_CONSTANT_CHECK(hana::is_a<hana::string_tag>(str));
    BOOST_HANA_CONSTANT_CHECK(hana::length(str) == hana::size_c<12>);

#endif

int main() { }


text:
#include <gtest/gtest.h>
#include <tuple>
#include <string>
#include <vector>
#include <iostream>

#include <boost/asio.hpp>
#include <boost/asio/buffer.hpp>
namespace net = boost::asio;
using tcp = net::ip::tcp;

#include <libcoind/jsonrpc/jsonrpc_coind.h>
#include <thread>

#ifdef PASS_EXIST
#include "pass.h"
#endif

using namespace coind::jsonrpc;
using namespace coind::jsonrpc::data;
using namespace std;

class Bitcoind_JSONRPC : public ::testing::Test
{
protected:
	std::shared_ptr<coind::JSONRPC_Coind> coind;

protected:

    virtual void SetUp()
    {
#ifdef PASS_EXIST
		auto context = std::make_shared<boost::asio::io_context>(2);

		auto _pass = get_pass();
		char *login = std::get<0>(_pass);
		char *addr = std::get<1>(_pass);
		char *port = std::get<2>(_pass);
		coind = std::make_shared<coind::JSONRPC_Coind>(context, std::make_shared<coind::DigibyteParentNetwork>(), addr, port, login);

#else
        coind = new Coind("bitcoin", "B1TC01ND", "http://127.0.0.1:8332/", std::make_shared<c2pool::DigibyteNetwork>());
#endif
    }

    virtual void TearDown()
    {

    }
};

TEST_F(Bitcoind_JSONRPC, getblockchaininfo)
{
	auto result = coind->getblockchaininfo();
	std::cout << result.write() << std::endl;
	ASSERT_EQ(result["chain"].get_str(), "main");
}

TEST_F(Bitcoind_JSONRPC, getmininginfo)
{
	auto result = coind->getmininginfo();
	std::cout << result.write() << std::endl;
	ASSERT_EQ(result["chain"].get_str(), "main");
}

TEST_F(Bitcoind_JSONRPC, getblock)
{
	uint256 block_hash;
	block_hash.SetHex("36643871691af2e0b6bfc7898a4399133c6e6e4885a67f9d9fe36942c2513772");
	auto getblock_req = std::make_shared<GetBlockRequest>(block_hash);

	auto result = coind->getblock(getblock_req);
	std::cout << result.write() << std::endl;
	ASSERT_EQ(result["nonce"].get_int(), 328117616);
}

TEST_F(Bitcoind_JSONRPC, getwork)
{
    coind::TXIDCache txidcache;
    map<uint256, coind::data::tx_type> known_txs;
    auto result = coind->getwork(txidcache, known_txs);

    std::cout << "version: " << result.version << std::endl;
    std::cout << "previous_block: " << result.previous_block.GetHex() << std::endl;
    std::cout << "transactions: ";
    for (auto v: result.transactions) {
        std::cout << v->version << std::endl;
    }
    std::cout << std::endl;

}

TEST_F(Bitcoind_JSONRPC, multi_getwork)
{
    coind::TXIDCache txidcache;
    map<uint256, coind::data::tx_type> known_txs;

    for (int i = 0; i < 10; i++) {
        auto result = coind->getwork(txidcache, known_txs);

        std::cout << "version: " << result.version << std::endl;
        std::cout << "previous_block: " << result.previous_block.GetHex() << std::endl;
        std::cout << "transactions: ";
        for (auto v: result.transactions) {
            std::cout << v->version << std::endl;
        }
        std::cout << std::endl;
        std::this_thread::sleep_for(std::chrono::seconds(5));
    }
}

max_issues_repo_name: frstrtr/cpool

max_forks_repo_name: frstrtr/cpool


text:
// Copyright © 2017-2020 Khaos Wallet.
//
// This file is part of Trust. The full Trust copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include "Crc.h"

#include <boost/crc.hpp>  // for boost::crc_32_type

#include <string>

using namespace TW;

uint16_t Crc::crc16(uint8_t* bytes, uint32_t length) {
    // Calculate checksum for existing bytes
    uint16_t crc = 0x0000;
    const uint16_t polynomial = 0x1021;

    for (auto i = 0; i < length; i++) {
        const auto byte = bytes[i];
        for (auto bitidx = 0; bitidx < 8; bitidx++) {
            const auto bit = ((byte >> (7 - bitidx) & 1) == 1);
            const auto c15 = ((crc >> 15 & 1) == 1);
            crc <<= 1;
            if (c15 ^ bit) {
                crc ^= polynomial;
            }
        }
    }

    return crc & 0xffff;
}

uint32_t Crc::crc32(const Data& data)
{
    boost::crc_32_type result;
    result.process_bytes((const void*)data.data(), data.size());
    return (uint32_t)result.checksum();
}

uint32_t Crc::crc32C(const Data& data)
{
    using crc_32c_type = boost::crc_optimal<32, 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true>;
    crc_32c_type result;
    result.process_bytes((const void*)data.data(), data.size());
    return (uint32_t)result.checksum();
}

max_stars_repo_name: Khaos-Labs/khaos-wallet-core

max_issues_repo_name: Khaos-Labs/khaos-wallet-core

max_forks_repo_name: Khaos-Labs/khaos-wallet-core


text:
#pragma once

#include <stdexcept>
#include <boost/filesystem.hpp>

namespace cuttle {
	namespace fileui {
		class incorrect_module_structure_error : public std::logic_error {
		public:
			incorrect_module_structure_error() : logic_error("Incorrect module structure error") {}

            explicit incorrect_module_structure_error(const boost::filesystem::path &path)
					: logic_error("Incorrect module structure error at '" + path.string() + "'") {}
		};
	}
}

max_stars_repo_name: cuttle-system/cuttle-fileui

max_issues_repo_name: cuttle-system/cuttle-fileui

max_forks_repo_name: cuttle-system/cuttle-fileui


text:
// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <iostream>
#include <fstream>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "gtest/gtest.h"

#include "modules/commons/util.hpp"

// TODO(fortiss): fill our this test
TEST(load_test, param_tests) {
  bool temp = false;
  BARK_EXPECT_TRUE(temp);

}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

ext: cc