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

    Pose2d operator*(const Pose2d& t)const
    {
      Pose2d rpos=t.rotate(theta());
      return Pose2d(x()+rpos.x(),y()+rpos.y(),rpos.theta());
    }

    bool operator<=(const Pose2d& target)
    {
      return x()<=target.x() && y()<target.y() && theta()<target.theta();
    }

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


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

    const float gridient()const;

    static float distance(const Pose2d& pose1,const Pose2d& pose2);
    static Pose2d relativePose(const Pose2d& world_pose,const Pose2d& axis_pose);
    //返回值[0,2PI)
    static float vector2yaw(float x,float y);

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




#endif //LIO_LIVOX_CPP_MPC_POSE2D_H_