//Node that makes the robot goto to point

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "gazebo_msgs/ModelStates.h"
#include <sstream>
#include <tf/tf.h>
#include <math.h>


ros::Publisher velocity_publisher; //publishes the robot velocity
ros::Subscriber pose_subscriber;  //subscribes the robot pose
//gazebo_msgs::ModelStates modelstate;

double x;
double y;
double z;
double w;

int state = 1;

const double PI = 3.14159265359;

using namespace std;

//method to move the robot straight with a certain speed and a certain direction
void move(double speed, double distance, bool isForward);

//method to rotate the robot
void rotate(double angular_speed, double angle, bool clockwise);

//method to convert angles from degrees to radians
double degrees_to_radians(double angle_in_degrees);

//method for the orientation of the rotation
void setDesiredOrientation (double desired_angle_radians);

//method that to update the robot position
void ModelStatecallback(const gazebo_msgs::ModelStates::ConstPtr& msg);

//GoTo function
void GoTo (double x_final, double y_final);


//main function, calls the necessary complementary functions
int main(int argc, char **argv)
{

    //Initialize ROS node
    ros::init(argc, argv, "moveto");
    ros::NodeHandle n;

    double speed;
    double distance;
    bool isForward;

  double angular_speed;
  double angle;
  bool clockwise;

  double x_final;
  double y_final;


  velocity_publisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); //publisher for velocities
  pose_subscriber = n.subscribe("/gazebo/model_states", 100, ModelStatecallback);  //subscriber to robot pose

  //test funcion move
  /*cout<<"enter speed: ";
    cin>>speed;
    cout<<"enter distance: ";
    cin>>distance;
    cout<<"forward: ";
  cin>>isForward;
  move (speed, distance, isForward);*/

  //test void rotate
  /*cout<<"enter angular speed: ";
  cin>>angular_speed;
  cout<<"enter angle: ";
  cin>>angle;
  cout<<"clockwise: ";
  cin>>clockwise;
  rotate (degrees_to_radians(angular_speed), degrees_to_radians(angle), clockwise);*/

  /*setDesiredOrientation (degrees_to_radians(120));
  ros::Rate loop_rate(0.5);
  loop_rate.sleep();
  setDesiredOrientation (degrees_to_radians(-60));
  loop_rate.sleep();
  setDesiredOrientation (degrees_to_radians(0));*/

  cout << "xfinal: " << endl;
  cin >> x_final;
  cout << "yfinal: " << endl;
  cin >> y_final;

  ros::Rate loop_rate (0.5);
  GoTo (x_final, y_final);
  loop_rate.sleep();

  ros::spin();
  return 0;
}

/*void move(double speed, double distance, bool isForward)  //makes the robot move on a straith line
{
    geometry_msgs::Twist vel_msg;

  //checking the direction of movement, notice that robt initial position is (0,0,0), with orientation in the y axis
    if (isForward)
        vel_msg.linear.y =-abs(speed);

    else
        vel_msg.linear.y = abs(speed);

  //Set all vels to zero
    vel_msg.linear.x=0;
    vel_msg.linear.z=0;

    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 0;

  //Time variables to know when the robot will stop (distance=speed*time)
    double t0 = ros::Time::now().toSec();
  double current_distance=0.0;
    ros::Rate loop_rate(10);

  //Cycle for the movement
  do{
        velocity_publisher.publish(vel_msg);
        double t1=ros::Time::now().toSec();
    current_distance = speed * (t1-t0);
    //current_distance = distance - abs(modelstate.pose.position.y);
    ros::spinOnce();
        loop_rate.sleep();
  }while (current_distance < distance);

  //When finished make velocity zero to stop the robot at the desired location
  vel_msg.linear.y = 0;
    velocity_publisher.publish(vel_msg);

  cout << x << y << z << w << endl;

  //cout<<"pos: "<<modelstate.pose.position.y << endl;
}*/

/*void rotate(double angular_speed, double angle, bool clockwise) {

  geometry_msgs::Twist vel_msg;

  //Set linear vels to zero
  vel_msg.linear.x=0;
  vel_msg.linear.y=0;
  vel_msg.linear.z=0;

  vel_msg.angular.x=0;
  vel_msg.angular.y=0;

  if (clockwise)
    vel_msg.angular.z = -abs(angular_speed);

  else
    vel_msg.angular.z = abs(angular_speed);

  double current_angle = 0.0;
  double t0 = ros::Time::now().toSec();
  ros::Rate loop_rate(10);

  do{
    velocity_publisher.publish(vel_msg);
    double t1=ros::Time::now().toSec();
    current_angle = angular_speed * (t1-t0);
    //ros::spinOnce();
    loop_rate.sleep();
  }while (current_angle < angle);

  vel_msg.angular.z = 0;
  velocity_publisher.publish(vel_msg);

}*/

double degrees_to_radians(double angle_in_degrees){
  return angle_in_degrees * (PI/180);

}

void ModelStatecallback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
  x = msg->pose[1].position.x;
  y = msg->pose[1].position.y;
  z = msg->pose[1].position.z;
  w = ((tf::getYaw(msg->pose[1].orientation)));
  //cout << y << endl;

}

/*void setDesiredOrientation (double desired_angle_radians)
{
  double relative_angle_radians = desired_angle_radians - w;
  bool clockwise = ((relative_angle_radians<0)?true:false);
  cout << desired_angle_radians << "," << w << "," << relative_angle_radians << endl;
  rotate (abs(relative_angle_radians)/4, abs(relative_angle_radians), clockwise);
}*/


void GoTo (double x_final, double y_final)
{

  ros::Rate loop_rate(100);
  geometry_msgs::Twist vel_msg;

  vel_msg.linear.x=0;
  vel_msg.linear.z=0;
  vel_msg.linear.y=0;
  vel_msg.angular.x = 0;
  vel_msg.angular.y = 0;
  vel_msg.angular.z = 0;

  double dist;

  dist = sqrt ( pow (x_final - x, 2) + pow(y_final - y, 2) );

  //while(1) {cout << y << endl;}

  switch (state) {


    case 1:   //move



     do{
      dist = sqrt ( pow (x_final - x, 2) + pow(y_final - y, 2) );

      //I_dist = I_dist + error_dist*0.5;
      vel_msg.linear.y = 0.25*dist;

      velocity_publisher.publish(vel_msg);

      ros::spinOnce();
      loop_rate.sleep();

     cout << y << endl;
    }while (dist < 0.5);

      if (dist < 0.5){state = 2;}



      break;

    case 2:
    vel_msg.linear.y = 0;
    velocity_publisher.publish(vel_msg);



  }


}