#include "impl/moveit_grasp_mediator.h"
#include <regex>
#include <gb_grasp/MapConfigLoader.h>
#include <moveit/robot_state/conversions.h>
#include "impl/wing_moveit_decorator.h"


void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
	robots_.push_back(robot);
	ROS_INFO("%s connected...", robot->name().c_str());
	auto mr = dynamic_cast<Moveit_robot*>(robot);

	manipulate_grasp_data(mr);

	auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
	double min_quality, max_candidates;
	bool top_only;

  rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);

  ROS_INFO("min_quality: %f", min_quality);
  ROS_INFO("max_candidate_count: %f", max_candidates);
  ROS_INFO("top_grasps_only: %f", top_only);


  map_gen->SetQuality(min_quality);
  map_gen->SetMaxGrasp(max_candidates);
	map_gen->SetZGraspOnly(top_only);
	mr->set_grasp_map_generator(map_gen);

}

void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
    double r,p,y;
    std::vector<double> rot_mapspace(3,0);

    tf2::Matrix3x3 m(tf.getRotation());
    m.getRPY(r,p, y);
    nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
    nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
    nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
    //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
    //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
    //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
}

void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
	XmlRpc::XmlRpcValue data;
	nh_->getParam("/", data);
	
	std::regex rx("panda_arm([0-9]+)");
    std::smatch match;
	std::regex_match(robot->name(), match, rx);
	data["base_link"]= "world";

	data[robot->map().at("eef_name").c_str()]["end_effector_name"] = robot->map().at("eef_name").c_str();
	data[robot->map().at("eef_name").c_str()]["end_effector_type"] = "finger";

	data[robot->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001;
	data[robot->map().at("eef_name").c_str()]["angle_resolution"] = 45;

	data[robot->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035;
	data[robot->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01;

	data[robot->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005;

	data[robot->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05;
	data[robot->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05;
	data[robot->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02;

	data[robot->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005;

	std::stringstream ss;
	ss << "panda_" <<  match[1] << "_tool_center_point";
	data[robot->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
	data[robot->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105;
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853;

	data[robot->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
	data[robot->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;

	data[robot->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04;
	data[robot->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
	data[robot->map().at("eef_name").c_str()]["joints"][0] = robot->mgi_hand()->getJoints()[1].c_str();

	data[robot->map().at("eef_name").c_str()]["max_finger_width"] = 0.085;
	data[robot->map().at("eef_name").c_str()]["min_finger_width"] = 0.06;

	data[robot->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08;

	data[robot->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015;

	data[robot->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022;
	data[robot->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012;

	data[robot->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
	data[robot->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
	nh_->setParam("/", data);
}

bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth,
                            double& y_width, double& z_height)
  {
    // Generate random cuboid
    double xmin = 0.1;
    double xmax = 0.7;
    double ymin = -0.25;
    double ymax = 0.25;
    double zmin = 0.89;
    double zmax = 1.2;
    rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);

    double cuboid_size_min = 0.01;
    double cuboid_size_max = 0.03;
    rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);

    object_name = "pick_target";
    visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds);
    visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED);
    visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
    visual_tools_->trigger();

    bool success = true;
    double timeout = 5;  // seconds
    ros::Rate rate(100);
    while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name))
    {
      rate.sleep();
      success = rate.cycleTime().toSec() < timeout;
    }
    return success;
}

void Moveit_grasp_mediator::mediate(){
	visual_tools_->deleteAllMarkers();

  ROS_INFO("ns is %s", grasp_ns_.c_str());
  MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
  map_config.SetObjects(cbd);
  float object_offset = 0.02f;

  tf2::Vector3 middle_pt(0,0,0);
  std::vector<double> x;
  std::vector<double> y;
  for(auto* ar: robots_){
    Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
    tf2::Vector3 pos = r->tf().getOrigin();
    pos.setZ((pos.getZ()*2) +object_offset);
    tf2::Transform man_tf(r->tf().getRotation(), pos);
    manipulate_mapspace(man_tf, r->size());
    map_config.Create(grasp_maps_);
    middle_pt+= pos;
    ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());

    tf2::Transform A = r->tf() * r->bounds()[0];
    tf2::Transform B = r->tf() * r->bounds()[1];
    tf2::Transform C = r->tf() * r->bounds()[2];
    tf2::Transform D = r->tf() * r->bounds()[3];
    x.push_back(A.getOrigin().getX());
    x.push_back(B.getOrigin().getX());
    x.push_back(C.getOrigin().getX());
    x.push_back(D.getOrigin().getX());
    y.push_back(A.getOrigin().getY());
    y.push_back(B.getOrigin().getY());
    y.push_back(C.getOrigin().getY());
    y.push_back(D.getOrigin().getY());

    r->grasp_map_generator()->SampleMap(grasp_maps_.front());
    for (auto* are: r->observers()){
      auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
      auto w = dynamic_cast<Wing*>(wmd->wing());
      pos.setX(w->world_tf().getOrigin().getX());
      pos.setY(w->world_tf().getOrigin().getY());
      man_tf.setRotation(w->world_tf().getRotation());
      man_tf.setOrigin(pos);
      manipulate_mapspace(man_tf, w->size());
      map_config.Create(grasp_maps_);
      middle_pt+= pos;
      ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());

      A = w->world_tf() * w->bounds()[0];
      B = w->world_tf() * w->bounds()[1];
      C = w->world_tf() * w->bounds()[2];
      D = w->world_tf() * w->bounds()[3];
      x.push_back(A.getOrigin().getX());
      x.push_back(B.getOrigin().getX());
      x.push_back(C.getOrigin().getX());
      x.push_back(D.getOrigin().getX());
      y.push_back(A.getOrigin().getY());
      y.push_back(B.getOrigin().getY());
      y.push_back(C.getOrigin().getY());
      y.push_back(D.getOrigin().getY());
      r->grasp_map_generator()->SampleMap(grasp_maps_.front());
    }
  }
  ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
  middle_pt/= grasp_maps_.size();
  ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
  auto x_min = *std::min_element(x.begin(),x.end());
  auto x_max = *std::max_element(x.begin(),x.end());
  auto y_min = *std::min_element(y.begin(),y.end());
  auto y_max = *std::max_element(y.begin(),y.end());
  ROS_INFO("width: %f", x_max-x_min);
  ROS_INFO("height: %f", y_max-y_min);

  visual_tools_->setAlpha(0.1);
  voxel_manager_->Voxelize();

  auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
  for(auto&o:cod){
    voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
  }
  voxel_manager_->Voxelize(true);
    

    // -----------------------------------
    // Generate random object to grasp
    geometry_msgs::Pose object_pose;
    double object_x_depth;
    double object_y_width;
    double object_z_height;
    std::string object_name;
    if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height))
    {
      ROS_INFO("Failed to add random cuboid ot planning scene");
    }

    // -----------------------------------
    // Generate grasp candidates
    std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;

    // Configure the desired types of grasps
    moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
        moveit_grasps::TwoFingerGraspCandidateConfig();
    grasp_generator_config.disableAll();
    grasp_generator_config.enable_face_grasps_ = true;
    grasp_generator_config.generate_y_axis_grasps_ = true;
    grasp_generator_config.generate_x_axis_grasps_ = true;
    grasp_generator_config.generate_z_axis_grasps_ = true;

    auto grasp_generator_ =std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_);

    // Set the ideal grasp orientation for scoring
    std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
    grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);

    // Set custom grasp score weights
    auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>();
    grasp_score_weights->orientation_x_score_weight_ = 2.0;
    grasp_score_weights->orientation_y_score_weight_ = 2.0;
    grasp_score_weights->orientation_z_score_weight_ = 2.0;
    grasp_score_weights->translation_x_score_weight_ = 1.0;
    grasp_score_weights->translation_y_score_weight_ = 1.0;
    grasp_score_weights->translation_z_score_weight_ = 1.0;
    // Finger gripper specific weights.
    grasp_score_weights->depth_score_weight_ = 2.0;
    grasp_score_weights->width_score_weight_ = 2.0;
    // Assign the grasp score weights in the grasp_generator
    grasp_generator_->setGraspScoreWeights(grasp_score_weights);
    auto grasp_data_ = std::make_shared<moveit_grasps::TwoFingerGraspData>(*nh_, "hand_1", visual_tools_->getRobotModel());
    if (!grasp_data_->loadGraspData(*nh_, "hand_1"))
    {
      ROS_INFO("Failed to load Grasp Data parameters.");
      exit(-1);
    }

    grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
    if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width,
                                          object_z_height, grasp_data_, grasp_candidates))
    {
      ROS_INFO("Grasp generator failed to generate any valid grasps");
    }

    // --------------------------------------------
    // Generating a seed state for filtering grasps
    auto seed_state = std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
    Eigen::Isometry3d eef_mount_grasp_pose =
        visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
    if (!getIKSolution(robot_model_->getJointModelGroup("panda_arm1"), eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
    {
      ROS_INFO("The ideal seed state is not reachable. Using start state as seed.");
    }

    // --------------------------------------------
    // Filtering grasps
    // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
    bool filter_pregrasps = true;
    auto grasp_filter_ = std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_);
    if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, robot_model_->getJointModelGroup("panda_arm1"), seed_state, filter_pregrasps,
                                     object_name))
    {
      ROS_INFO("Filter grasps failed");
    }
    if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
    {
      ROS_INFO("Grasp filtering removed all grasps");
    }

    // Plan free-space approach, cartesian approach, lift and retreat trajectories
    moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
    moveit_msgs::MotionPlanResponse pre_approach_plan;
    if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name))
    {
      ROS_INFO("Failed to plan grasp motions");
    }

    visualizePick(selected_grasp_candidate, pre_approach_plan);
}

  void Moveit_grasp_mediator::visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                     const moveit_msgs::MotionPlanResponse& pre_approach_plan)
  {
    EigenSTL::vector_Isometry3d waypoints;
    moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);

    // Visualize waypoints
    visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
    visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
    visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
    visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
    visual_tools_->trigger();

    // Get the pre and post grasp states
    visual_tools_->prompt("pre_grasp");
    robot_state::RobotStatePtr pre_grasp_state =
        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
    valid_grasp_candidate->getPreGraspState(pre_grasp_state);
    visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE);
    robot_state::RobotStatePtr grasp_state =
        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
    if (valid_grasp_candidate->getGraspStateClosed(grasp_state))
    {
      visual_tools_->prompt("grasp");
      visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
    }
    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
        valid_grasp_candidate->segmented_cartesian_traj_[1].size())
    {
      visual_tools_->prompt("lift");
      visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
                                       rviz_visual_tools::BLUE);
    }
    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
        valid_grasp_candidate->segmented_cartesian_traj_[2].size())
    {
      visual_tools_->prompt("retreat");
      visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
                                       rviz_visual_tools::PURPLE);
    }

    visual_tools_->prompt("show free space approach");
    visual_tools_->hideRobot();
    visual_tools_->trigger();

    bool wait_for_animation = true;
    visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation);
    ros::Duration(0.25).sleep();
    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH)
      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH],
                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
    ros::Duration(0.25).sleep();

    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT)
      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT],
                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
    ros::Duration(0.25).sleep();

    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT)
      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT],
                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
    ros::Duration(0.25).sleep();
}

bool Moveit_grasp_mediator::planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
                     moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                     moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name)
  {
    moveit::core::RobotStatePtr current_state;
    {
      boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
          new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
      current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
    }

    std::shared_ptr<moveit_grasps::GraspPlanner>grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);

    // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
    grasp_planner_->setWaitForNextStepCallback(boost::bind(&Moveit_grasp_mediator::waitForNextStep, this, visual_tools_, _1));

    bool success = false;
    for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
    {
      valid_grasp_candidate = grasp_candidates.front();
      valid_grasp_candidate->getPreGraspState(current_state);
      if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false,
                                                   object_name))
      {
        ROS_INFO("failed to plan approach lift retreat");
        continue;
      }

      robot_state::RobotStatePtr pre_grasp_state =
          valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
      if (!planPreApproach(*pre_grasp_state, pre_approach_plan))
      {
        ROS_INFO("failed to plan to pregrasp_state");
        continue;
      }

      success = true;
      break;
    }
    return success;
}

void Moveit_grasp_mediator::waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
{
  visual_tools->prompt(prompt);
}

bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan)
  {
    planning_interface::MotionPlanRequest req;
    planning_interface::MotionPlanResponse res;

    double tolerance_above = 0.01;
    double tolerance_below = 0.01;
    // req.planner_id = "RRTConnectkConfigDefault";
    req.group_name = "panda_1_arm1";
    req.num_planning_attempts = 5;
    req.allowed_planning_time = 1.5;
    moveit_msgs::Constraints goal =
        kinematic_constraints::constructGoalConstraints(goal_state, robot_model_->getJointModelGroup("panda_arm1"), tolerance_below, tolerance_above);

    req.goal_constraints.push_back(goal);
    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
        new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));

    // ---------------------------------
    // Change the robot current state
    // NOTE: We have to do this since Panda start configuration is in self collision.
    robot_state::RobotState rs = (*ls)->getCurrentState();
    std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
    std::vector<std::string> joint_names = { "panda_1_joint1", "panda_1_joint2", "panda_1_joint3", "panda_1_joint4",
                                             "panda_1_joint5", "panda_1_joint6", "panda_1_joint7" };
    // arm_jmg_->getActiveJointModelNames();
    for (std::size_t i = 0; i < joint_names.size(); ++i)
    {
      rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
    }
    rs.update();
    robot_state::robotStateToRobotStateMsg(rs, req.start_state);
    // ---------------------------

    planning_pipeline_->generatePlan(*ls, req, res);
    if (res.error_code_.val != res.error_code_.SUCCESS)
    {
      ROS_INFO( "Failed to plan approach successfully");
      return false;
    }

    res.getMessage(pre_approach_plan);
    return true;
}

bool Moveit_grasp_mediator::isStateValid(const planning_scene::PlanningScene* planning_scene,
                  const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
                  const robot_model::JointModelGroup* group, const double* ik_solution)
{
  robot_state->setJointGroupPositions(group, ik_solution);
  robot_state->update();
  return !planning_scene->isStateColliding(*robot_state, group->getName());
}

bool Moveit_grasp_mediator::getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose,
                     robot_state::RobotState& solution, const std::string& link_name)
  {
    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
        new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));

    moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
        &Moveit_grasp_mediator::isStateValid,this, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3);

    // seed IK call with current state
    solution = (*ls)->getCurrentState();

    // Solve IK problem for arm
    // disable explicit restarts to guarantee close solution if one exists
    const double timeout = 0.1;
    return solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn);
}

void Moveit_grasp_mediator::scene_setup(){
	for(Abstract_robot* ar: robots_) ar->notify();
    visual_tools_->setAlpha(1.0);
	  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
    auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();

	  ROS_INFO("%i die cuboids", cbd.size());
	  ROS_INFO("%i cuboids", cod.size());

    for(auto&o:cod){
      o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
      psi_->addCollisionObjects({o.getCoMsg()});
    }

    for(auto&o:cbd){
		//o.publish(visual_tools_,rviz_visual_tools::GREEN);
		  psi_->addCollisionObjects({o.getCoMsg()});
	  }

	visual_tools_->publishRobotState(mgi_->getCurrentState());
	visual_tools_->trigger();
        
}

Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
	: Moveit_mediator(objects, pub, nh)
    , robot_reader_(std::make_unique<Robot_reader>(nh))
    , wing_reader_(std::make_unique<Wing_reader>(nh))
    , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
    // , job_reader_(std::make_unique<Job_reader>(nh)) {
        visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
        robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
        visual_tools_->loadMarkerPub();
        visual_tools_->loadRemoteControl();
        visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);

        visual_tools_->loadRobotStatePub("/display_robot_state");
        visual_tools_->setRobotStateTopic("/robot_state_publisher");
        visual_tools_->loadTrajectoryPub("/display_planned_path");

        visual_tools_->loadSharedRobotState();
        visual_tools_->enableBatchPublishing();
        visual_tools_->deleteAllMarkers();
        visual_tools_->removeAllCollisionObjects();
        visual_tools_->trigger();

        nh_->getParam("/grasps_ns", grasp_ns_);
        grasp_ns_ = "grasps_ns";
        ROS_INFO("ns is %s", grasp_ns_.c_str());

        planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
        voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);

        auto rd = robot_reader_->robot_data();
        for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));

        auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
        for (int i = 0; i < robots_.size(); i++){
            for(object_data& w : wd[i].first){
                w.pose_ = robots_[i]->tf().inverse() * w.pose_;
            }
            robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
        }

        set_wings(wd);    

        for (int i = 0; i < robots_.size(); i++){
            std::bitset<3> bs = wd[i].second;
            build_wings(bs, i);
        }

        for (auto* r : robots_)r->notify();

        publish_tables();
        scene_setup();
    };