/*
  * teleop_pr2
  * Copyright (c) 28, 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 the <ORGANIZATION> 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.
  */
 
 #include <cstdlib>
 #include <cstdio>
 #include <unistd.h>
 #include <math.h>
 #include <fcntl.h>
 #include "ros/ros.h"
 #include "joy/Joy.h"
 #include "geometry_msgs/Twist.h"
 #include "sensor_msgs/JointState.h"
 #include "trajectory_msgs/JointTrajectory.h"
 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
 #include "pr2_controllers_msgs/Pr2GripperCommand.h"
 #include "topic_tools/MuxSelect.h"
 #include "std_msgs/String.h"
 #include "std_msgs/Float64.h"
 
 #define TORSO_TOPIC "torso_controller/command"
 #define HEAD_TOPIC "head_traj_controller/command"
 const int PUBLISH_FREQ = 20;
 
 using namespace std;
 
 class TeleopARToolkit
 {
  public:
  geometry_msgs::Twist cmd;
  double min_torso, max_torso;
  double req_torso_vel, torso_step;
  //joy::Joy joy;
  double req_vx, req_vy, req_vw, req_torso, req_pan, req_tilt;
  double req_tilt_vel, req_pan_vel;
  double max_vx, max_vy, max_vw, max_vx_run, max_vy_run, max_vw_run;
  double max_pan, max_tilt, min_tilt;
  int axis_vx, axis_vy, axis_vw;
  int deadman_button, run_button, torso_dn_button, torso_up_button, base_right_button, base_left_button, r_gripper_button, l_gripper_button;
  bool deadman_no_publish_, torso_publish;
  pr2_controllers_msgs::Pr2GripperCommand open_cmd_;
  pr2_controllers_msgs::Pr2GripperCommand close_cmd_;
  bool right_gripper_open_, left_gripper_open_;

  bool deadman_;
  bool use_mux_, last_deadman_;
  std::string last_selected_topic_;

  ros::Time last_recieved_joy_message_time_;
  ros::Duration joy_msg_timeout_;

  ros::NodeHandle n_, n_private_;
  ros::Publisher vel_pub_;
  ros::Publisher head_pub_;
  ros::Publisher torso_pub_;
  ros::Publisher r_gripper_pub_;
  ros::Publisher l_gripper_pub_;
  ros::Subscriber joy_sub_;
  ros::Subscriber torso_state_sub_;
  ros::Subscriber ar_pose_sub_;
  ros::ServiceClient mux_client_;

  TeleopARToolkit(bool deadman_no_publish = false) :
   max_vx(0.6), max_vy(0.6), max_vw(0.8),
   max_vx_run(0.6), max_vy_run(0.6), max_vw_run(0.8),
   /*max_pan(2.7), max_tilt(1.4), min_tilt(-0.4),*/
   /*pan_step(0.02), tilt_step(0.015),*/
   deadman_no_publish_(deadman_no_publish), deadman_(false), 
   use_mux_(false), last_deadman_(false),
   n_private_("~")
  { }

  void init()
  {
       cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
       req_pan = req_tilt = 0;
       req_torso = 0.0;
       req_torso_vel = 0.0;

       //parameters for interaction with a mux on cmd_vel topics
       n_private_.param("use_mux", use_mux_, false);

       n_private_.param("max_vx", max_vx, max_vx);
       n_private_.param("max_vy", max_vy, max_vy);
       n_private_.param("max_vw", max_vw, max_vw);

       // Set max speed while running
       n_private_.param("max_vx_run", max_vx_run, max_vx_run);
       n_private_.param("max_vy_run", max_vy_run, max_vy_run);
       n_private_.param("max_vw_run", max_vw_run, max_vw_run);

       // Head pan/tilt parameters
       /*n_private_.param("max_pan", max_pan, max_pan);
       n_private_.param("max_tilt", max_tilt, max_tilt);
       n_private_.param("min_tilt", min_tilt, min_tilt);

       n_private_.param("tilt_step", tilt_step, tilt_step);
       n_private_.param("pan_step", pan_step, pan_step);

       n_private_.param("axis_pan", axis_pan, 0);
       n_private_.param("axis_tilt", axis_tilt, 2);*/

       n_private_.param("axis_vx", axis_vx, 3);
       n_private_.param("axis_vw", axis_vw, 0);
       n_private_.param("axis_vy", axis_vy, 2);

       n_private_.param("torso_step", torso_step, 0.01);
       n_private_.param("min_torso", min_torso, 0.0);
       n_private_.param("max_torso", max_torso, 0.3);
       
       n_private_.param("open_position",   open_cmd_.position,     0.08);
       n_private_.param("open_max_effort", open_cmd_.max_effort,  -1.0);

       n_private_.param("close_position",   close_cmd_.position,  -100.00);
       n_private_.param("close_max_effort", close_cmd_.max_effort,   -1.0);

       n_private_.param("deadman_button", deadman_button, 0);
       n_private_.param("run_button", run_button, 0);
       n_private_.param("torso_dn_button", torso_dn_button, 0);
       n_private_.param("torso_up_button", torso_up_button, 0);
       //n_private_.param("head_button", head_button, 0);
       n_private_.param("r_gripper_button",  r_gripper_button,  0);
       n_private_.param("l_gripper_button",  l_gripper_button,  0);
       n_private_.param("base_left_button",  base_left_button,  0);
       n_private_.param("base_right_button",  base_right_button,  0);

       double joy_msg_timeout;
       n_private_.param("joy_msg_timeout", joy_msg_timeout, 0.5); //default to 0.5 seconds timeout
       if (joy_msg_timeout <= 0)
         {
           joy_msg_timeout_ = ros::Duration().fromSec(9999999);//DURATION_MAX;
           ROS_DEBUG("joy_msg_timeout <= 0 -> no timeout");
         }
       else
         {
           joy_msg_timeout_.fromSec(joy_msg_timeout);
           ROS_DEBUG("joy_msg_timeout: %.3f", joy_msg_timeout_.toSec());
         }

       ROS_DEBUG("max_vx: %.3f m/s\n", max_vx);
       ROS_DEBUG("max_vy: %.3f m/s\n", max_vy);
       ROS_DEBUG("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);

       ROS_DEBUG("max_vx_run: %.3f m/s\n", max_vx_run);
       ROS_DEBUG("max_vy_run: %.3f m/s\n", max_vy_run);
       ROS_DEBUG("max_vw_run: %.3f deg/s\n", max_vw_run*180.0/M_PI);

       ROS_DEBUG("axis_vx: %d\n", axis_vx);
       ROS_DEBUG("axis_vy: %d\n", axis_vy);
       ROS_DEBUG("axis_vw: %d\n", axis_vw);

       ROS_DEBUG("deadman_button: %d\n", deadman_button);
       ROS_DEBUG("run_button: %d\n", run_button);
       ROS_DEBUG("torso_dn_button: %d\n", torso_dn_button);
       ROS_DEBUG("torso_up_button: %d\n", torso_up_button);
       ROS_DEBUG("joy_msg_timeout: %f\n", joy_msg_timeout);

       if (torso_dn_button != 0)
         torso_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(TORSO_TOPIC, 1);

       head_pub_ = n_.advertise<trajectory_msgs::JointTrajectory>(HEAD_TOPIC, 1);

       vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
       
       r_gripper_pub_ = n_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("r_gripper_controller/command", 1);
       
       l_gripper_pub_ = n_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("l_gripper_controller/command", 1);

       joy_sub_ = n_.subscribe("joy/joy", 10, &TeleopARToolkit::joy_cb, this);
       
       torso_state_sub_ = n_.subscribe("torso_controller/state", 1, &TeleopARToolkit::torsoCB, this);

       //if we're going to use the mux, then we'll subscribe to state changes on the mux
       if(use_mux_){
         ros::NodeHandle mux_nh("mux");
         mux_client_ = mux_nh.serviceClient<topic_tools::MuxSelect>("select");
       }
     }

  ~TeleopARToolkit() { }

  void joy_cb(const joy::Joy::ConstPtr& joy_msg)
  {
   //Record this message reciept
   last_recieved_joy_message_time_ = ros::Time::now();

   deadman_ = (((unsigned int)deadman_button < joy_msg->get_buttons_size()) && joy_msg->buttons[deadman_button]);

   if (!deadman_)
     return;

   // Base
   bool running = (((unsigned int)run_button < joy_msg->get_buttons_size()) && joy_msg->buttons[run_button]);
   double vx = running ? max_vx_run : max_vx;
   double vy = running ? max_vy_run : max_vy;
   double vw = running ? max_vw_run : max_vw;

   if((axis_vx >= 0) && (((unsigned int)axis_vx) < joy_msg->get_axes_size()))
     req_vx = joy_msg->axes[axis_vx] * vx;
   else
     req_vx = 0.0;
   if((axis_vw >= 0) && (((unsigned int)axis_vw) < joy_msg->get_axes_size()))
     req_vw = joy_msg->axes[axis_vw] * vw;
   else
     req_vw = 0.0;
     
   bool right = (((unsigned int)base_right_button < joy_msg->get_buttons_size()) && joy_msg->buttons[base_right_button]);
   bool left = (((unsigned int)base_left_button < joy_msg->get_buttons_size()) && joy_msg->buttons[base_left_button]);
   
   if (right && !left)
     req_vy = -vy;
   else if (left && !right)
     req_vy = vy;
   else
     req_vy = 0;

   // Enforce max/mins for velocity
   // Joystick should be [-1, 1], but it might not be
   req_vx = max(min(req_vx, vx), -vx);
   req_vw = max(min(req_vw, vw), -vw);

   // Torso
   bool down = (((unsigned int)torso_dn_button < joy_msg->get_buttons_size()) && joy_msg->buttons[torso_dn_button]);
   bool up = (((unsigned int)torso_up_button < joy_msg->get_buttons_size()) && joy_msg->buttons[torso_up_button]);

   // Bring torso up/down
   if (down && !up)
     req_torso_vel = -torso_step;
   else if (up && !down)
     req_torso_vel = torso_step;
   else
     req_torso_vel = 0;
     
   //Grippers open/close
   if ((int) joy_msg->buttons.size() <= r_gripper_button || (int) joy_msg->buttons.size() <= l_gripper_button || r_gripper_button < 0 || l_gripper_button < 0)
    {
      ROS_ERROR("Array lookup error: Joystick message has %u elems. Open Index [%u]. Close Index [%u]", (unsigned int)joy_msg->buttons.size(), r_gripper_button, l_gripper_button);
      return;
    }

    if (joy_msg->buttons[r_gripper_button] == 1)
    {
      right_gripper_open_ = 1;
      //r_gripper_pub_.publish(close_cmd_);
      //ROS_DEBUG("Closing right gripper");
    }
    else
    {
      right_gripper_open_ = 0;
      //r_gripper_pub_.publish(open_cmd_);
      //ROS_DEBUG("Opening right gripper");
    }
    
    if (joy_msg->buttons[l_gripper_button] == 1)
    {
      left_gripper_open_ = 1;
      //l_gripper_pub_.publish(close_cmd_);
      //ROS_DEBUG("Closing left gripper");
    }
    else
    {
      left_gripper_open_ = 0;
      //l_gripper_pub_.publish(open_cmd_);
      //ROS_DEBUG("Opening left gripper");
    }
  }


  void send_cmd_vel()
  {
   if(deadman_ && last_recieved_joy_message_time_ + joy_msg_timeout_ > ros::Time::now())
   {
     //check if we need to switch the mux to our topic for teleop
     if(use_mux_ && !last_deadman_){
       topic_tools::MuxSelect select_srv;
       select_srv.request.topic = vel_pub_.getTopic();
       if(mux_client_.call(select_srv)){
         last_selected_topic_ = select_srv.response.prev_topic;
         ROS_DEBUG("Setting mux to %s for teleop", select_srv.request.topic.c_str());
       }
       else{
         ROS_ERROR("Failed to call select service %s on mux. Are you sure that it is up and connected correctly to the teleop node?", mux_client_.getService().c_str());
       }
     }

     // Base
     cmd.linear.x = req_vx;
     cmd.linear.y = req_vy;
     cmd.angular.z = req_vw;
     vel_pub_.publish(cmd);

     // Torso
     {
       double dt = 1.0/double(PUBLISH_FREQ);
       double horizon = 5.0 * dt;

       trajectory_msgs::JointTrajectory traj;
       traj.header.stamp = ros::Time::now() + ros::Duration(0.01);
       traj.joint_names.push_back("torso_lift_joint");
       traj.points.resize(1);
       traj.points[0].positions.push_back(req_torso + req_torso_vel * horizon);
       traj.points[0].velocities.push_back(req_torso_vel);
       traj.points[0].time_from_start = ros::Duration(horizon);
       torso_pub_.publish(traj);

       // Updates the current positions
       req_torso += req_torso_vel * dt;
       req_torso = max(min(req_torso, max_torso), min_torso);
     }
     
     //Grippers
     if (right_gripper_open_)
     {
       r_gripper_pub_.publish(close_cmd_);
       ROS_DEBUG("Closing right gripper");
     }
     else
     {
       r_gripper_pub_.publish(open_cmd_);
       ROS_DEBUG("Opening right gripper");
     }
    
     if (left_gripper_open_)
     {
       l_gripper_pub_.publish(close_cmd_);
       ROS_DEBUG("Closing left gripper");
     }
     else
     {
       l_gripper_pub_.publish(open_cmd_);
       ROS_DEBUG("Opening left gripper");
     }
   }
   else
   {
     //make sure to set the mux back to whatever topic it was on when we grabbed it if the deadman has just toggled
     if(use_mux_ && last_deadman_){
       topic_tools::MuxSelect select_srv;
       select_srv.request.topic = last_selected_topic_;
       if(mux_client_.call(select_srv)){
         ROS_DEBUG("Setting mux back to %s", last_selected_topic_.c_str());
       }
       else{
         ROS_ERROR("Failed to call select service %s on mux. Are you sure that it is up and connected correctly to the teleop node?", mux_client_.getService().c_str());
       }
     }

     // Publish zero commands iff deadman_no_publish is false
     cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
     if (!deadman_no_publish_)
     {
       // Base
       vel_pub_.publish(cmd);
     }
   }

   //make sure we store the state of our last deadman
   last_deadman_ = deadman_;
  }

  void torsoCB(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
  {
    double xd = req_torso;
    const double A = 0.3;
    if (fabs(msg->actual.positions[0] - xd) > A*1.1)
    {
      req_torso = min(max(msg->actual.positions[0] - A, xd), msg->actual.positions[0] + A);
    }
  }
};
 
 int main(int argc, char **argv)
 {
   ros::init(argc, argv, "teleop_ar_toolkit");
   const char* opt_no_publish    = "--deadman_no_publish";
 
   bool no_publish = false;
   for(int i=1;i<argc;i++)
   {
     if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
       no_publish = true;
   }
 
   TeleopARToolkit teleop_ar_toolkit(no_publish);
   teleop_ar_toolkit.init();
 
   ros::Rate pub_rate(PUBLISH_FREQ);
 
   while (teleop_ar_toolkit.n_.ok())
   {
     ros::spinOnce();
     teleop_ar_toolkit.send_cmd_vel();
     pub_rate.sleep();
   }
 
   exit(0);
   return 0;
 }