#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "srmp_example");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // ============ Panda0 to 0000000 ============
  // Create a MoveGroupInterface for the desired planning group.
  moveit::planning_interface::MoveGroupInterface move_group0("panda0_arm");
  // Set the OMPL planner to RRTConnect.
  move_group0.setPlannerId("MHAstar");

  // Specify the target joint values (update these as needed).
  move_group0.setJointValueTarget({0.0, -0.0, 0.0, -0.0, 0.0, 0.0, 0.0});

  // Plan a motion from the current state to the target.
  moveit::planning_interface::MoveGroupInterface::Plan plan0;
  bool success0 = (move_group0.plan(plan0) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO("Planning %s", success0 ? "succeeded" : "failed");

  if (success0) {
      // Execute the motion.
      move_group0.execute(plan0);
  }

  // ============ Panda1 to left ============
  std::vector<std::vector<double>> goal_states1 = {
    {-0.129, -0.76, -0.548, -2.394, -0.37, 1.703, -0.494},
//    {-0.08, -0.14, 0.85, -1.6, 0.1, 1.5, 0.7},
    {0, -0.5, 0, -1.5, 0, 1, 0}
  };
  for (auto goal_state1 : goal_states1)
  {
    // Create a MoveGroupInterface for the desired planning group.
    moveit::planning_interface::MoveGroupInterface move_group1("panda1_arm");
    // Set the OMPL planner to RRTConnect.
    move_group1.setPlannerId("MHAstar");

    // Specify the target joint values (update these as needed).
    move_group1.setJointValueTarget(goal_state1);

    // Plan a motion from the current state to the target.
    moveit::planning_interface::MoveGroupInterface::Plan plan1;
    bool success1 = (move_group1.plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    ROS_INFO("Planning %s", success1 ? "succeeded" : "failed");

    if (success1)
    {
      // Execute the motion.
      move_group1.execute(plan1);
    }
  }
  ros::shutdown();
  return 0;
}
