// Include roscpp, the ROS C++ client library
#include <ros/ros.h>
// Include the action client infrastructure, so that we can send goals 
// to action servers
#include <actionlib/client/simple_action_client.h>

// Include messages that we'll use
#include <approach_table_tools/GetCheckerboardPose.h>
#include <approach_table_tools/GetApproachPose.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <pr2_common_action_msgs/TuckArmsAction.h>
#include <pr2_controllers_msgs/SingleJointPositionAction.h>

int
main(int argc, char** argv)
{
  // Initialize roscpp, naming our node
  ros::init(argc, argv, "nav_executive");
  // Create a NodeHandle, which we'll use to create publishers and subscribers
  ros::NodeHandle n;

  //////////////////////////////////////
  // Lower the torso, because navigation works better that way.
  actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> torso_client("/torso_controller/position_joint_action", true);
  // Wait for the action server to become available
  torso_client.waitForServer();
  // Create and fill in a goal for this action
  pr2_controllers_msgs::SingleJointPositionGoal torso_goal;
  // We send it to 0.03, because it can't actually reach 0.0
  torso_goal.position = 0.03;
  torso_goal.min_duration = ros::Duration(2.0);
  torso_goal.max_velocity = 1.0;
  ROS_INFO("lowering the torso");
  torso_client.sendGoal(torso_goal);
  torso_client.waitForResult();
  if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_BREAK();
  //////////////////////////////////////

  //////////////////////////////////////
  // Tuck the arms, because navigation requires it
  actionlib::SimpleActionClient<pr2_common_action_msgs::TuckArmsAction> tuck_arms_client("tuck_arms", true);
  tuck_arms_client.waitForServer();
  pr2_common_action_msgs::TuckArmsGoal tuck_arms_goal;
  tuck_arms_goal.tuck_left = true;
  tuck_arms_goal.tuck_right = true;
  tuck_arms_client.sendGoal(tuck_arms_goal);
  tuck_arms_client.waitForResult();
  if(tuck_arms_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_BREAK();
  //////////////////////////////////////
  
  //////////////////////////////////////
  // Get the checkerboard pose

  // TODO
  // Call the service provided by the checkerboard detector, 
  // to get the pose of the big checkerboard that is attached to the table.  
  // The service is defined in approach_table_tools/srv/GetCheckerboardPose.srv.
  // The service is called "get_checkerboard_pose".
  // That checkerboard is 5 X 4, with 0.08m X 0.08m squares.  You should
  // end up with a geometry_msgs::PoseStamped that tells you where the
  // checkerboard is.
  //////////////////////////////////////

  //////////////////////////////////////
  // Get the approach pose

  // TODO
  // Call the service provided by the approach pose finder
  // to get a pose that is 1m in front of the checkerboard.
  // The service is defined in approach_table_tools/srv/GetApproachPose.srv.
  // The service is called "get_approach_pose".
  // You should provide the pose that you got from the checkerboard
  // detector, and get in return the approach pose.  Once you have the
  // approach pose, create a ros::Publisher, publish the pose (as a
  // geometry_msgs::PoseStamped), and use rviz to visualize it (hint: make
  // your Publisher latched, to avoid the chance of rviz missing the initial
  // publication).
  //////////////////////////////////////

  //////////////////////////////////////
  // Approach the table

  // TODO
  // Send the robot to the approach pose that you got from the approach 
  // pose finder.  Use the navigation stack, which is an action server that
  // accepts a PoseStamped goal.  The action is defined in 
  // move_base_msgs/action/MoveBase.action.
  // The action is called "move_base_local".
  //////////////////////////////////////

  //////////////////////////////////////
  // Untuck the arms, to prepare for manipulation

  // TODO
  // Reuse the tuck_arms_client, and send a new goal with 
  // tuck_left and tuck_right set to false.
  //////////////////////////////////////

  //////////////////////////////////////
  // Raise the torso, to prepare for manipulation
  // We set the goal to 0.195, because that's almost all the way at the top
  torso_goal.position = 0.195;
  torso_goal.min_duration = ros::Duration(2.0);
  torso_goal.max_velocity = 1.0;
  ROS_INFO("Raising the torso");
  torso_client.sendGoal(torso_goal);
  torso_client.waitForResult();
  if(torso_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_BREAK();
  //////////////////////////////////////

  //////////////////////////////////////
  // Move close to the table, using local navigation (NO OBSTACLE AVOIDANCE!)
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> drive_base_client("drive_base_action", true);
  drive_base_client.waitForServer();
  move_base_msgs::MoveBaseGoal drive_base_goal;
  // Construct a goal for local navigation that is 0.3m straight forward
  drive_base_goal.target_pose.header.frame_id = "base_link";
  drive_base_goal.target_pose.pose.position.x = 0.3;
  drive_base_goal.target_pose.pose.position.y = 0.0;
  drive_base_goal.target_pose.pose.position.z = 0.0;
  drive_base_goal.target_pose.pose.orientation.x = 0.0;
  drive_base_goal.target_pose.pose.orientation.y = 0.0;
  drive_base_goal.target_pose.pose.orientation.z = 0.0;
  drive_base_goal.target_pose.pose.orientation.w = 1.0;
  ROS_INFO("Moving close to the table");
  drive_base_client.sendGoal(drive_base_goal);
  drive_base_client.waitForResult();
  if(drive_base_client.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_BREAK();
  //////////////////////////////////////

  //////////////////////////////////////
  // Move to the right-hand table, using local navigation (NO OBSTACLE AVOIDANCE!)
  // TODO
  // Reuse the drive_base_client to send a new goal that is 1.1m to the 
  // robot's right (hint: that's negative in Y).
  //////////////////////////////////////

  ROS_INFO("Finished");
  return 0;
}