#include <cmath>  // For M_PI.

#include "ros/ros.h"
#include "tf/tf.h"
#include "robot/robot.h"
#include "geometry_msgs/Twist.h"
#include "robot/rrt.h"
#include <vector>
#include "robot/obstacles.h"
#include "robot/waypoint.h"
#include <time.h>
#include "geometry_msgs/Pose.h"

#define success false
#define running true

Robot::Robot() :
  threshold_(0.1) {
}

void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
{
  ROS_INFO("got odometry");
  robot_pose_ = msg->pose.pose;
  ROS_INFO_STREAM("robot pose_x:" << robot_pose_.position.x << "pose_y:" << robot_pose_.position.y);

  if (execute_) 
  {
    followTheCarrot();
  }
}

void Robot::startExecutePath() 
{
  execute_start_time_ = ros::Time::now();
  execute_ = true;
}

void Robot::stopExecutePath() 
{
  geometry_msgs::Twist command;
  command.angular.z = 0;
  command.linear.x = 0;
  command_publisher_->publish(command);
  execute_ = false;
}

bool Robot::addPathService(robot_coordination::AddPath::Request& req, robot_coordination::AddPath::Response& res) 
{
  ROS_WARN("service addPath called");
  plan_.clear();
  ROS_INFO_STREAM("Plan has " << req.waypoints.size() << " points");
  for (int i =0; i < req.waypoints.size(); i++) {
    plan_.push_back(Waypoint(req.waypoints[i].pose, req.waypoints[i].timepoint));
    ROS_DEBUG_STREAM("added waypoint (" <<
        req.waypoints[i].pose.position.x <<
        ", " <<
        req.waypoints[i].pose.position.y <<
        ")");
  }
  res.result = true;

  return true;
}

bool Robot::startMovementService(robot_coordination::StartMovement::Request& req, robot_coordination::StartMovement::Response& res) {
   startExecutePath();
   res.ack = true;
   return true;
}

bool Robot::stopMovementService(robot_coordination::StopMovement::Request& req, robot_coordination::StopMovement::Response& res) {
   stopExecutePath();
   res.ack = true;
   return true;
}

void Robot::setPath(const std::list<Waypoint>& path) {
  plan_ = path;
}

void Robot::setCommandPublisher(ros::Publisher * cmdpub) {
  command_publisher_ = cmdpub;
}

bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray)
{
    int nearestNodeID = myRRT.getNearestNodeID(tempNode.posX, tempNode.posY);
    RRT::rrtNode nearestNode = myRRT.getNode(nearestNodeID);

    double theta = atan2(tempNode.posY - nearestNode.posY,tempNode.posX - nearestNode.posX);

    tempNode.posX = nearestNode.posX + (rrtStepSize * cos(theta));
    tempNode.posY = nearestNode.posY + (rrtStepSize * sin(theta));

    if(checkIfInsideBoundary(tempNode) && checkIfOutsideObstacles(obstArray,tempNode))
    {
        tempNode.parentID = nearestNodeID;
        tempNode.nodeID = myRRT.getTreeSize();
        myRRT.addNewNode(tempNode);

        return true;
    }
    else
        return false;
}

void Robot::generateTempPoint(RRT::rrtNode &tempNode)
{
    int x = rand() % 150 + 1;
    int y = rand() % 150 + 1;
    tempNode.posX = x;
    tempNode.posY = y;
}

void Robot::addBranchtoRRTTree(RRT::rrtNode &tempNode, RRT &myRRT)
{
  geometry_msgs::Point point;
  point.x = tempNode.posX;
  point.y = tempNode.posY;
  point.z = 0;

  RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
  point.x = parentNode.posX;
  point.y = parentNode.posY;
  point.z = 0;
}

bool Robot::checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode)
{
    double distance = sqrt(pow(X-tempNode.posX,2)+pow(Y-tempNode.posY,2));
    if(distance < 3)
    {
        return true;
    }
    return false;
}

vector< vector<geometry_msgs::Point> > Robot::getObstacles(int pose_x, int pose_y)
{
    obstacles obst;
    return obst.getObstacleArray(pose_x, pose_y);
}

bool Robot::checkIfOutsideObstacles(vector< vector<geometry_msgs::Point> > &obstArray, RRT::rrtNode &tempNode)
{
    double AB, AD, AMAB, AMAD;

    for(int i=0; i<obstArray.size(); i++)
    {
        AB = (pow(obstArray[i][0].x - obstArray[i][1].x,2) + pow(obstArray[i][0].y - obstArray[i][1].y,2));
        AD = (pow(obstArray[i][0].x - obstArray[i][3].x,2) + pow(obstArray[i][0].y - obstArray[i][3].y,2));
        AMAB = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][1].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][1].y - obstArray[i][0].y)));
        AMAD = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][3].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][3].y - obstArray[i][0].y)));
        
        if((0 < AMAB) && (AMAB < AB) && (0 < AMAD) && (AMAD < AD))
        {
            return false;
        }
    }
    return true;
}

bool Robot::checkIfInsideBoundary(RRT::rrtNode &tempNode)
{
    if(tempNode.posX < 0 || tempNode.posY < 0  || tempNode.posX > 100 || tempNode.posY > 100 ) 
      return false;
    else 
      return true;
}

bool Robot::followTheCarrot() 
{
  if (plan_.empty()) 
  {
    geometry_msgs::Twist command;
    command.angular.z = 0;
    command.linear.x = 0;
    command_publisher_->publish(command);
    return true;
  } 
  else 
  {
    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) 
    {
      plan_.pop_front();
    }
    
    // rrt path planner
    Waypoint actual_goal = plan_.front();
    //initializing rrtTree
    RRT myRRT(robot_pose_.position.x, robot_pose_.position.y);
    int goalX = 0;
    int goalY = 0;
    goalX = actual_goal.pose.position.x;
    goalY = actual_goal.pose.position.y;

    int rrtStepSize = 3;

    vector< vector<int> > rrtPaths;
    vector<int> path;
    int rrtPathLimit = 1;

    int shortestPathLength = 9999;
    int shortestPath = -1;

    RRT::rrtNode tempNode;

    //Param input other robot current pose.x, pose.y
    vector< vector<geometry_msgs::Point> >  obstacleList = getObstacles(0.1, 0.1);

    bool addNodeResult = false, nodeToGoal = false;
    bool status = running;
    while(ros::ok() && status)
    {
        srand(time(NULL));
        if(rrtPaths.size() < rrtPathLimit)
        {
            generateTempPoint(tempNode);
            addNodeResult = addNewPointtoRRT(myRRT, tempNode, rrtStepSize, obstacleList);
            if(addNodeResult)
            {
                ROS_INFO("tempnode accepted");
                addBranchtoRRTTree(tempNode, myRRT);
                nodeToGoal = checkNodetoGoal(goalX, goalY, tempNode);
                if(nodeToGoal)
                {
                    path = myRRT.getRootToEndPath(tempNode.nodeID);
                    rrtPaths.push_back(path);
                    ROS_WARN_STREAM("New Path Found. Total paths " << rrtPaths.size());
                }
            }
        }
        else //if(rrtPaths.size() >= rrtPathLimit)
        {
            status = success;
            ROS_WARN("Finding Optimal Path !");
            for(int i=0; i<rrtPaths.size(); i++)
            {
                if(rrtPaths[i].size() < shortestPath)
                {
                    shortestPath = i;
                    shortestPathLength = rrtPaths[i].size();
                }
            }
        }
    }//end while

    //default method move origin to goal.
    #if 1
    Waypoint first;
    geometry_msgs::Pose temp_pose;
    first.pose = temp_pose;
    first.pose.position.x = myRRT.getNode(1).posX;
    first.pose.position.y = myRRT.getNode(1).posY;
    first.timepoint = ros::Duration(10.0);

    Waypoint temp_goal = first;
    const double dist  = temp_goal.distanceTo(robot_pose_);
    const double angle = temp_goal.angleTo(robot_pose_);
    tf::Quaternion q(
        robot_pose_.orientation.x,
        robot_pose_.orientation.y,
        robot_pose_.orientation.z,
        robot_pose_.orientation.w);
    tf::Matrix3x3 m(q);

    double roll;
    double pitch;
    double yaw;
    m.getRPY(roll, pitch, yaw);
    ROS_INFO("distance %f and angle %f to goal", dist,angle);
    ROS_INFO("robot roll %f , pitch %f yaw %f", roll, pitch, yaw);

    geometry_msgs::Twist command;
    double delta = angle - yaw;
    if (delta > M_PI ) {
      delta -= 2 * M_PI;
    }
    if (delta < -M_PI ) {
      delta += 2 * M_PI;
    }
    ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
    const double time_to_goal = ((execute_start_time_ + actual_goal.timepoint) - ros::Time::now()).toSec();
    ROS_INFO("time to goal %f ", time_to_goal);

    double speed;
    if (time_to_goal > 0) 
    {
      speed = dist/time_to_goal ;
    }
    else 
    {
      speed = 1;
    }
    command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
    command.angular.z = delta;
    command.linear.x = speed * (cos(delta) + 0.333 * cos(3 * delta));
    if (command.linear.x < 0) 
    {
      command.linear.x = 0;
    }
    command_publisher_->publish(command);
    #endif

    return false;
  }
}
