#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_(1.3) 
{

}

void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
{
  //ROS_INFO("got odometry");
  robot_pose_ = msg->pose.pose;
  
  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_INFO_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, double 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);
        ROS_INFO("addNewPointtoRRT return true");
        return true;
    }
    else
        ROS_INFO("addNewPointtoRRT return false");
        return false;
}
float Robot::GetRand(  float fMin, float fMax  )
{
	assert( fMax > fMin );
	float fRand=0.0f, f1=0.0f;

	int n;
	float f;
	n = static_cast<float>(fMax-fMin);
	f = (fMax-fMin) - n;

	if ( n>0 )
		fRand = static_cast<float>( rand() % n );
	else
		fRand = 0.f;

	do 
	{
		f1 = static_cast<float>( rand() ) / RAND_MAX;
		f1 *= ( rand()%2==0 ? 1 : -1 );
	} while ( fRand+f1<0 || fRand+f1 > (fMax-fMin) );

	fRand += (fMin+f1);

	return fRand;
}


void Robot::generateTempPoint(RRT::rrtNode &tempNode)
{
    tempNode.posX = GetRand(0.3, 6.5);
    tempNode.posY = GetRand(0.3, 11.0);
    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() << ",tempNode.posX: " << tempNode.posX
                   <<",tempNode.posY:" << tempNode.posY);
}

bool Robot::checkNodetoGoal(double X, double Y, RRT::rrtNode &tempNode)
{
    double distance = sqrt(pow(X-tempNode.posX, 2)+pow(Y-tempNode.posY, 2));
    ROS_INFO_STREAM("checkNodetoGoal: input x: " << X << " ,y:" << Y << " ,tempNode.posX:"
                       <<tempNode.posX<<" ,y:"<<tempNode.posY);
    ROS_INFO_STREAM("distance:" << distance);

    if(distance < 1.5)
    {
        ROS_INFO("checkNodetoGoal return true");
        return true;
    }

    ROS_INFO("checkNodetoGoal return false");
    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);
    ROS_WARN_STREAM("All Plan Dest reached!");

    return true;
  } 
  else 
  {
    while (!plan_.empty() && plan_.front().distanceTo(robot_pose_) < threshold_) 
    {
      plan_.pop_front();
    }
    
    // rrt path planner
    Waypoint actual_goal = plan_.front();
    //ROS_WARN_STREAM("");


    //initializing rrtTree
    RRT myRRT(robot_pose_.position.x, robot_pose_.position.y);
    double goalX = 0;
    double goalY = 0;
    goalX = actual_goal.pose.position.x;
    goalY = actual_goal.pose.position.y;

    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<" ,current pose_x:" << double(robot_pose_.position.x) << " ,pose_y:" 
                    << double(robot_pose_.position.y) << " ,goal_x:" << goalX << " ,goal_y:" << goalY);

    double rrtStepSize = 0.5;

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

    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.01, 0.01);

    bool addNodeResult = false, nodeToGoal = false;
    bool status = running;
    srand(time(NULL));
    while(ros::ok() && status)
    {
        if(rrtPaths.size() < rrtPathLimit)
        {
            generateTempPoint(tempNode);
            addNodeResult = addNewPointtoRRT(myRRT, tempNode, rrtStepSize, obstacleList);
            if(addNodeResult)
            {
                //ROS_INFO("tempnode accepted");
                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()<<" ,NodeID:"<<tempNode.nodeID);
                }
            }
        }
        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(tempNode.nodeID).posX;
    first.pose.position.y = myRRT.getNode(tempNode.nodeID).posY;
    first.timepoint = ros::Duration(10.0);
    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"PATH first node x:"<<first.pose.position.x<<",y:"<<first.pose.position.y);
    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"CURRENT robot pose x:"<<robot_pose_.position.x<<",y:"<<robot_pose_.position.y);

    Waypoint temp_goal = first;
    double dist = 1;
    double angle = 0;

    //while(dist > 0.1)
    {
      dist  = temp_goal.distanceTo(robot_pose_);
      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_STREAM("Name:"<< ros::this_node::getName() <<"time to goal:"<<time_to_goal<<" ,dist:"<<dist);
      ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"CURRENT robot pose x:"<<robot_pose_.position.x<<",y:"<<robot_pose_.position.y);
      ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"Goal pose x:" << first.pose.position.x << ",y:" << first.pose.position.y);

      double speed;
      if (time_to_goal > 0) 
      {
        speed = dist/time_to_goal;
      }
      else 
      {
        speed = 0.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;
  }
}
