#include "mkt_plugins/equation_line.h"
#include <robot/console.h>
#include <cmath>

namespace mkt_plugins
{

bool LineGenerator::calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point)
{
  /**
   *  Phương trình đường thẳng có dạng: 
   *  (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
   */
  Leq_.Gi = start_point; Leq_.Gj = end_point;
  Leq_.a  = (Leq_.Gj.y - Leq_.Gi.y);
  Leq_.b  = (Leq_.Gi.x - Leq_.Gj.x);
  Leq_.c  = (Leq_.Gj.x * Leq_.Gi.y - Leq_.Gi.x * Leq_.Gj.y);

  return Leq_.a != 0 || Leq_.b !=0;
}

void LineGenerator::calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev)
{
  float pha = rev? M_PI : 0;
  float Yaw_rad   = pose.theta - pha;

/*
 * Tính tọa độ điểm R
*/
  float G_xr       = float(pose.x);
  float G_yr       = float(pose.y);

/*
 * Tính tọa độ điểm H
*/
  present_pose_.Gxh = G_xr + Lm * cos(Yaw_rad);
  present_pose_.Gyh = G_yr + Lm * sin(Yaw_rad);

/**
 *  Phương trình đường thẳng (rh) có dạng: 
 *  (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
*/  
  float A_rh = (present_pose_.Gyh - G_yr);
  float B_rh = (G_xr - present_pose_.Gxh);
  float C_rh = (present_pose_.Gxh * G_yr - G_xr * present_pose_.Gyh);
/*
 * Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
 * và đi qua Gh có dạng: 
 * -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0
*/
  float A_he = -B_rh;
  float B_he = A_rh;
  float C_he = (-B_rh) * (-present_pose_.Gxh) + A_rh * (-present_pose_.Gyh);
/*
 * Đường thẳng (he) giao với đường thẳng (ij) tại e 
 * vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên:
*/
  float D = A_he * Leq_.b -  Leq_.a * B_he;
  float Dx = (-C_he) * Leq_.b - (-Leq_.c) * B_he;
  float Dy = A_he * (-Leq_.c) - Leq_.a * (-C_he);
/*
 * Tính tọa độ điểm E
*/
  if(D == 0) {
    if(Dx + Dy == 0) 
    {
/*
 * Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0
*/
      present_pose_.error_line = 0;
    }
    else throw NoSolution();
  }
  else 
  {
    float G_xe = Dx/D;
    float G_ye = Dy/D;
/*
 * Tính khoảng cách từ điểm E đến đường thẳng (RH)
*/
    present_pose_.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2)));
  }
  // present_pose_.error_line        = (Leq.a * present_pose_.Gxh + Leq.b * present_pose_.Gyh + Leq.c) / (sqrt(pow(Leq.a, 2) + pow(Leq.b, 2)));
  present_pose_.distance_to_goal      = sqrt(pow((Leq_.Gj.x - G_xr),2) + pow((Leq_.Gj.y - G_yr),2));  
  present_pose_.distance_to_start    = sqrt(pow((Leq_.Gi.x - G_xr),2) + pow((Leq_.Gi.y - G_yr),2));

 
}

double LineGenerator::calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose)
{
  /**
   *  Phương trình đường thẳng có dạng: 
   *  (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
   */
  // LineEquation Leq;
  // Leq.Gi = query_pose; Leq.Gj = goal_pose;
  // Leq.a  = (Leq.Gj.y - Leq.Gi.y);
  // Leq.b  = (Leq.Gi.x - Leq.Gj.x);
  // Leq.c  = (Leq.Gj.x * Leq.Gi.y - Leq.Gi.x * Leq.Gj.y);

  /**
   * Phương trình đường thẳng vuông góc
  */
  LineEquation Leq_l;
  Leq_l.a = -Leq_.b;
  Leq_l.b = Leq_.a;
  Leq_l.c = Leq_.b * goal_pose.x - Leq_.a * goal_pose.y;

  present_pose_.distance_to_goal     = sqrt(pow((Leq_.Gj.x - query_pose.x),2) + pow((Leq_.Gj.y - query_pose.y),2));  
  present_pose_.distance_to_start    = sqrt(pow((Leq_.Gi.x - query_pose.x),2) + pow((Leq_.Gi.y - query_pose.y),2));
  present_pose_.distace_start_goal   = sqrt(pow((Leq_.Gj.x - Leq_.Gi.x),2) + pow((Leq_.Gj.y - Leq_.Gi.y),2)); 
  // ROS_INFO_THROTTLE(0.2, "Goal checker %f %f %f %f %f", Leq_.Gj.x, Leq_.Gj.y, Leq_.Gj.theta, query_pose.x, query_pose.y);
 
  return (Leq_l.a * query_pose.x + Leq_l.b * query_pose.y + Leq_l.c ) / (sqrt(pow(Leq_l.a, 2) + pow(Leq_l.b, 2)));
}

} // namespace mkt_plugins
