#ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#include <ros/ros.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <iostream>

namespace mkt_algorithm
{

/**
 * @class NoSolution
 */
class NoSolution : public std::exception {
  public:
    const char * what () const throw () {
      return "Robot cannot calculate error line!";
    }
}; // class NoSolution

/**
 * @class LineGenerator
 */
class LineGenerator 
{
  /* data */
  struct LineEquation {
    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
    robot_geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
    float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
  };

  /* Save data present position */
  struct PresentPosition {
    float Gxh, Gyh;             /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot  */ 
    float Yaw_degree;           /* Angle of Robot (degree)              */
    float Yaw_rad;              /* Angle of Robot (degree)              */
    float error_line;           /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2)       */
    double distance_to_goal;    /* The distance from query pose to goal */
    double distance_to_start;   /* The distance from query pose to start*/
    double distace_start_goal;
  };
public:

  /**
   * @brief To caculated the equation of the line 
   * @param start_point   start point of the line
   * @param end_point     end point of the line
   */
  virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);

  /**
   * @brief Calculating error 
   * @param[in] Lm          The distance from the center of the wheel axis to the center line
   * @param[in] pose
   * @param[in] rev         
   */
  virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);

  /**
   * @brief Calculating tolerance 
   * @param query_pose The pose to check
   * @param goal_pose The pose to check against
   */
  virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);

  /* Properties */
  LineEquation      Leq_; 
  PresentPosition   present_pose_;

}; // class LineGenerator 

} // namespace mkt_algorithm

#endif // __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__