//
// Created by zx on 22-12-1.
//

#ifndef LIO_LIVOX_CPP_MPC_POSE2D_H_
#define LIO_LIVOX_CPP_MPC_POSE2D_H_

#include <iostream>
#include <atomic>
#include <math.h>
/*
 * 带direction的二维点,与x轴正方向逆时针为正方向
 */

class Pose2d
{
 public:
    Pose2d();
    Pose2d(float x,float y,float theta);
    Pose2d(const Pose2d& point)
    {
      m_x=point.x();
      m_y=point.y();
      m_theta=point.theta();
    }
    ~Pose2d();
    Pose2d& operator=(const Pose2d& point)
    {
      m_x=point.x();
      m_y=point.y();
      m_theta=point.theta();
      return *this;
    }

    /*
     * 两位姿相减，角度在-PI ,PI
     */
    Pose2d operator-(const Pose2d& pose)const
    {

      float x=m_x-pose.x();
      float y=m_y-pose.y();
      float theta=m_theta-pose.theta();

      const double PI=3.14159265;
      if(theta<0)
        theta=theta+2*PI;

      if(theta>=2*PI)
        theta-=2*PI;

      if(theta>PI)
        theta=theta-2*PI;

      return Pose2d(x,y,theta);
    }
    Pose2d operator+(const Pose2d& pose)const
    {
      float x=m_x+pose.x();
      float y=m_y+pose.y();
      float theta=m_theta+pose.theta();
      //转换到 [-pi/2, pi/2]下
      int n=int(theta/(M_PI));
      if(theta<-M_PI/2)
      {
        theta += M_PI*(n+1);
      }
      if(theta>M_PI/2)
      {
        theta -= M_PI*(n+1);
      }
      return Pose2d(x,y,theta);
    }
    static Pose2d abs(const Pose2d& pose)
    {
      Pose2d ret(std::abs(pose.x()),std::abs(pose.y()),std::abs(pose.theta()));
      return ret;
    }
    bool operator<(const Pose2d& pose)const
    {
      return x()<pose.x()&&y()<pose.y()&&theta()<pose.theta();
    }

    /*
     * 将点顺时针旋转theta
     */
    Pose2d rotate(float theta)const;
    friend std::ostream& operator<<(std::ostream &out,const Pose2d& point);


    const float x() const {return m_x;};
    const float y() const {return m_y;};
    const float theta() const {return m_theta;}

    const float gridient()const;

    static float distance(const Pose2d& pose1,const Pose2d& pose2);
    static Pose2d PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose);

 protected:
    std::atomic<float>                      m_x;
    std::atomic<float>                      m_y;
    std::atomic<float>                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
};




#endif //LIO_LIVOX_CPP_MPC_POSE2D_H_
