#include "ros/ros.h"
#include "ros/rate.h"
#include "nav_msgs/Odometry.h"
#include "tf2_ros/transform_listener.h"
#include "tf/tf.h"
#include "geometry_msgs/PoseStamped.h"
#include <math.h>
#include <stdlib.h>
#define PI 3.14159265358979323846
const float delta = PI / 8;
const float near_Dist = 0.5;
const float stop_Dist = 0.15;

float destinationLocation[2];
float currentLocation[2];
bool flag;
double curr_Yaw = 0.0;
double dest_Yaw = 0.0;

void goalCallback(const geometry_msgs::PoseStamped &recvDest)
{
    destinationLocation[0] = recvDest.pose.position.x;
    destinationLocation[1] = recvDest.pose.position.y;
    flag = false;
}

// funkcija povratnog poziva
void poseMessageReceived (const nav_msgs::Odometry::ConstPtr& msg)
{
    currentLocation[0] = msg->pose.pose.position.x;
    currentLocation[1] = msg->pose.pose.position.y;

    tf::Quaternion q(
      msg->pose.pose.orientation.x,
      msg->pose.pose.orientation.y,
      msg->pose.pose.orientation.z,
      msg->pose.pose.orientation.w
    );

    tf::Matrix3x3 m(q);

    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
    curr_Yaw = yaw;
}

int main (int argc, char** argv) {
  // Inicijaliziraj ROS i postani čvor
  ros::init (argc, argv, "zadaca2");
  ros::NodeHandle nh;

  // Kreiraj pretplatnički objekt
  // Parametri su: ime_teme, veličina_spremnika, funkcija_povratnog_poziva
  ros::Subscriber subGoal = nh.subscribe("move_base_simple/goal", 1000, &goalCallback);
  ros::Subscriber subOdom = nh.subscribe("odom", 1000, &poseMessageReceived);

  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
  ros::Rate rate(20);
  geometry_msgs::Twist movement;
  dest_Yaw = 0.0;
  float past_Distance = 0.0;
  float curr_Distance = 0.0;
  flag = true;

  while(nh.ok()){

    if(flag){
      rate.sleep();
      ros::spinOnce();
      continue;
    }else{
        double dx, dy;
        dx = destinationLocation[0] - currentLocation[0];
        dy = destinationLocation[1] - currentLocation[1];
        dest_Yaw = std::atan2(dy, dx);
        float diff_Yaw =  dest_Yaw - curr_Yaw;
                ROS_INFO("yaw diff 1: %.2f", dest_Yaw - curr_Yaw);
        while(diff_Yaw > PI) diff_Yaw -= 2*PI;
        while(diff_Yaw < -PI) diff_Yaw += 2*PI;
                ROS_INFO("yaw diff 2: %.2f", dest_Yaw - curr_Yaw);


        past_Distance = curr_Distance;
        curr_Distance = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
        movement.linear.y = 0.0;
        movement.linear.z = 0.0;
        movement.angular.x = 0.0;
        movement.angular.y = 0.0;

        //Okreni se u mjestu ukoliko je razlika kuteva veća od "delta"
        if(diff_Yaw > delta && curr_Distance > stop_Dist)
        {
          movement.linear.x = 0.0;
          ROS_INFO("yaw > 0.015");
          movement.angular.z = 0.5 * std::abs(diff_Yaw);
        }
        else if (diff_Yaw < -delta && curr_Distance > stop_Dist)
        {
          movement.linear.x = 0.0;
          ROS_INFO("yaw < 0.015");
          movement.angular.z = -0.5 * std::abs(diff_Yaw);
        }
        //Ukoliko razlika kuteva nije veća od "delta"
        else
        {
          if(curr_Distance > near_Dist)
          {
            movement.linear.x = 0.4;
            ROS_INFO("DISTANCE > THRESHOLD_DISTANCE");
            movement.angular.z = (std::abs(diff_Yaw)<=2) ? 0.5 * (diff_Yaw) : 1;
          }
          else if(curr_Distance > stop_Dist)
          {
            movement.linear.x = 0.2;
            ROS_INFO("DISTANCE > STOP_DISTANCE");
            movement.angular.z = (std::abs(diff_Yaw)<=2) ? 0.5 * (diff_Yaw) : 1;
          }
          else
          {
            ROS_INFO("Current distance good. STOP.");
            movement.linear.x = 0.0;
            movement.angular.z = 0.0;
          }
        }

        ROS_INFO("X: %.2f, Y: %.2f", currentLocation[0], currentLocation[1]);
        ROS_INFO("yaw diff: %.2f", dest_Yaw - curr_Yaw);
        ROS_INFO("Dest.yaw: %.2f, current.yaw: %.2f", dest_Yaw, curr_Yaw);
        ROS_INFO("DISTANCE: %.2f, SPEED: %.2f\n", curr_Distance, movement.linear.x);

        pub.publish(movement);
        rate.sleep();

        ros::spinOnce();
    }
}

  return 0;
}