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: Mario Prats, Ioan Sucan */

#include <main_window.h>
#include <job_processing.h>
#include <benchmark_processing_thread.h>

#include <moveit/kinematic_constraints/utils.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/warehouse/constraints_storage.h>
#include <moveit/warehouse/state_storage.h>
#include <moveit/benchmarks/benchmark_execution.h>
#include <moveit/planning_interface/planning_interface.h>

#include <pluginlib/class_loader.h>

#include <rviz/display_context.h>
#include <rviz/window_manager_interface.h>

#include <eigen_conversions/eigen_msg.h>

#include <QMessageBox>
#include <QInputDialog>
#include <QFileDialog>
#include <QProgressDialog>

#include <boost/math/constants/constants.hpp>

namespace benchmark_tool
{
void MainWindow::createGoalAtPose(const std::string &name, const Eigen::Affine3d &pose)
{
  goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(name, pose));

  geometry_msgs::Pose marker_pose;
  tf::poseEigenToMsg(pose * goal_offset_, marker_pose);
  static const float marker_scale = 0.15;

  GripperMarkerPtr goal_pose(new GripperMarker(
      scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_,
      name, scene_display_->getRobotModel()->getModelFrame(), robot_interaction_->getActiveEndEffectors()[0],
      marker_pose, marker_scale, GripperMarker::NOT_TESTED));
  goal_poses_.insert(GoalPosePair(name, goal_pose));

  // Connect signals
  goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &)));

  // If connected to a database, save all the goals back to the database
  if (constraints_storage_)
  {
    saveGoalsToDB();
  }
}

void MainWindow::saveGoalsToDB()
{
  if (constraints_storage_)
  {
    // Convert all goal pose markers into constraints and store them
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      moveit_msgs::Constraints constraints;
      constraints.name = it->first;

      shape_msgs::SolidPrimitive sp;
      sp.type = sp.BOX;
      sp.dimensions.resize(3, std::numeric_limits<float>::epsilon() * 10.0);

      moveit_msgs::PositionConstraint pc;
      pc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
      pc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link;  // TODO this is hacky i think
      pc.constraint_region.primitives.push_back(sp);
      geometry_msgs::Pose posemsg;
      it->second->getPosition(posemsg.position);
      posemsg.orientation.x = 0.0;
      posemsg.orientation.y = 0.0;
      posemsg.orientation.z = 0.0;
      posemsg.orientation.w = 1.0;
      pc.constraint_region.primitive_poses.push_back(posemsg);
      pc.weight = 1.0;
      constraints.position_constraints.push_back(pc);

      moveit_msgs::OrientationConstraint oc;
      oc.header.frame_id = scene_display_->getRobotModel()->getModelFrame();
      oc.link_name = robot_interaction_->getActiveEndEffectors()[0].parent_link;  // TODO this is hacky i think
      it->second->getOrientation(oc.orientation);
      oc.absolute_x_axis_tolerance = oc.absolute_y_axis_tolerance = oc.absolute_z_axis_tolerance =
          std::numeric_limits<float>::epsilon() * 10.0;
      oc.weight = 1.0;
      constraints.orientation_constraints.push_back(oc);

      try
      {
        constraints_storage_->addConstraints(constraints);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("Cannot save constraint: %s", ex.what());
      }
    }
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::createGoalPoseButtonClicked(void)
{
  std::stringstream ss;

  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if (!ps || robot_interaction_->getActiveEndEffectors().empty())
    {
      if (!ps)
        ROS_ERROR("Not planning scene");
      if (robot_interaction_->getActiveEndEffectors().empty())
        ROS_ERROR("No end effector");
      return;
    }
    else
      ss << ps->getName().c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();
  }

  bool ok = false;
  QString text = QInputDialog::getText(this, tr("Choose a name"), tr("Goal pose name:"), QLineEdit::Normal,
                                       QString(ss.str().c_str()), &ok);

  std::string name;
  if (ok)
  {
    if (!text.isEmpty())
    {
      name = text.toStdString();
      if (goal_poses_.find(name) != goal_poses_.end())
        QMessageBox::warning(this, "Name already exists",
                             QString("The name '").append(name.c_str()).append("' already exists. Not creating goal."));
      else
      {
        // Create the new goal pose at the current eef pose, and attach an interactive marker to it
        scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
        Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(
            robot_interaction_->getActiveEndEffectors()[0].parent_link);
        createGoalAtPose(name, tip_pose);
      }
    }
    else
      QMessageBox::warning(this, "Goal not created", "Cannot use an empty name for a new goal pose.");
  }

  populateGoalPosesList();
}

void MainWindow::showBBoxGoalsDialog()
{
  std::string goals_base_name;
  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if (!ps || robot_interaction_->getActiveEndEffectors().empty())
    {
      if (!ps)
        ROS_ERROR("No planning scene");
      if (robot_interaction_->getActiveEndEffectors().empty())
        ROS_ERROR("No end effector");
      return;
    }
    goals_base_name = ps->getName() + "_pose_";
  }

  bbox_dialog_ui_.base_name_text->setText(QString(goals_base_name.c_str()));

  bbox_dialog_->exec();
}

void MainWindow::createBBoxGoalsButtonClicked(void)
{
  bbox_dialog_->close();

  double minx = bbox_dialog_ui_.center_x_text->text().toDouble() - bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
  double maxx = bbox_dialog_ui_.center_x_text->text().toDouble() + bbox_dialog_ui_.size_x_text->text().toDouble() / 2.0;
  double stepx = (maxx - minx) / std::max<double>(bbox_dialog_ui_.ngoals_x_text->text().toDouble() - 1, 1);
  double miny = bbox_dialog_ui_.center_y_text->text().toDouble() - bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
  double maxy = bbox_dialog_ui_.center_y_text->text().toDouble() + bbox_dialog_ui_.size_y_text->text().toDouble() / 2.0;
  double stepy = (maxy - miny) / std::max<double>(bbox_dialog_ui_.ngoals_y_text->text().toDouble() - 1, 1);
  double minz = bbox_dialog_ui_.center_z_text->text().toDouble() - bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
  double maxz = bbox_dialog_ui_.center_z_text->text().toDouble() + bbox_dialog_ui_.size_z_text->text().toDouble() / 2.0;
  double stepz = (maxz - minz) / std::max<double>(bbox_dialog_ui_.ngoals_z_text->text().toDouble() - 1, 1);

  Eigen::Affine3d goal_pose;
  goal_pose.setIdentity();
  for (std::size_t x = 0; x < bbox_dialog_ui_.ngoals_x_text->text().toShort(); ++x)
    for (std::size_t y = 0; y < bbox_dialog_ui_.ngoals_y_text->text().toShort(); ++y)
      for (std::size_t z = 0; z < bbox_dialog_ui_.ngoals_z_text->text().toShort(); ++z)
      {
        goal_pose(0, 3) = minx + x * stepx;
        goal_pose(1, 3) = miny + y * stepy;
        goal_pose(2, 3) = minz + z * stepz;

        std::stringstream ss;
        ss << bbox_dialog_ui_.base_name_text->text().toStdString() << std::setfill('0') << std::setw(4)
           << goal_poses_.size();

        createGoalAtPose(ss.str(), goal_pose);
      }

  populateGoalPosesList();
}

void MainWindow::removeSelectedGoalsButtonClicked(void)
{
  QList<QListWidgetItem *> found_items = ui_.goal_poses_list->selectedItems();
  for (unsigned int i = 0; i < found_items.size(); i++)
  {
    goal_poses_.erase(found_items[i]->text().toStdString());
  }
  populateGoalPosesList();
}

void MainWindow::removeAllGoalsButtonClicked(void)
{
  goal_poses_.clear();
  goals_initial_pose_.clear();
  populateGoalPosesList();
}

void MainWindow::loadGoalsFromDBButtonClicked(void)
{
  // Get all the constraints from the database, convert to goal pose markers
  if (constraints_storage_ && !robot_interaction_->getActiveEndEffectors().empty())
  {
    // First clear the current list
    removeAllGoalsButtonClicked();

    std::vector<std::string> names;
    try
    {
      constraints_storage_->getKnownConstraints(ui_.load_poses_filter_text->text().toStdString(), names);
    }
    catch (std::runtime_error &ex)
    {
      QMessageBox::warning(this, "Cannot query the database",
                           QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
      return;
    }

    for (unsigned int i = 0; i < names.size(); i++)
    {
      // Create a goal pose marker
      moveit_warehouse::ConstraintsWithMetadata c;
      bool got_constraint = false;
      try
      {
        got_constraint = constraints_storage_->getConstraints(c, names[i]);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("%s", ex.what());
      }
      if (!got_constraint)
        continue;

      if (c->position_constraints.size() > 0 &&
          c->position_constraints[0].constraint_region.primitive_poses.size() > 0 &&
          c->orientation_constraints.size() > 0)
      {
        // Overwrite if exists. TODO: Ask the user before overwriting? copy the existing one with another name before?
        if (goal_poses_.find(c->name) != goal_poses_.end())
        {
          goal_poses_.erase(c->name);
        }
        geometry_msgs::Pose shape_pose;
        shape_pose.position = c->position_constraints[0].constraint_region.primitive_poses[0].position;
        shape_pose.orientation = c->orientation_constraints[0].orientation;

        Eigen::Affine3d shape_pose_eigen;
        tf::poseMsgToEigen(shape_pose, shape_pose_eigen);
        goals_initial_pose_.insert(std::pair<std::string, Eigen::Affine3d>(c->name, shape_pose_eigen));

        tf::poseEigenToMsg(shape_pose_eigen * goal_offset_, shape_pose);

        static const float marker_scale = 0.15;
        GripperMarkerPtr goal_pose(new GripperMarker(
            scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(),
            visualization_manager_, c->name, scene_display_->getRobotModel()->getModelFrame(),
            robot_interaction_->getActiveEndEffectors()[0], shape_pose, marker_scale, GripperMarker::NOT_TESTED, false,
            ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(), ui_.show_z_checkbox->isChecked()));
        // Connect signals
        goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &)));

        goal_poses_.insert(GoalPosePair(c->name, goal_pose));
      }
    }
    populateGoalPosesList();
  }
  else
  {
    if (!constraints_storage_)
      QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::saveGoalsOnDBButtonClicked(void)
{
  saveGoalsToDB();
}

void MainWindow::deleteGoalsOnDBButtonClicked(void)
{
  if (constraints_storage_)
  {
    // Warn the user
    QMessageBox msgBox;
    msgBox.setText("All the selected items will be removed from the database");
    msgBox.setInformativeText("Do you want to continue?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::No);
    int ret = msgBox.exec();

    switch (ret)
    {
      case QMessageBox::Yes:
      {
        // Go through the list of goal poses, and delete those selected
        QList<QListWidgetItem *> found_items = ui_.goal_poses_list->selectedItems();
        for (std::size_t i = 0; i < found_items.size(); i++)
        {
          try
          {
            constraints_storage_->removeConstraints(found_items[i]->text().toStdString());
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("%s", ex.what());
          }
        }
        break;
      }
    }
  }
  removeSelectedGoalsButtonClicked();
}

void MainWindow::loadStatesFromDBButtonClicked(void)
{
  // Get all the start states from the database
  if (robot_state_storage_)
  {
    // First clear the current list
    removeAllStatesButtonClicked();

    std::vector<std::string> names;
    try
    {
      robot_state_storage_->getKnownRobotStates(ui_.load_states_filter_text->text().toStdString(), names);
    }
    catch (std::runtime_error &ex)
    {
      QMessageBox::warning(this, "Cannot query the database",
                           QString("Wrongly formatted regular expression for goal poses: ").append(ex.what()));
      return;
    }

    for (unsigned int i = 0; i < names.size(); i++)
    {
      moveit_warehouse::RobotStateWithMetadata rs;
      bool got_state = false;
      try
      {
        got_state = robot_state_storage_->getRobotState(rs, names[i]);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("%s", ex.what());
      }
      if (!got_state)
        continue;

      // Overwrite if exists.
      if (start_states_.find(names[i]) != start_states_.end())
      {
        start_states_.erase(names[i]);
      }

      // Store the current start state
      start_states_.insert(StartStatePair(names[i], StartStatePtr(new StartState(*rs))));
    }
    populateStartStatesList();
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::saveStatesOnDBButtonClicked(void)
{
  if (robot_state_storage_)
  {
    // Store all start states
    for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
      try
      {
        robot_state_storage_->addRobotState(it->second->state_msg, it->first);
      }
      catch (std::runtime_error &ex)
      {
        ROS_ERROR("Cannot save robot state: %s", ex.what());
      }
  }
  else
  {
    QMessageBox::warning(this, "Warning", "Not connected to a database.");
  }
}

void MainWindow::deleteStatesOnDBButtonClicked(void)
{
  if (robot_state_storage_)
  {
    // Warn the user
    QMessageBox msgBox;
    msgBox.setText("All the selected items will be removed from the database");
    msgBox.setInformativeText("Do you want to continue?");
    msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
    msgBox.setDefaultButton(QMessageBox::No);
    int ret = msgBox.exec();

    switch (ret)
    {
      case QMessageBox::Yes:
      {
        QList<QListWidgetItem *> found_items = ui_.start_states_list->selectedItems();
        for (unsigned int i = 0; i < found_items.size(); ++i)
        {
          try
          {
            robot_state_storage_->removeRobotState(found_items[i]->text().toStdString());
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("%s", ex.what());
          }
        }
        break;
      }
    }
  }
  removeSelectedStatesButtonClicked();
}

void MainWindow::visibleAxisChanged(int state)
{
  if (!robot_interaction_->getActiveEndEffectors().empty())
  {
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      if (it->second->isVisible())
      {
        it->second->setAxisVisibility(ui_.show_x_checkbox->isChecked(), ui_.show_y_checkbox->isChecked(),
                                      ui_.show_z_checkbox->isChecked());
      }
    }
  }
}

void MainWindow::populateGoalPosesList(void)
{
  ui_.goal_poses_list->clear();
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
  {
    QListWidgetItem *item = new QListWidgetItem(QString(it->first.c_str()));
    ui_.goal_poses_list->addItem(item);
    if (!it->second->isVisible())
    {
      item->setBackground(QBrush(Qt::Dense4Pattern));
    }
    else if (it->second->isSelected())
    {
      // If selected, highlight in the list
      item->setSelected(true);
    }
  }
}

void MainWindow::switchGoalVisibilityButtonClicked(void)
{
  QList<QListWidgetItem *> selection = ui_.goal_poses_list->selectedItems();
  for (std::size_t i = 0; i < selection.size(); ++i)
  {
    std::string name = selection[i]->text().toStdString();
    if (goal_poses_[name]->isVisible())
    {
      // Set invisible
      goal_poses_[name]->hide();
      selection[i]->setBackground(QBrush(Qt::Dense4Pattern));
    }
    else
    {
      // Set visible
      goal_poses_[name]->show(scene_display_->getSceneNode(), visualization_manager_);
      selection[i]->setBackground(QBrush(Qt::NoBrush));
    }
  }
}

void MainWindow::goalPoseSelectionChanged(void)
{
  for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
  {
    QListWidgetItem *item = ui_.goal_poses_list->item(i);
    std::string name = item->text().toStdString();
    if (goal_poses_.find(name) != goal_poses_.end() && ((item->isSelected() && !goal_poses_[name]->isSelected()) ||
                                                        (!item->isSelected() && goal_poses_[name]->isSelected())))
      switchGoalPoseMarkerSelection(name);
  }
}

void MainWindow::goalPoseDoubleClicked(QListWidgetItem *item)
{
  JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeGoalPoseDoubleClicked, this, item));
}

void MainWindow::computeGoalPoseDoubleClicked(QListWidgetItem *item)
{
  if (!robot_interaction_ || robot_interaction_->getActiveEndEffectors().empty())
    return;

  std::string item_text = item->text().toStdString();

  // Switch the marker color to processing color while processing
  JobProcessing::addMainLoopJob(
      boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, item_text, GripperMarker::PROCESSING));

  checkIfGoalReachable(item_text, true);
}

/* Receives feedback from the interactive marker attached to a goal pose */
void MainWindow::goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &feedback)
{
  if (feedback.event_type == feedback.BUTTON_CLICK)
  {
    // Unselect all but the clicked one. Needs to be in order, first unselect, then select.
    QListWidgetItem *item = 0;
    for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
    {
      item = ui_.goal_poses_list->item(i);
      if (item->text().toStdString() != feedback.marker_name)
        JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, false));
    }

    for (unsigned int i = 0; i < ui_.goal_poses_list->count(); ++i)
    {
      item = ui_.goal_poses_list->item(i);
      if (item->text().toStdString() == feedback.marker_name)
        JobProcessing::addMainLoopJob(boost::bind(&MainWindow::selectItemJob, this, item, true));
    }
  }
  else if (feedback.event_type == feedback.MOUSE_DOWN)
  {
    // First check to see if this mouse_down is happening on an
    // already-selected goal pose.  A mouse_down event is sent even if
    // the gesture turns out to be nothing but a click.
    bool this_goal_already_selected = false;
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      if (it->second->isSelected() && it->second->isVisible() && it->second->imarker->getName() == feedback.marker_name)
      {
        this_goal_already_selected = true;
      }
    }

    // If this is a mouse-down on an already-selected goal pose, we
    // want to initialize dragging all selected goal poses at the same
    // time.
    if (this_goal_already_selected)
    {
      // Store current poses
      goals_dragging_initial_pose_.clear();
      for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
      {
        if (it->second->isSelected() && it->second->isVisible())
        {
          Eigen::Affine3d pose(
              Eigen::Quaterniond(it->second->imarker->getOrientation().w, it->second->imarker->getOrientation().x,
                                 it->second->imarker->getOrientation().y, it->second->imarker->getOrientation().z));
          pose(0, 3) = it->second->imarker->getPosition().x;
          pose(1, 3) = it->second->imarker->getPosition().y;
          pose(2, 3) = it->second->imarker->getPosition().z;
          goals_dragging_initial_pose_.insert(
              std::pair<std::string, Eigen::Affine3d>(it->second->imarker->getName(), pose));

          if (it->second->imarker->getName() == feedback.marker_name)
            drag_initial_pose_ = pose;
        }
      }
      goal_pose_dragging_ = true;
    }
  }
  else if (feedback.event_type == feedback.POSE_UPDATE && goal_pose_dragging_)
  {
    // Compute displacement from stored pose, and apply to the rest of selected markers
    Eigen::Affine3d current_pose_eigen;
    tf::poseMsgToEigen(feedback.pose, current_pose_eigen);

    Eigen::Affine3d current_wrt_initial = drag_initial_pose_.inverse() * current_pose_eigen;

    // Display the pose in the ui
    Eigen::Vector3d v = current_pose_eigen.linear().eulerAngles(0, 1, 2);
    setStatusFromBackground(STATUS_INFO, QString().sprintf("%.2f %.2f %.2f    %.2f %.2f %.2f", current_pose_eigen(0, 3),
                                                           current_pose_eigen(1, 3), current_pose_eigen(2, 3),
                                                           v(0) * 180.0 / boost::math::constants::pi<double>(),
                                                           v(1) * 180.0 / boost::math::constants::pi<double>(),
                                                           v(2) * 180.0 / boost::math::constants::pi<double>()));

    // Update the rest of selected markers
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      if (it->second->isVisible() && it->second->imarker->getName() != feedback.marker_name && it->second->isSelected())
      {
        visualization_msgs::InteractiveMarkerPose impose;

        Eigen::Affine3d newpose = drag_initial_pose_ * current_wrt_initial * drag_initial_pose_.inverse() *
                                  goals_dragging_initial_pose_[it->second->imarker->getName()];
        tf::poseEigenToMsg(newpose, impose.pose);

        it->second->imarker->setPose(
            Ogre::Vector3(impose.pose.position.x, impose.pose.position.y, impose.pose.position.z),
            Ogre::Quaternion(impose.pose.orientation.w, impose.pose.orientation.x, impose.pose.orientation.y,
                             impose.pose.orientation.z),
            "");
      }
    }
  }
  else if (feedback.event_type == feedback.MOUSE_UP)
  {
    goal_pose_dragging_ = false;
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, feedback.marker_name));
  }
}

void MainWindow::checkGoalsReachable(void)
{
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalReachable, this, it->first, false));
}

void MainWindow::checkGoalsInCollision(void)
{
  for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::checkIfGoalInCollision, this, it->first));
}

void MainWindow::checkIfGoalReachable(const std::string &goal_name, bool update_if_reachable)
{
  if (goal_poses_.find(goal_name) == goal_poses_.end())
    return;

  if (!goal_poses_[goal_name]->isVisible())
    return;

  const boost::shared_ptr<rviz::InteractiveMarker> &imarker = goal_poses_[goal_name]->imarker;

  geometry_msgs::Pose current_pose_msg;
  current_pose_msg.position.x = imarker->getPosition().x;
  current_pose_msg.position.y = imarker->getPosition().y;
  current_pose_msg.position.z = imarker->getPosition().z;
  current_pose_msg.orientation.x = imarker->getOrientation().x;
  current_pose_msg.orientation.y = imarker->getOrientation().y;
  current_pose_msg.orientation.z = imarker->getOrientation().z;
  current_pose_msg.orientation.w = imarker->getOrientation().w;

  // Call to IK
  setStatusFromBackground(STATUS_INFO, "Computing inverse kinematics...");
  robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
  static const int ik_attempts = 5;
  static const float ik_timeout = 0.2;
  bool feasible = robot_interaction_->updateState(ks, robot_interaction_->getActiveEndEffectors()[0], current_pose_msg,
                                                  ik_attempts, ik_timeout);
  if (feasible)
  {
    setStatusFromBackground(STATUS_INFO, "Updating state...");
    if (update_if_reachable)
    {
      scene_display_->getPlanningSceneRW()->setCurrentState(ks);
      scene_display_->queueRenderSceneGeometry();
    }

    setStatusFromBackground(STATUS_INFO, "Updating marker...");
    // Switch the marker color to reachable
    JobProcessing::addMainLoopJob(
        boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::REACHABLE));
  }
  else
  {
    // Switch the marker color to not-reachable
    JobProcessing::addMainLoopJob(
        boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::NOT_REACHABLE));
  }
  setStatusFromBackground(STATUS_INFO, "");
}

bool MainWindow::isGroupCollidingWithWorld(robot_state::RobotState &robot_state, const std::string &group_name)
{
  collision_detection::AllowedCollisionMatrix acm(scene_display_->getPlanningSceneRO()->getAllowedCollisionMatrix());
  // get link names in group_name
  const std::vector<std::string> &group_link_names =
      scene_display_->getRobotModel()->getJointModelGroup(group_name)->getLinkModelNamesWithCollisionGeometry();

  // Create a set of links which is all links minus links in the group.
  const std::vector<std::string> &all_links = scene_display_->getRobotModel()->getLinkModelNames();
  std::set<std::string> link_set(all_links.begin(), all_links.end());
  for (size_t i = 0; i < group_link_names.size(); i++)
  {
    link_set.erase(group_link_names[i]);
  }

  // for each link name in the set,
  for (std::set<std::string>::const_iterator it = link_set.begin(); it != link_set.end(); it++)
  {
    // allow collisions with link.
    acm.setEntry(*it, true);
  }

  // call checkCollision();
  collision_detection::CollisionRequest req;
  req.verbose = false;
  collision_detection::CollisionResult res;
  scene_display_->getPlanningSceneRO()->checkCollision(req, res, robot_state, acm);

  // return result boolean.
  return res.collision;
}

void MainWindow::checkIfGoalInCollision(const std::string &goal_name)
{
  if (goal_poses_.find(goal_name) == goal_poses_.end())
    return;

  // Check if the end-effector is in collision at the current pose
  if (!goal_poses_[goal_name]->isVisible())
    return;

  const robot_interaction::RobotInteraction::EndEffector &eef = robot_interaction_->getActiveEndEffectors()[0];

  const boost::shared_ptr<rviz::InteractiveMarker> &im = goal_poses_[goal_name]->imarker;
  Eigen::Affine3d marker_pose_eigen;
  goal_poses_[goal_name]->getPose(marker_pose_eigen);

  robot_state::RobotState ks(scene_display_->getPlanningSceneRO()->getCurrentState());
  ks.updateStateWithLinkAt(eef.parent_link, marker_pose_eigen);
  bool in_collision = isGroupCollidingWithWorld(ks, eef.eef_group);

  if (in_collision)
  {
    JobProcessing::addMainLoopJob(
        boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::IN_COLLISION));
  }
  else
  {
    JobProcessing::addMainLoopJob(
        boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name, GripperMarker::NOT_TESTED));
  }
}

void MainWindow::switchGoalPoseMarkerSelection(const std::string &marker_name)
{
  if (robot_interaction_->getActiveEndEffectors().empty() || !goal_poses_[marker_name]->isVisible())
    return;

  if (goal_poses_[marker_name]->isSelected())
  {
    // If selected, unselect
    goal_poses_[marker_name]->unselect();
    setItemSelectionInList(marker_name, false, ui_.goal_poses_list);
  }
  else
  {
    // If unselected, select. Only display the gripper mesh for one
    if (ui_.goal_poses_list->selectedItems().size() == 1)
      goal_poses_[marker_name]->select(true);
    else
      goal_poses_[marker_name]->select(false);
    setItemSelectionInList(marker_name, true, ui_.goal_poses_list);
  }
}

void MainWindow::copySelectedGoalPoses(void)
{
  QList<QListWidgetItem *> sel = ui_.goal_poses_list->selectedItems();
  if (sel.empty() || robot_interaction_->getActiveEndEffectors().empty())
    return;

  std::string scene_name;
  {
    const planning_scene_monitor::LockedPlanningSceneRO &ps = scene_display_->getPlanningSceneRO();
    if (!ps)
      return;
    else
      scene_name = ps->getName();
  }

  for (int i = 0; i < sel.size(); ++i)
  {
    std::string name = sel[i]->text().toStdString();
    if (!goal_poses_[name]->isVisible())
      continue;

    std::stringstream ss;
    ss << scene_name.c_str() << "_pose_" << std::setfill('0') << std::setw(4) << goal_poses_.size();

    scene_display_->getPlanningSceneRW()->getCurrentStateNonConst().update();
    Eigen::Affine3d tip_pose = scene_display_->getPlanningSceneRO()->getCurrentState().getGlobalLinkTransform(
        robot_interaction_->getActiveEndEffectors()[0].parent_link);
    geometry_msgs::Pose marker_pose;
    marker_pose.position.x = goal_poses_[name]->imarker->getPosition().x;
    marker_pose.position.y = goal_poses_[name]->imarker->getPosition().y;
    marker_pose.position.z = goal_poses_[name]->imarker->getPosition().z;
    marker_pose.orientation.x = goal_poses_[name]->imarker->getOrientation().x;
    marker_pose.orientation.y = goal_poses_[name]->imarker->getOrientation().y;
    marker_pose.orientation.z = goal_poses_[name]->imarker->getOrientation().z;
    marker_pose.orientation.w = goal_poses_[name]->imarker->getOrientation().w;

    static const float marker_scale = 0.15;
    GripperMarkerPtr goal_pose(new GripperMarker(
        scene_display_->getPlanningSceneRO()->getCurrentState(), scene_display_->getSceneNode(), visualization_manager_,
        ss.str(), scene_display_->getRobotModel()->getModelFrame(), robot_interaction_->getActiveEndEffectors()[0],
        marker_pose, marker_scale, GripperMarker::NOT_TESTED, true));

    goal_poses_.insert(GoalPosePair(ss.str(), goal_pose));

    // Connect signals
    goal_pose->connect(this, SLOT(goalPoseFeedback(visualization_msgs::InteractiveMarkerFeedback &)));

    // Unselect the marker source of the copy
    switchGoalPoseMarkerSelection(name);
  }

  JobProcessing::addMainLoopJob(boost::bind(&MainWindow::populateGoalPosesList, this));
}

void MainWindow::saveStartStateButtonClicked(void)
{
  bool ok = false;

  std::stringstream ss;
  ss << scene_display_->getRobotModel()->getName().c_str() << "_state_" << std::setfill('0') << std::setw(4)
     << start_states_.size();

  QString text = QInputDialog::getText(this, tr("Choose a name"), tr("Start state name:"), QLineEdit::Normal,
                                       QString(ss.str().c_str()), &ok);

  std::string name;
  if (ok)
  {
    if (!text.isEmpty())
    {
      name = text.toStdString();
      if (start_states_.find(name) != start_states_.end())
        QMessageBox::warning(
            this, "Name already exists",
            QString("The name '").append(name.c_str()).append("' already exists. Not creating state."));
      else
      {
        // Store the current start state
        moveit_msgs::RobotState msg;
        robot_state::robotStateToRobotStateMsg(scene_display_->getPlanningSceneRO()->getCurrentState(), msg);
        start_states_.insert(StartStatePair(name, StartStatePtr(new StartState(msg))));

        // Save to the database if connected
        if (robot_state_storage_)
        {
          try
          {
            robot_state_storage_->addRobotState(msg, name);
          }
          catch (std::runtime_error &ex)
          {
            ROS_ERROR("Cannot save robot state on the database: %s", ex.what());
          }
        }
      }
    }
    else
      QMessageBox::warning(this, "Start state not saved", "Cannot use an empty name for a new start state.");
  }
  populateStartStatesList();
}

void MainWindow::removeSelectedStatesButtonClicked(void)
{
  QList<QListWidgetItem *> found_items = ui_.start_states_list->selectedItems();
  for (unsigned int i = 0; i < found_items.size(); i++)
  {
    start_states_.erase(found_items[i]->text().toStdString());
  }
  populateStartStatesList();
}

void MainWindow::removeAllStatesButtonClicked(void)
{
  start_states_.clear();
  populateStartStatesList();
}

void MainWindow::populateStartStatesList(void)
{
  ui_.start_states_list->clear();
  for (StartStateMap::iterator it = start_states_.begin(); it != start_states_.end(); ++it)
  {
    QListWidgetItem *item = new QListWidgetItem(QString(it->first.c_str()));
    ui_.start_states_list->addItem(item);
    if (it->second->selected)
    {
      // If selected, highlight in the list
      item->setSelected(true);
    }
  }
}

void MainWindow::startStateItemDoubleClicked(QListWidgetItem *item)
{
  scene_display_->getPlanningSceneRW()->setCurrentState(start_states_[item->text().toStdString()]->state_msg);
  scene_display_->queueRenderSceneGeometry();
}

void MainWindow::runBenchmark(void)
{
  if (!robot_interaction_ || robot_interaction_->getActiveEndEffectors().size() == 0)
  {
    QMessageBox::warning(this, "Error", "Please make sure a planning group with an end-effector is active");
    return;
  }

  run_benchmark_ui_.benchmark_goal_text->setText(ui_.load_poses_filter_text->text());
  run_benchmark_ui_.benchmark_start_state_text->setText(ui_.load_states_filter_text->text());

  // Load available planners
  boost::shared_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
  std::map<std::string, planning_interface::PlannerManagerPtr> planner_interfaces;
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException &ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }

  if (planner_plugin_loader)
  {
    // load the planning plugins
    const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
    for (std::size_t i = 0; i < classes.size(); ++i)
    {
      ROS_DEBUG("Attempting to load and configure %s", classes[i].c_str());
      try
      {
        planning_interface::PlannerManagerPtr p = planner_plugin_loader->createInstance(classes[i]);
        p->initialize(scene_display_->getPlanningSceneRO()->getRobotModel(), "");
        planner_interfaces[classes[i]] = p;
      }
      catch (pluginlib::PluginlibException &ex)
      {
        ROS_ERROR_STREAM("Exception while loading planner '" << classes[i] << "': " << ex.what());
      }
    }

    // Get the list of planning interfaces and planning algorithms
    if (!planner_interfaces.empty())
    {
      std::stringstream interfaces_ss, algorithms_ss;
      for (std::map<std::string, planning_interface::PlannerManagerPtr>::const_iterator it = planner_interfaces.begin();
           it != planner_interfaces.end(); ++it)
      {
        interfaces_ss << it->first << " ";

        std::vector<std::string> known;
        it->second->getPlanningAlgorithms(known);
        for (std::size_t i = 0; i < known.size(); ++i)
          algorithms_ss << known[i] << " ";
      }
      ROS_DEBUG("Available planner instances: %s. Available algorithms: %s", interfaces_ss.str().c_str(),
                algorithms_ss.str().c_str());
      run_benchmark_ui_.planning_interfaces_text->setText(QString(interfaces_ss.str().c_str()));
      run_benchmark_ui_.planning_algorithms_text->setText(QString(algorithms_ss.str().c_str()));
    }
  }

  run_benchmark_dialog_->show();
}

void MainWindow::benchmarkFolderButtonClicked(void)
{
  QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"), "/home",
                                                  QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
  run_benchmark_ui_.benchmark_output_folder_text->setText(dir);
}

void MainWindow::cancelBenchmarkButtonClicked(void)
{
  run_benchmark_dialog_->close();
}

bool MainWindow::saveBenchmarkConfigButtonClicked(void)
{
  if (run_benchmark_ui_.benchmark_output_folder_text->text().isEmpty())
  {
    QMessageBox::warning(this, "Missing data", "Must specify an output folder");
    return false;
  }

  QString outfilename = run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
  std::ofstream outfile(outfilename.toUtf8());
  if (outfile)
  {
    outfile << "[scene]" << std::endl;
    outfile << "group=" << ui_.planning_group_combo->currentText().toStdString() << std::endl;
    outfile << "default_constrained_link=" << robot_interaction_->getActiveEndEffectors()[0].parent_link << std::endl;
    outfile << "planning_frame=" << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getModelFrame()
            << std::endl;
    outfile << "name=" << scene_display_->getPlanningSceneRO()->getName() << std::endl;

    outfile << "timeout=" << run_benchmark_ui_.timeout_spin->value() << std::endl;
    outfile << "runs=" << run_benchmark_ui_.number_of_runs_spin->value() << std::endl;
    outfile << "output=" << run_benchmark_ui_.benchmark_output_folder_text->text().toStdString() << "/"
            << scene_display_->getPlanningSceneMonitor()->getRobotModel()->getName() << "_"
            << scene_display_->getPlanningSceneRO()->getName() << "_" << ros::Time::now() << std::endl;
    outfile << "start=" << run_benchmark_ui_.benchmark_start_state_text->text().toStdString() << std::endl;
    outfile << "query=" << std::endl;
    outfile << "goal=" << run_benchmark_ui_.benchmark_goal_text->text().toStdString() << std::endl;
    outfile << "goal_offset_roll=" << ui_.goal_offset_roll->value() << std::endl;
    outfile << "goal_offset_pitch=" << ui_.goal_offset_pitch->value() << std::endl;
    outfile << "goal_offset_yaw=" << ui_.goal_offset_yaw->value() << std::endl
            << std::endl;

    outfile << "[plugin]" << std::endl;
    outfile << "name=" << run_benchmark_ui_.planning_interfaces_text->text().toStdString() << std::endl;
    outfile << "planners=" << run_benchmark_ui_.planning_algorithms_text->text().toStdString() << std::endl;

    outfile.close();

    run_benchmark_dialog_->close();
  }
  else
  {
    QMessageBox::warning(this, "Error", QString("Cannot open file ").append(outfilename).append(" for writing"));
    return false;
  }

  return true;
}

void MainWindow::runBenchmarkButtonClicked(void)
{
  if (!saveBenchmarkConfigButtonClicked())
    return;

  QMessageBox msgBox;
  msgBox.setText("This will run the benchmark pipeline. The process will take several minutes during which you will "
                 "not be able to interact with the interface. You can follow the progress in the console output");
  msgBox.setInformativeText("Do you want to continue?");
  msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
  msgBox.setDefaultButton(QMessageBox::Yes);
  int ret = msgBox.exec();

  switch (ret)
  {
    case QMessageBox::Yes:
    {
      QString outfilename = run_benchmark_ui_.benchmark_output_folder_text->text().append("/config.cfg");
      moveit_benchmarks::BenchmarkType btype = 0;
      moveit_benchmarks::BenchmarkExecution be(scene_display_->getPlanningSceneMonitor()->getPlanningScene(),
                                               database_host_, database_port_);
      if (run_benchmark_ui_.benchmark_include_planners_checkbox->isChecked())
        btype += moveit_benchmarks::BENCHMARK_PLANNERS;
      if (run_benchmark_ui_.benchmark_check_reachability_checkbox->isChecked())
        btype += moveit_benchmarks::BENCHMARK_GOAL_EXISTANCE;

      if (be.readOptions(outfilename.toStdString()))
      {
        std::stringstream ss;
        be.printOptions(ss);
        ROS_INFO_STREAM("Calling benchmark with options:" << std::endl
                                                          << ss.str() << std::endl);

        BenchmarkProcessingThread benchmark_thread(be, btype, this);
        benchmark_thread.startAndShow();

        if (benchmark_thread.isRunning())
        {
          benchmark_thread.terminate();
          benchmark_thread.wait();
          QMessageBox::warning(this, "", "Benchmark computation canceled");
        }
        else
        {
          QMessageBox::information(
              this, "Benchmark computation finished",
              QString("The results were logged into '").append(run_benchmark_ui_.benchmark_output_folder_text->text()));
        }
      }

      break;
    }
  }
}

void MainWindow::loadBenchmarkResults(void)
{
  // Select a log file of the set.
  QString path =
      QFileDialog::getOpenFileName(this, tr("Select a log file in the set"), tr(""), tr("Log files (*.log)"));
  if (!path.isEmpty())
  {
    JobProcessing::addBackgroundJob(boost::bind(&MainWindow::computeLoadBenchmarkResults, this, path.toStdString()));
  }
}

void MainWindow::computeLoadBenchmarkResults(const std::string &file)
{
  std::string logid, basename, logid_text;
  try
  {
    logid = file.substr(file.find_last_of(".", file.length() - 5) + 1,
                        file.find_last_of(".") - file.find_last_of(".", file.length() - 5) - 1);
    basename = file.substr(0, file.find_last_of(".", file.length() - 5));
    logid_text = logid.substr(logid.find_first_of("_"), logid.length() - logid.find_first_of("_"));
  }
  catch (...)
  {
    ROS_ERROR("Invalid benchmark log file. Cannot load results.");
    return;
  }

  int count = 1;
  bool more_files = true;
  // Parse all the log files in the set
  while (more_files)
  {
    std::ifstream ifile;
    std::stringstream file_to_load;
    file_to_load << basename << "." << count << logid_text << ".log";

    ifile.open(file_to_load.str().c_str());
    if (ifile.good())
    {
      // Parse results
      char text_line[512];
      static std::streamsize text_line_length = 512;
      bool valid_file = false;
      ifile.getline(text_line, text_line_length);

      // Check if the file is valid. The first line must contain "Experiment <scene>". There must be a "total_time"
      // line, followed by the results
      if (ifile.good() && strstr(text_line, "Experiment") != NULL && strlen(text_line) > 11 &&
          strncmp(&text_line[11], scene_display_->getPlanningSceneRO()->getName().c_str(),
                  scene_display_->getPlanningSceneRO()->getName().length()) == 0)
      {
        while (ifile.good())
        {
          if (strstr(text_line, "total_time REAL") != NULL)
          {
            valid_file = true;
            break;
          }

          ifile.getline(text_line, text_line_length);
        }
      }
      else
      {
        ROS_ERROR("Not a valid log file, or a different planning scene loaded");
      }

      if (valid_file)
      {
        if (basename.find(".trajectory") != std::string::npos)
        {
          // Trajectory results may contain multiple goals
          int nwaypoint = 0;
          while (ifile.getline(text_line, text_line_length) && ifile.good() && strlen(text_line) > 6)
          {
            // This should be a reachability results line composed of reachable, collision-free and time fields (bool;
            // bool; float)
            try
            {
              bool reachable = boost::lexical_cast<bool>(text_line[0]);
              bool collision_free = boost::lexical_cast<bool>(text_line[3]);

              // Update colors accordingly
              if (count <= trajectories_.size())
              {
                std::string trajectory_name = ui_.trajectory_list->item(count - 1)->text().toStdString();
                if (nwaypoint < trajectories_[trajectory_name]->waypoint_markers.size())
                {
                  if (reachable)
                  {
                    if (collision_free)
                    {
                      // Reachable and collision-free
                      JobProcessing::addMainLoopJob(boost::bind(
                          &MainWindow::updateMarkerState, this,
                          trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::REACHABLE));
                    }
                    else
                    {
                      // Reachable, but in collision
                      JobProcessing::addMainLoopJob(boost::bind(
                          &MainWindow::updateMarkerState, this,
                          trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::IN_COLLISION));
                    }
                  }
                  else
                  {
                    // Not reachable
                    JobProcessing::addMainLoopJob(boost::bind(
                        &MainWindow::updateMarkerState, this,
                        trajectories_[trajectory_name]->waypoint_markers[nwaypoint], GripperMarker::NOT_REACHABLE));
                  }
                }
              }
              nwaypoint++;
            }
            catch (...)
            {
              ROS_ERROR("Error parsing the log file");
            }
          }
        }
        else
        {
          ifile.getline(text_line, text_line_length);
          if (ifile.good() && strlen(text_line) > 6)
          {
            // This should be the reachability results line composed of reachable, collision-free and time fields (bool;
            // bool; float)
            try
            {
              bool reachable = boost::lexical_cast<bool>(text_line[0]);
              bool collision_free = boost::lexical_cast<bool>(text_line[3]);

              // Update colors accordingly
              if (count <= goal_poses_.size())
              {
                std::string goal_name = ui_.goal_poses_list->item(count - 1)->text().toStdString();
                if (reachable)
                {
                  if (collision_free)
                  {
                    // Reachable and collision-free
                    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this,
                                                              goal_name, GripperMarker::REACHABLE));
                  }
                  else
                  {
                    // Reachable, but in collision
                    JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this,
                                                              goal_name, GripperMarker::IN_COLLISION));
                  }
                }
                else
                {
                  // Not reachable
                  JobProcessing::addMainLoopJob(boost::bind(&MainWindow::updateGoalMarkerStateFromName, this, goal_name,
                                                            GripperMarker::NOT_REACHABLE));
                }
              }
            }
            catch (...)
            {
              ROS_ERROR("Error parsing the log file");
            }
          }
        }
      }
      else
      {
        ROS_ERROR("Invalid benchmark log file. Cannot load results.");
      }

      ifile.close();
    }
    else
    {
      more_files = false;
    }
    count++;
  }
}

void MainWindow::updateGoalMarkerStateFromName(const std::string &name, const GripperMarker::GripperMarkerState &state)
{
  if (goal_poses_.find(name) != goal_poses_.end())
    goal_poses_[name]->setState(state);
}

void MainWindow::updateMarkerState(GripperMarkerPtr marker, const GripperMarker::GripperMarkerState &state)
{
  marker->setState(state);
}

void MainWindow::goalOffsetChanged()
{
  goal_offset_.setIdentity();
  goal_offset_ = Eigen::AngleAxisd(ui_.goal_offset_roll->value() * boost::math::constants::pi<double>() / 180.0,
                                   Eigen::Vector3d::UnitX()) *
                 Eigen::AngleAxisd(ui_.goal_offset_pitch->value() * boost::math::constants::pi<double>() / 180.0,
                                   Eigen::Vector3d::UnitY()) *
                 Eigen::AngleAxisd(ui_.goal_offset_yaw->value() * boost::math::constants::pi<double>() / 180.0,
                                   Eigen::Vector3d::UnitZ());

  if (!robot_interaction_->getActiveEndEffectors().empty())
  {
    Eigen::Affine3d current_pose;
    for (GoalPoseMap::iterator it = goal_poses_.begin(); it != goal_poses_.end(); ++it)
    {
      current_pose = goals_initial_pose_[it->first] * goal_offset_;
      it->second->setPose(current_pose);
    }
  }
}

}  // namespace

max_stars_repo_name: KenmeiFusamae/moveit

max_issues_repo_name: KenmeiFusamae/moveit

max_forks_repo_name: KenmeiFusamae/moveit


text:
#include <chrono>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <boost/format.hpp>
#include "bloom_filter.h"
#include "/home/vagrant/nfllib/include/nfl.hpp"
#include "/home/vagrant/nfllib/include/nfl/params.hpp"

//#define memset_s(W,WL,V,OL) memset(W,V,OL)
//#include "/Users/mkilliji/Code/keccak-tiny/keccak-tiny.c"

#define CONFIG 512, 62, uint64_t

template <class P>
__attribute__((noinline)) static void encrypt(P& a, P& b, P const & message, uint64_t pt_modulus, P const & pka, P const & pkb, P const & pkaprime, P const & pkbprime, FastGaussianNoise<uint8_t, typename P::value_type, 2> *g_prng)
{
    // u
    P e1 = nfl::gaussian<typename P::value_type>(g_prng);
    e1.ntt_pow_phi();

    // p*e_1
    P e2 = nfl::gaussian<typename P::value_type>(g_prng, pt_modulus);
    e2.ntt_pow_phi();

    // p*e_2
    P e3 = nfl::gaussian<typename P::value_type>(g_prng, pt_modulus);
    e3 = e3 + message;
    e3.ntt_pow_phi();

    // encCset_a = pka * u + p*e_2
    a = e1 * pka + e2 ;

    // encCset_b = pkb * u + p*e_3 + message
    b = e1 * pkb + e3;
}

template <class P>
__attribute__((noinline)) static void decrypt(P& tmp, P const & a, P const& b, P const& s, P const& sprime, typename P::value_type const modulus, uint64_t const pt_modulus)
{
    using fuck = typename P::signed_value_type;
    tmp = b -  nfl::shoup(a * s, sprime);

    tmp.invntt_pow_invphi();

    uint64_t const adder=(uint64_t)(((1ULL<<63)/pt_modulus)*pt_modulus-modulus);

    for(auto & v : tmp)
    {
        v= (v<modulus/2) ? v%pt_modulus : (v + adder)%pt_modulus;
    }

}

template <class P, class Q>
static void plonge(P& out, Q& in) {
    for(int j=0;j<out.nmoduli;j++) {
        for(int i=0;i<out.degree;i++) {
            out(j,i)=in(j,i);
        }
    }
}

template <class P>
static void encode(P& res, unsigned char const bits[],  int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    for(int j=0;j<tmp.nmoduli;j++) {
        for(int i=0;i<tmp.degree&&i<size;i++) {
            tmp(j,i)=bits[i];
        }
    }
    tmp.invntt_pow_invphi();
    plonge(res,tmp);
}

template <class P>
static void decode(unsigned char *& clear, P const res, int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    plonge(tmp,res);
    tmp.ntt_pow_phi();
    clear=(unsigned char *)malloc(size*sizeof(int));
    for(int i=0;i<res.degree&&i<size;i++) {
        clear[i]=tmp(0,i);
    }
}

template <class P>
static void decode_noalloc( char  * clear, P const res, int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    plonge(tmp,res);
    tmp.ntt_pow_phi();
    for(int i=0;i<res.degree&&i<size;i++) {
        clear[i]=tmp(0,i);
    }
}

template <class P>
static void encode_nontt(P& res, uint8_t* const bits,  int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    for(int j=0;j<tmp.nmoduli;j++) {
        for(int i=0;i<tmp.degree&&i<size;i++) {
            tmp(j,i)=bits[i];
        }
    }
    plonge(res,tmp);
}

uint64_t cleartext_modulus() {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    return poly_cleartext::get_modulus(0);
}

template <class T>
double get_time_us(T const& start, T const& end, uint32_t N)
{
    auto diff = end-start;
    return (long double)(std::chrono::duration_cast<std::chrono::microseconds>(diff).count())/N;
}

template <class T, size_t Align, class... Args>
T* alloc_aligned(size_t n, Args&& ... args)
{
    T* ret;
    if (posix_memalign((void**) &ret, Align, sizeof(T)*n) != 0) {
        throw std::bad_alloc();
    }
    for (size_t i = 0; i < n; i++) {
        new (&ret[i]) T{std::forward<Args>(args)...};
    }
    return ret;
}

template<size_t degree, size_t modulus, class T>
int testencode_decode () {
    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    uint8_t in[degree];
    fastrandombytes(in,degree);
    for(int i=0;i<degree;i++) {
        in[i]=(in[i])%2;
    }

    uint8_t  *out;
    poly_cyphertext c;
    encode(c,in,degree);
    decode(out, c,degree);
    for(int i=0;i<degree;i++) {
        assert(in[i] == out[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "Encode/Decode test passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testencrypt_decrypt () {
    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;
    poly_cyphertext &pka = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkaprime = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkb = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkbprime = *alloc_aligned<poly_cyphertext, 32>(1),
    &a = *alloc_aligned<poly_cyphertext, 32>(1),
    &b = *alloc_aligned<poly_cyphertext, 32>(1),
    &d = *alloc_aligned<poly_cyphertext, 32>(1);

    // This step generates a secret key
    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);

    poly_cyphertext &s = *alloc_aligned<poly_cyphertext, 32>(1, nfl::gaussian<T>(&g_prng));
    poly_cyphertext &sprime = *alloc_aligned<poly_cyphertext, 32>(1);
    s.ntt_pow_phi();
    sprime = nfl::compute_shoup(s);

    // This step generates a public key
    pka = nfl::uniform();
    pkb = nfl::gaussian<T>(&g_prng, cleartext_modulus());
    pkb.ntt_pow_phi();

    // pkb = pkb + pka * s;
    pkb = pkb + nfl::shoup(pka * s, sprime);
    pkaprime = nfl::compute_shoup(pka);
    pkbprime = nfl::compute_shoup(pkb);

    uint8_t in[degree];
    fastrandombytes(in,degree);
    for(int i=0;i<degree;i++) {
        in[i]=(in[i])%2;
    }

    uint8_t  *out;
    poly_cyphertext c;
    encode(c,in,degree);

    encrypt(a, b, c, cleartext_modulus(), pka, pkb, pkaprime, pkbprime, &g_prng);
    decrypt(d, a, b, s, sprime, d.get_modulus(0),cleartext_modulus());

    decode(out, d,degree);
    for(int i=0;i<degree;i++) {
        assert(in[i] == out[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "Encrypt/Decrypt test passed" << std::endl;

}

// The PSI protocol is simple
// Remember it is an asymetric protocol where C learns the intersection of its set with S's set
// The protocol isn't secure against malicious participants, i.e. if C's set = Universe then Intersection(Universe,S's set)=S's set
// --> C's can learn S's set
// The protocol is secure with honest but curious (semi-honest) participants
// C calls initiateC with its set
// sends the resulting char buffer to S (which includes C's public key)
// S calls initiateS with its set and the received buffer
// sends back the resulting char buffer to C
// C calls terminateC with the received buffer
// and computes the intersection
// As classical with PSI, it is an asymetric protocol, i.e. C learns the intersection but not S
// if you want it to be symetrical, just do it also the other way around, i.e. C becomes S and vice versa (see testSymetricPSIAsLib as an example)

// initiateC is called by C to launch the protocol
template<size_t degree, size_t modulus, class T>
void initiateC(char*& outbuf,char* const inbuf,uint64_t  &outlen, uint64_t const inlen, nfl::poly_from_modulus<T, degree, modulus> *&SecretKey){

    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    // If size doesn't divide degree, we have to increase the inbuf size
    // and then recursively call initiateC (next time the buffer size is going to divide degree and then no recursion)
    if((inlen%degree!=0)) {
        char *new_inbuf=(char*)malloc(nbP*degree);
        memcpy(new_inbuf,inbuf,inlen);
        initiateC<degree,modulus,T>(outbuf,new_inbuf,outlen,nbP*degree,SecretKey);
    } else {

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // the input is in inbuf, has length inlen

    // Allocate a large enough buffer for :
    // the public keys (and their shoupified versions) and the two (times nbP) ciphers a and b
    outlen = (nbP*2+4)*degree*sizeof(T);
    outbuf=(char*)malloc(outlen);

    poly_cyphertext *pka = (poly_cyphertext*)outbuf,
                    *pkaprime = (poly_cyphertext*)(outbuf+degree*sizeof(T)),
                    *pkb = (poly_cyphertext*)(outbuf+2*degree*sizeof(T)),
                    *pkbprime = (poly_cyphertext*)(outbuf+3*degree*sizeof(T));

    // This step generates a secret key
    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);
    SecretKey = alloc_aligned<poly_cyphertext, 32>(1, nfl::gaussian<T>(&g_prng));
    SecretKey->ntt_pow_phi();

    // This step generates a public key
    // pka is just uniform
    *pka = nfl::uniform();
    *pkb = nfl::gaussian<T>(&g_prng, cleartext_modulus());
    pkb->ntt_pow_phi();
    // pkb = pkb + pka * s;
    *pkb = *pkb + *pka * *SecretKey;
    *pkaprime = nfl::compute_shoup(*pka);
    *pkbprime = nfl::compute_shoup(*pkb);

    // Then alloc a single polynom for encoding and encrypting in the loop
    poly_cyphertext p_in=*alloc_aligned<poly_cyphertext, 32>(1);

    // Then do the encoding and encrypting in the loop
    for(int i=0;i<nbP;i++) {
        encode(p_in,(unsigned char*)(inbuf+i*degree),degree);
        // encrypt (a[i]),b[i],p_in,...) but we are doing this directly in the output buffer
        encrypt(*((poly_cyphertext*)(outbuf+(4+i)*degree*sizeof(T))), *((poly_cyphertext*)(outbuf+(nbP+4+i)*degree*sizeof(T))), p_in, cleartext_modulus(), *pka, *pkb, *pkaprime, *pkbprime, &g_prng);
    }}
}

// initiateS is called by S upon receiving a request by C, that's the second step of the protocol
template<size_t degree, size_t modulus, class T>
void initiateS(char*& outbuf,char* const inbuf, char* Sinput, uint64_t &outlen, uint64_t const inlen){
    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // C sent its public keys in the input buffer (pka,pkb,pkaprime,pkbprime)
    poly_cyphertext *pka = (poly_cyphertext*)inbuf,
                    *pkaprime = (poly_cyphertext*)(inbuf+degree*sizeof(T)),
                    *pkb = (poly_cyphertext*)(inbuf+2*degree*sizeof(T)),
                    *pkbprime = (poly_cyphertext*)(inbuf+3*degree*sizeof(T)),
                    // and we are going to produce a cipher of a zero polynom
                    &p_zero = *alloc_aligned<poly_cyphertext, 32>(1),
                    &zeroa = *alloc_aligned<poly_cyphertext, 32>(1),
                    &zerob = *alloc_aligned<poly_cyphertext, 32>(1);

    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);

    // We are going to encode S input set into p_inS
    poly_cyphertext p_inS=*alloc_aligned<poly_cyphertext, 32>(1);

    // The output buffer and its length
    outlen = (nbP*2)*degree*sizeof(T);
    outbuf=(char*)malloc(outlen);

    // A zero polynom that is going to be ciphered in the loop.
    // Please note that it could be sent by C and S would not need C's public key
    // But it is more secure to produce a new zero polynom for each addition
    // Henceforth by enabling S to produce them (by giving it S public key)
    // We save on communication, but not on computation
    char zero[degree];
    for(int i=0;i<degree;i++) {
        zero[i]=0;
    }
    encode(p_zero,(unsigned char*)zero,degree);

    for(int i=0;i<nbP;i++) {
        // S's set is encoded into p_inS and NTTified before mult
        encode(p_inS,(unsigned char*)(Sinput+i*degree),degree);
        p_inS.ntt_pow_phi();
        // We produce a new cipher of zero
        encrypt(zeroa, zerob, p_zero, cleartext_modulus(),*pka, *pkb, *pkaprime, *pkbprime, &g_prng);
        // and compute the intersection and add zero
        // a = a *p_ins + zero and b =b * p_ins +zero
        *((poly_cyphertext*)(outbuf+i*degree*sizeof(T))) = *((poly_cyphertext*)(inbuf+(4+i)*degree*sizeof(T))) * p_inS + zeroa;
        *((poly_cyphertext*)(outbuf+(i+nbP)*degree*sizeof(T))) = *((poly_cyphertext*)(inbuf+(4+i+nbP)*degree*sizeof(T))) * p_inS + zerob;
    }
}

// terminateC is called by C upon receiving a reply by S, that's the third and final step of the protocol
template<size_t degree, size_t modulus, class T>
void terminateC(char* const outbuf,char* const inbuf,uint64_t outlen, uint64_t const inlen, nfl::poly_from_modulus<T, degree, modulus> *SecretKey){
    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // Sprime is the shoupified secret key
    poly_cyphertext &sprime = *alloc_aligned<poly_cyphertext, 32>(1);
    sprime = nfl::compute_shoup(*SecretKey);

    // d is a polynom into which we are going to decrypt in the loop
    poly_cyphertext &d=*alloc_aligned<poly_cyphertext, 32>(1);

    // TODO: Do we really need to compute this apart from stats ?
    outlen=nbP*degree*sizeof(T);

    for(int i=0;i<nbP;i++) {
        // We decrypt S's reply into d (decrypt(d,a,b,...))
        decrypt(d, *(poly_cyphertext*)(inbuf+i*degree*sizeof(T)), *(poly_cyphertext*)(inbuf+(nbP+i)*degree*sizeof(T)), *SecretKey, sprime, d.get_modulus(0),cleartext_modulus());
        // And decode d into outbuf
        //  the length of the decode depends on whether or not the size of the universe fits into degree's polynoms
        if(i==nbP-1&&inlen%degree!=0)
            decode_noalloc((outbuf+i*degree*sizeof(char)), d ,inlen%degree);
        else
            decode_noalloc((outbuf+i*degree*sizeof(char)), d ,degree);
    }

}

static char* getRandomBinaryBytes(uint64_t size) {
    unsigned char *buff=(unsigned char*)malloc(size);
    fastrandombytes(buff,size);
    for(int i=0;i<size;i++) {
        buff[i]=(buff[i])%2;
    }
    return (char*)buff;
}

template<size_t degree, size_t modulus, class T>
int testPSI1(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    uint8_t Sset[size];
    for(int i=0;i<size;i++) {
        Sset[i]=1;
    }

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==Cset[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI test1 passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSI0(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);

    uint8_t Sset[size];
    for(int i=0;i<size;i++) {
        Sset[i]=0;
    }

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==0);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI test0 passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSIAND(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==(Cset[i]&&Sset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI testAND passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size];

    start = std::chrono::steady_clock::now();

    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==(Cset[i]&&Sset[i]));
    }
    auto etime=get_time_us(start, end, 1);
    uint64_t bps=1000000*size/etime;
    uint64_t xfact=(Coutlen+Soutlen);
    xfact=(uint64_t)((float)xfact/(float)size);
    std::cout << "PSI : n=" << size << " \tin " << etime << " us \t\t" <<
                  bps << "bit/sec "<<
                  " \tand " << Coutlen + Soutlen << " bytes communication"<<
                  " \t expension factor = "<< xfact <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int testSymetricPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyAlice;
    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyBob;

    char *Aliceset=getRandomBinaryBytes(size);
    char *Bobset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char IntersectionAlice[size];
    char IntersectionBob[size];

    start = std::chrono::steady_clock::now();
    // Alice -> Bob
    initiateC<degree,modulus,T>(Cout,(char*)Aliceset,Coutlen,size,SecretKeyAlice);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Bobset,Soutlen,size);
    terminateC<degree,modulus,T>(IntersectionAlice,Sout,Soutlen,size,SecretKeyAlice);
    // Bob -> Alice
    initiateC<degree,modulus,T>(Cout,(char*)Bobset,Coutlen,size,SecretKeyBob);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Aliceset,Soutlen,size);
    terminateC<degree,modulus,T>(IntersectionBob,Sout,Soutlen,size,SecretKeyBob);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(IntersectionBob[i]==(Aliceset[i]&&Bobset[i]));
        assert(IntersectionAlice[i]==(Aliceset[i]&&Bobset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "SPSI : n=" << size << " \tin " << get_time_us(start, end, 1) << " us \t\t" <<
                  1000000*size/get_time_us(start, end, 1) << "bit/sec "<<
                  " \tand " << (Coutlen + Soutlen)*2 << " bytes communication"<<
                  " \t expension factor = "<< (Coutlen+Soutlen)*2/size <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int test3PartiesPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyAlice;

    char *Aliceset=getRandomBinaryBytes(size);
    char *Bobset=getRandomBinaryBytes(size);
    char *Charlieset=getRandomBinaryBytes(size);

    char *Aout;
    uint64_t Aoutlen;
    char *Bout;
    uint64_t Boutlen;
    char *Cout;
    uint64_t Coutlen;
    char IntersectionAlice[size];

    start = std::chrono::steady_clock::now();
    initiateC<degree,modulus,T>(Aout,(char*)Aliceset,Aoutlen,size,SecretKeyAlice);
    initiateS<degree,modulus,T>(Bout,Aout,(char*)Bobset,Boutlen,size);
    // terminateC<degree,modulus,T>(IntersectionAlice,Bout,Boutlen,size,SecretKeyAlice);
    // initiateC<degree,modulus,T>(Aout,IntersectionAlice,Aoutlen,size,SecretKeyAlice);
    //initiateS<degree,modulus,T>(Cout,Aout,(char*)Charlieset,Aoutlen,size);
    initiateS<degree,modulus,T>(Cout,Bout,(char*)Charlieset,Coutlen,size);
    terminateC<degree,modulus,T>(IntersectionAlice,Cout,Coutlen,size,SecretKeyAlice);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(IntersectionAlice[i]==(Aliceset[i]&&Bobset[i]&&Charlieset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "3PSI : n=" << size << " \tin " << get_time_us(start, end, 1) << " us \t\t" <<
                  1000000*size/get_time_us(start, end, 1) << "bit/sec "<<
                  " \tand " << (Aoutlen + Boutlen + Coutlen) << " bytes communication"<<
                  " \t expension factor = "<< (Aoutlen + Boutlen + Coutlen)/size <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPerfPSI(uint64_t size,uint64_t reps)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();
    uint64_t t1 = 0;
    uint64_t t2 = 0;
    uint64_t t3 = 0;

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size];
    for(int i=0;i<reps;i++) {
    start = std::chrono::steady_clock::now();
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();
    t1=t1+(get_time_us(start, end, 1));
    start = std::chrono::steady_clock::now();
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
    end = std::chrono::steady_clock::now();
    t2=t2+(get_time_us(start, end, 1));
    start = std::chrono::steady_clock::now();
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();
    t3=t3+(get_time_us(start, end, 1));
}

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI : n=" << size << " in " << (t1+t2+t3)/reps << " us " << (t1+t2+t3)/reps/size << "us/bit"<< std::endl;
    std::cout << "PSI : initC " << size << " in " << (t1)/reps << " us " << (100*t1/(t1+t2+t3)) <<"%"<< std::endl;
    std::cout << "PSI : initS " << size << " in " << (t2)/reps << " us " << (100*t2/(t1+t2+t3)) <<"%"<< std::endl;
    std::cout << "PSI : finaC " << size << " in " << (t3)/reps << " us " << (100*t3/(t1+t2+t3)) <<"%"<< std::endl;

}

void testBBF(size_t size) {
    unsigned char a[]="marco";size_t asize=5;
    unsigned char b[]="carlos";size_t bsize=6;
    unsigned char c[]="joris";size_t csize=5;
    BloomFilter BF(512,4);
    BF.insert(a,asize);
    BF.insert(b,bsize);
    bool res=BF.test(a,asize);
    assert(res==true);
    res=BF.test(b,bsize);
    assert(res==true);
    res=BF.test(c,csize);
    assert(res==false);
}

template<size_t degree, size_t modulus, class T>
void testPSIBF(size_t size) {
    /* L'idée est de :
      C crée un BF et y met son set
      C encode ce BF de 512 bytes vers un bitset de 512*8 bits
      puis intiateC et envoie à S
      S encode son BF contenant son set vers un bitset de 512*8 bits
      puis initiateS et envoie à C
      C terminateC dans un newBF
      pour chacun de ses éléments fait un newBF.test
    */

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
    unsigned char a[]="marco";size_t asize=5;
    unsigned char b[]="carlos";size_t bsize=6;
    unsigned char c[]="joris";size_t csize=5;

    BloomFilter BF(size,4);
    BF.insert(a,asize);
    BF.insert(b,bsize);
    BF.insert(c,bsize);
    char* serC=BF.serialize();

    BloomFilter BFS(size,4);
    BFS.insert(a,asize);
    BFS.insert(b,bsize);
    char* serS=BFS.serialize();

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size*8];

    initiateC<degree,modulus,T>(Cout,serC,Coutlen,size*8,SecretKey);
    initiateS<degree,modulus,T>(Sout,Cout,serS,Soutlen,size*8);
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size*8,SecretKey);

    BloomFilter BFInter(size,4);

    BFInter.deserialize(Intersection);

    bool res=BFInter.test(a,asize);
    assert(res==true);
    res=BFInter.test(b,bsize);
    assert(res==true);
    res=BFInter.test(c,csize);
    assert(res==false);
}

// template<size_t degree, size_t modulus, class T>
// int testPerfPSIBloom(uint64_t size,uint64_t reps)
// {
//
//     auto start = std::chrono::steady_clock::now();
//     auto end = std::chrono::steady_clock::now();
//  uint64_t t1 = 0;
//  uint64_t t2 = 0;
//  uint64_t t3 = 0;
//
//  nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
//
//  BloomFilter Cset(512,4);
//  char *Sset=getRandomBinaryBytes(size);
//
//  char *Cout;
//  uint64_t Coutlen;
//  char *Sout;
//  uint64_t Soutlen;
//  char Intersection[size];
//  for(int i=0;i<reps;i++) {
//     start = std::chrono::steady_clock::now();
//  initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
//     end = std::chrono::steady_clock::now();
//  t1=t1+(get_time_us(start, end, 1));
//     start = std::chrono::steady_clock::now();
//  initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
//     end = std::chrono::steady_clock::now();
//  t2=t2+(get_time_us(start, end, 1));
//     start = std::chrono::steady_clock::now();
//  terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
//     end = std::chrono::steady_clock::now();
//  t3=t3+(get_time_us(start, end, 1));
// }
//
//  std::cout << "======================================================================" << std::endl;
//  std::cout << "PSI : n=" << size << " in " << (t1+t2+t3)/reps << " us " << std::endl;
//  std::cout << "PSI : initC " << size << " in " << (t1)/reps << " us " << (100*t1/(t1+t2+t3)) <<"%"<< std::endl;
//  std::cout << "PSI : initS " << size << " in " << (t2)/reps << " us " << (100*t2/(t1+t2+t3)) <<"%"<< std::endl;
//  std::cout << "PSI : finaC " << size << " in " << (t3)/reps << " us " << (100*t3/(t1+t2+t3)) <<"%"<< std::endl;
//
//
// }

template<size_t degree, size_t modulus, class T>
int run()
{

    // testencode_decode<degree,modulus,T>();
    // testencrypt_decrypt<degree,modulus,T>();
    // testPSI1<degree,modulus,T>(1024);
    // testPSI0<degree,modulus,T>(1024);
    // testPSIAND<degree,modulus,T>(1024);
    // for(int i=512;i<2*512;i+=1)
    //   testPSIAsLib<degree,modulus,T>(i);
    // testPSIAsLib<degree,modulus,T>(2*512);
    // testPSIAsLib<degree,modulus,T>(10*512);
    // testPSIAsLib<degree,modulus,T>(100*512);
    // testPSIAsLib<degree,modulus,T>(1000*512);
    // testPSIAsLib<degree,modulus,T>(2000*512);
    // testPSIAsLib<degree,modulus,T>(2*2000*512);
    //testPSIAsLib<degree,modulus,T>(3*2000*512);
    //testPSIAsLib<degree,modulus,T>(512*512);
    //testSymetricPSIAsLib<degree,modulus,T>(2000*512);
    testPerfPSI<degree,modulus,T>(100000,10);
    testPerfPSI<degree,modulus,T>(10000,10);
    testPerfPSI<degree,modulus,T>(1000,100);
    testPerfPSI<degree,modulus,T>(100,100);
    //test3PartiesPSIAsLib<degree,modulus,T>(512*512);

    // testHash<degree,modulus,T>(512,512);
    // testHash<degree,modulus,T>(1024,512);
    // testHash<degree,modulus,T>(2048,512);
    // testHash<degree,modulus,T>(4096,512);

    testBBF(512);
    std::cout<<"---"<<std::endl;
    testPSIBF<degree,modulus,T>(512);

    return 0;
}

int main() {
    return run<CONFIG>();
}

ext: cpp


text:
#include <boost/filesystem.hpp>
#include <boost/range/istream_range.hpp>
#include <iostream>

namespace fs = boost::filesystem;

int
main(int argc, char** argv)
{
  if (argc < 1)
  {
    std::cout << "need path" << std::endl;
    return 1;
  }

  for (int i = 1; i < argc; i++)
  {
    fs::path p    = argv[i];
    auto     rstr = p.generic_string();
    auto     rlen = rstr.length();
    auto     dir  = boost::make_iterator_range(fs::recursive_directory_iterator(p), {});
    for (const auto& e : dir)
    {
      if (!fs::is_directory(e))
      {
        auto path = e.path();
        auto pstr = path.generic_string();
        auto len  = pstr[rlen] == '/' ? rlen + 1 : rlen;
        auto rels = pstr.substr(len);
        std::cout << path << ":\n"
                  << "\tREL:\t" << path.relative_path() << "\n"
                  << "\tREL2:\t" << rels << "\n"
                  << "\tFILE:\t" << path.filename() << "\n"
                  << "\tROOT:\t" << path.root_path() << "\n"
                  << "\tROOT:\t" << path.root_directory() << "\n"
                  << "\tPAR:\t" << path.parent_path() << "\n"
                  << "\tRN:\t" << path.root_name() << "\n"
                  << std::endl;
      }
    }
  }

  return 0;
}


text:
// #include <boost/bind.hpp>
#include <boost/bind/bind.hpp>
#include <iostream>
#include <algorithm>

using namespace std;

int f(int a, int b)
{
    return a + b;
}

int g(int a, int b, int c)
{
    return a + b + c;
}

int h(int &a, int &b)
{
    return a++ + b++;
}

void print(int x)
{
    cout << x << endl;
}

struct F2
{
    int s;

    typedef void result_type;
    void operator()(int x) { s += x; }
};

class A
{
public:
    int m_a = 0;
    void inc(int x)
    {
        m_a += x;
    }
};

int main()
{
    int a = 10, b = 20;
    print(boost::bind(f, 10, 20)());                                                 // 30
    print(boost::bind(f, boost::placeholders::_1, boost::placeholders::_1)(10, 20)); // 20
    print(boost::bind(h, a, boost::placeholders::_2)(a, b));                         // 30
    print(a);                                                                        // 10
    print(b);                                                                        // 21
    // values are stored into the function object by copy of value, use boost::ref or boost::cref to store values by reference
    print(boost::bind(h, boost::ref(a), boost::placeholders::_2)(a, b)); // 31
    print(a);                                                            // 11
    print(b);                                                            // 22

    F2 f2 = {0};
    int ar[] = {1, 2, 3};
    // By default, bind makes a copy of the provided function object
    std::for_each(ar, ar + 3, boost::bind(f2, boost::placeholders::_1));
    print(f2.s); // 0
    std::for_each(ar, ar + 3, boost::bind(boost::ref(f2), boost::placeholders::_1));
    print(f2.s); // 6

    A ao;
    boost::bind(&A::inc, ao, boost::placeholders::_1)(100);
    print(ao.m_a); // 0
    boost::bind(&A::inc, boost::ref(ao), boost::placeholders::_1)(100);
    print(ao.m_a); // 100
    return 0;
}

max_stars_repo_name: lolyu/aoi

max_issues_repo_name: lolyu/aoi

max_forks_repo_name: lolyu/aoi


text:
/* A simple server that generates keys for the different users */
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <openssl/bn.h>

#include <boost/algorithm/string.hpp>

#include "../lib/utils.h"
#include "../lib/kms.h"

int main(void)
{
    int key_size = SECURITY_KEYSIZE;
    int string_size = (key_size / 4 ) + 2;
    int sockfd, newsockfd, portno;
    socklen_t clilen;
    char buffer[string_size];
    struct sockaddr_in serv_addr, cli_addr;
    int n;

    KMS * kms = new KMS();
    kms->InitKMS(key_size);
    // We add a user by default
    kms->addUser();

    std::pair<BIGNUM *, BIGNUM *> keys = kms->keyGenKMS();

    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0)
        error("ERROR opening socket");

    bzero((char *) &serv_addr, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
    serv_addr.sin_port = htons(PORT_NUMBER);

    if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
          error("ERROR on binding");

    char * aux_component;
    while(1)
    {
        listen(sockfd, 5);
        clilen = sizeof(cli_addr);
        newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
        if (newsockfd < 0)
            error("ERROR on accept");
        bzero(buffer,(SECURITY_KEYSIZE/4)+1);
        n = read(newsockfd,buffer,(SECURITY_KEYSIZE/4)+1);
        if (n < 0) error("ERROR reading from socket");

        std::string * msg = new std::string(buffer);
        if (msg->compare("primeQ") == 0)
        {
            aux_component = BN_bn2hex(kms->getPrimeQ());
            //printf("Here is the primeQ: %s\n",aux_component);
            n = write(newsockfd,aux_component, string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }
		else if(msg->compare("addUser") == 0)
        {
			unsigned int new_id = kms->addUser();

            const char * aux_c = std::to_string(new_id).c_str();
			aux_component = (char *) malloc(sizeof(char) * (strlen(aux_c) + 1));
			aux_component = strcpy(aux_component, aux_c);
            //printf("New User Id: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }
        /*else if(msg->compare("masterK") == 0)
        {
            aux_component = BN_bn2hex(keys.first);
            printf("Here is the MasterK: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");
        }*/
        else if(msg->compare(0, 5, "userK") == 0)
        {
            /*aux_component = BN_bn2hex(keys.first);
            printf("Here is the UserK: %s\n",aux_component);
            n = write(newsockfd,aux_component,string_size+1);
            if (n < 0) error("ERROR writing to socket");*/
            std::vector<std::string> parts2;
			boost::split(parts2, *msg, boost::is_any_of(":"));
			if (parts2.size() == 2)
			{
				aux_component = kms->getServerKey(std::stoul (parts2[1],nullptr,0));
				//printf("Here is the UserK: %s\n",aux_component);
				n = write(newsockfd,aux_component, string_size+1);
				if (n < 0) error("ERROR writing to socket");
			}
            else
                error("The User Key is not being requested correctly");
        }
        else if(msg->compare(0, 7, "serverK") == 0)
        {
			std::vector<std::string> parts;
			boost::split(parts, *msg, boost::is_any_of(":"));
			if (parts.size() == 2)
			{
				aux_component = kms->getServerKey(std::stoul (parts[1],nullptr,0));
				//printf("Here is the ServerK: %s\n",aux_component);
				n = write(newsockfd,aux_component, string_size+1);
				if (n < 0) error("ERROR writing to socket");
			}
            else
                error("The server Key is not being requested correctly");
        }
        else // Check if the message begins with S or U
        {
            n = write(newsockfd,"puto", string_size);
            if (n < 0) error("ERROR writing to socket");
        }
		close(newsockfd);
        delete msg;
    }
    close(sockfd);
    return 0;
}

ext: cc

max_stars_repo_name: mesarpe/libprotector

max_issues_repo_name: mesarpe/libprotector

max_forks_repo_name: mesarpe/libprotector


text:
/*
   Copyright 2016 Kai Huebl (kai@huebl-sgh.de)

   Lizenziert gemäß Apache Licence Version 2.0 (die „Lizenz“); Nutzung dieser
   Datei nur in Übereinstimmung mit der Lizenz erlaubt.
   Eine Kopie der Lizenz erhalten Sie auf http://www.apache.org/licenses/LICENSE-2.0.

   Sofern nicht gemäß geltendem Recht vorgeschrieben oder schriftlich vereinbart,
   erfolgt die Bereitstellung der im Rahmen der Lizenz verbreiteten Software OHNE
   GEWÄHR ODER VORBEHALTE – ganz gleich, ob ausdrücklich oder stillschweigend.

   Informationen über die jeweiligen Bedingungen für Genehmigungen und Einschränkungen
   im Rahmen der Lizenz finden Sie in der Lizenz.

   Autor: Kai Huebl (kai@huebl-sgh.de)
 */

#include <boost/lexical_cast.hpp>
#include <sstream>
#include "OpcUaStackCore/Base/ObjectPool.h"
#include "OpcUaClient/ClientCommand/CommandWrite.h"

using namespace OpcUaStackCore;

namespace OpcUaClient
{

	CommandWrite::CommandWrite(void)
	: CommandBase(CommandBase::Cmd_Write)
	, nodeIdVec_()
	, dataValueVec_()
	, attributeIdVec_()
	{
	}

	CommandWrite::~CommandWrite(void)
	{
	}

	CommandBase::SPtr
	CommandWrite::createCommand(void)
	{
		CommandBase::SPtr commandBase = constructSPtr<CommandWrite>();
		return commandBase;
	}

	bool
	CommandWrite::validateCommand(void)
	{
		if (nodeIdVec_.size() == 0) {
			std::stringstream ss;
			ss << "neeed at least one node id parameter";
			errorMessage(ss.str());
			return false;
		}

		if (nodeIdVec_.size() != dataValueVec_.size()) {
			std::stringstream ss;
			ss << "data value missing";
			errorMessage(ss.str());
			return false;
		}

		return true;
	}

	bool
	CommandWrite::addParameter(const std::string& parameterName, const std::string& parameterValue)
	{
		if (parameterName == "-NODEID") {
			OpcUaNodeId::SPtr nodeId = constructSPtr<OpcUaNodeId>();
			if (!nodeId->fromString(parameterValue)) {
				std::stringstream ss;
				ss << "node id parameter invalid (" << parameterValue << ")";
				errorMessage(ss.str());
				return false;
			}
			nodeIdVec_.push_back(nodeId);
			attributeIdVec_.push_back(13);
		}
		else if (parameterName == "-VALUE") {
			OpcUaVariant variant;
			if (!variant.fromString(parameterValue)) {
				std::stringstream ss;
				ss << "value parameter invalid (" << parameterValue << ")";
				errorMessage(ss.str());
				return false;
			}

			OpcUaDataValue::SPtr dataValue = constructSPtr<OpcUaDataValue>();
			OpcUaDateTime dateTime(boost::posix_time::microsec_clock::universal_time());
			dataValue->variant()->copyFrom(variant);
			dataValue->sourceTimestamp(dateTime);
			dataValue->serverTimestamp(dateTime);
			dataValue->statusCode(Success);
			dataValueVec_.push_back(dataValue);
		}
		else if (parameterName == "-ATTRIBUTEID") {
			uint32_t attributeId;
			try {
				attributeId = boost::lexical_cast<uint32_t>(parameterValue);
			}
			catch (...)
			{
				std::stringstream ss;
				ss << "AttributeID parameter invalid (" << parameterValue << ")";
				errorMessage(ss.str());
				return false;
		    }

			if (attributeIdVec_.size() == 0) {
				std::stringstream ss;
				ss << "cannot add AttributeId, because NodeId not exist";
				errorMessage(ss.str());
				return false;
			}

			attributeIdVec_[attributeIdVec_.size()-1] = attributeId;
		}
		else {
			std::stringstream ss;
			ss << "invalid parameter " << parameterName;
			errorMessage(ss.str());
			return false;
		}
		return true;
	}

	std::string
	CommandWrite::help(void)
	{
		std::stringstream ss;
		ss << "  -Write: Write one ore more data values to a opc ua server\n"
		   << "    -Session (0..1): Name of the session.\n"
		   << "    -NodeId (1..N): NodeId of the value to write to opc ua server\n"
		   << "      -Value (1): Value to write to the opc ua server\n"
		   << "      -AttributeId (0..1): Attribute Identifier to read. (Default: 13)\n";
		return ss.str();
	}

	OpcUaNodeId::Vec&
	CommandWrite::nodeIdVec(void)
	{
		return nodeIdVec_;
	}

	OpcUaDataValue::Vec&
	CommandWrite::dataValueVec(void)
	{
		return dataValueVec_;
	}

	std::vector<uint32_t>&
	CommandWrite::attributeIdVec(void)
	{
		return attributeIdVec_;
	}

}

max_stars_repo_name: gianricardo/OpcUaStack

max_issues_repo_name: gianricardo/OpcUaStack

max_forks_repo_name: gianricardo/OpcUaStack