#include "follow_waypoints/follow_waypoints.h"

namespace follow_waypoints 
{
  FollowWayPoints::FollowWayPoints(ros::NodeHandlePtr nh, std::string node_name) : private_nh_(nh)
  {
    if(private_nh_ == NULL) exit(0);
    // private_nh_ = ros::NodeHandle("~/"+node_name);
    // Read parameters off the parameter server --------------------------------------------
    private_nh_->param("goal_frame_id", frame_id_, std::string("map"));
    private_nh_->param("odom_frame_id", odom_frame_id_, std::string("odom"));
    private_nh_->param("base_frame_id", base_frame_id_, std::string("base_footprint"));
    private_nh_->param("wait_duration", wait_duration_, 0.0);
    private_nh_->param("waypoint_distance_tolerance", distance_tolerance_, 0.0);
    private_nh_->param("waypoints_to_follow_topic", waypoints_to_follow_topic_, std::string("/clicked_points"));
    private_nh_->param("waypoints_list_topic", waypoints_list_topic_, std::string("/waypoints"));
    
    // listen to points or to poses?
    private_nh_->param("waypoints_are_poses", waypoints_are_poses_, true);

    // list of waypoints to follow
    // self.waypoints = collections.deque()
   
    // Is the path provided by the user ready to follow?
    path_ready_ = false;

    // Dynamic Reconfigure -----------------------------------------------------------------
    f = boost::bind(&FollowWayPoints::dyn_callback, this, _1, _2);
    server.setCallback(f);

    // move_base Action client -------------------------------------------------------------
    move_base_client_ = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>("/move_base", true);
    ROS_INFO("Connecting to move_base...");
    if(move_base_client_ != NULL) move_base_client_->waitForServer();
    ROS_INFO("Connected to move_base.");

    // Subscribers & Publishers & Services initialisation ----------------------------------
    path_ready_srv_ = private_nh_->advertiseService("/path_ready",&FollowWayPoints::make_path_ready, this);
    path_reset_srv_ = private_nh_->advertiseService("/path_reset",&FollowWayPoints::do_path_reset, this);
    ROS_INFO("Ready to add two ints.");
    
    // publish waypoints as pose array - visualise them in rviz
    pose_array_publisher_ = private_nh_->advertise<geometry_msgs::PoseArray>(waypoints_list_topic_,1, true);

    // Listen to the goal locations
    waypoints_sub_ = private_nh_->subscribe(waypoints_to_follow_topic_, 1000, &FollowWayPoints::point_cb, this);
    loop_rate_ = new ros::Rate(20);
    // // Create thread 
    // lineThread = boost::thread(&FollowWayPoints::run, this);
    // if(lineThread.joinable()) {
    //     lineThread.join();
    // }
  }
  
  FollowWayPoints::~FollowWayPoints()
  {
    delete(move_base_client_);

  }

  void FollowWayPoints::dyn_callback(follow_waypoints::FollowWayPointsConfig &config, int32_t level)
  {
    dyn_config_ = config;
    ROS_INFO("Reconfigure Request: %d", config.patrol_mode);
  }

  bool FollowWayPoints::make_path_ready(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
  {
    path_ready_ = true;
    res.success = path_ready_;
    res.message = "path_ready_ = true";
    return true;
  }

  bool FollowWayPoints::do_path_reset(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
  {
    ROS_WARN("Issuing cancel command to move_base");

    ros::Publisher pub = private_nh_->advertise<actionlib_msgs::GoalID>("/move_base/cancel", 1);
    pub.publish(actionlib_msgs::GoalID());
    ROS_WARN("move_base goal cancelled.");

    if(!waypoints_.empty())
    {
      waypoints_.clear();
      pub_waypoints_list();
    }
    res.success = path_ready_;
    res.message = "move_base goal cancelled.";
    return true;
  }

  void FollowWayPoints::pub_waypoints_list()
  {
    pose_array_publisher_.publish(toPoseArray(waypoints_));
  }

  void FollowWayPoints::point_cb(geometry_msgs::PointStamped point)
  {
    waypoints_.insert(waypoints_.end(), this->to_pose_with_Cov(point));
    ROS_INFO("Added new waypoint -> (%f, %f) | # Waypoints: %d", point.point.x, point.point.y, (int)waypoints_.size());
    if(waypoints_.size() > 1)
    {
      for(int i = 0; i < waypoints_.size() - 1; i++)
      {
        double x1 = waypoints_[i].pose.pose.position.x;
        double y1 = waypoints_[i].pose.pose.position.y;
        double x2 = waypoints_[i+1].pose.pose.position.x;
        double y2 = waypoints_[i+1].pose.pose.position.y;
        double theta = atan2(y2 - y1, x2 - x1);
        tf2::Quaternion temp; 
        temp.setRPY(0, 0, theta);
        waypoints_[i+1].pose.pose.orientation.x = temp.getX();
        waypoints_[i+1].pose.pose.orientation.y = temp.getY();
        waypoints_[i+1].pose.pose.orientation.z = temp.getZ();
        waypoints_[i+1].pose.pose.orientation.w = temp.getW();
      }
    }
    else
    {
      tf::StampedTransform transform;
      try
      {
        listener_.lookupTransform(frame_id_, base_frame_id_, ros::Time(0), transform);
      }
      catch (tf::TransformException &ex) 
      {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        return;
      }
      double x1 = transform.getOrigin().x();
      double y1 = transform.getOrigin().y();
      double x2 = waypoints_[0].pose.pose.position.x;
      double y2 = waypoints_[0].pose.pose.position.y;
      double theta = atan2(y2 - y1, x2 - x1);
      tf2::Quaternion temp; 
      temp.setRPY(0, 0, theta);
      waypoints_[0].pose.pose.orientation.x = temp.getX();
      waypoints_[0].pose.pose.orientation.y = temp.getY();
      waypoints_[0].pose.pose.orientation.z = temp.getZ();
      waypoints_[0].pose.pose.orientation.w = temp.getW();
    }
    this->pub_waypoints_list();
  }

  void FollowWayPoints::send_move_base_goal(geometry_msgs::PoseWithCovarianceStamped pose)
  {
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = frame_id_;
    goal.target_pose.pose.position = pose.pose.pose.position;
    goal.target_pose.pose.orientation = pose.pose.pose.orientation;
    ROS_INFO("Executing move_base goal -> (%f, %f) ...", pose.pose.pose.position.x, pose.pose.pose.position.y);
    if(move_base_client_ != NULL) move_base_client_->sendGoal(goal);
  }

  void FollowWayPoints::run_once()
  {
    if(!path_ready_) 
    {
      ros::Duration(1.0).sleep();
      return;
    }
    if(waypoints_.empty()) 
    {
      ROS_WARN("No more waypoints to follow.");
      path_ready_ = false;
      return;
    }

    ROS_INFO("Following path with # %d waypoints...", (int)waypoints_.size());
    tf::StampedTransform transform;
    try
    {
      listener_.lookupTransform(frame_id_, base_frame_id_, ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      return;
    }
    double x1 = transform.getOrigin().x();
    double y1 = transform.getOrigin().y();
    double x2 = waypoints_[0].pose.pose.position.x;
    double y2 = waypoints_[0].pose.pose.position.y;
    double theta = atan2(y2 - y1, x2 - x1);
    tf2::Quaternion temp; 
    temp.setRPY(0, 0, theta);
    waypoints_[0].pose.pose.orientation.x = temp.getX();
    waypoints_[0].pose.pose.orientation.y = temp.getY();
    waypoints_[0].pose.pose.orientation.z = temp.getZ();
    waypoints_[0].pose.pose.orientation.w = temp.getW();

    // we have waypoints, let's follow them!
    while (!waypoints_.empty() && ros::ok()) 
    {
      geometry_msgs::PoseWithCovarianceStamped goal = waypoints_.front();
      if(dyn_config_.patrol_mode)
      {

      }
      else
      {
        // not in patrol mode - forget about this waypoint
        waypoints_.erase(waypoints_.begin());
      }
      send_move_base_goal(goal);
      if(!(distance_tolerance_ > 0.0)) 
      {
        // just wait until move_base reaches the goal...
        if(move_base_client_ != NULL) move_base_client_->waitForResult();
        ROS_INFO("Waiting for %f seconds before proceeding to the next goal...", wait_duration_);
        ros::Duration(wait_duration_).sleep();
      }
      else
      {
        /**
        raise NotImplementedError("distance_tolerance not implemented yet.")

        TODO - Rewrite
        This is the loop which exist when the robot is near a certain GOAL point.
        */
        double distance = 10.0;
        while(distance > distance_tolerance_)
        {
          try
          {
            listener_.lookupTransform(frame_id_, base_frame_id_, ros::Time(0), transform);
          }
          catch (tf::TransformException &ex) 
          {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
          }
          distance = sqrt(
            pow(goal.pose.pose.position.x - transform.getOrigin().x(), 2) + 
            pow(goal.pose.pose.position.y - transform.getOrigin().y(), 2)
            );
          ros::spinOnce();
        }
      }
      // current waypoint has now been reached! move on to the next waypoint on the next
      // iteration
      this->pub_waypoints_list();
      ros::spinOnce();
      loop_rate_->sleep();
       
    }
  }

  void FollowWayPoints::run()
  {
    while (ros::ok()) 
    {
      this->run_once();
      ros::spinOnce();
    }
  }

  geometry_msgs::PoseWithCovarianceStamped FollowWayPoints::to_pose_with_Cov(geometry_msgs::PointStamped point)
  {
    geometry_msgs::PoseWithCovarianceStamped pose;
    pose.header = point.header;
    pose.pose.pose.position = point.point;
    geometry_msgs::Quaternion angle;
    angle.x = 0;
    angle.y = 0;
    angle.z = 0;
    angle.w = 1;
    pose.pose.pose.orientation = angle;
    return pose;
  }

  geometry_msgs::PoseArray FollowWayPoints::toPoseArray(std::vector<geometry_msgs::PoseWithCovarianceStamped> waypoints)\
  {
    geometry_msgs::PoseArray poses;
    poses.header.frame_id  = frame_id_;
    for(int i = 0; i < waypoints_.size(); i++)
      poses.poses.insert(poses.poses.begin() , waypoints_[i].pose.pose);
    return poses;
  }

}