//
// 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 <vector>
#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();

      if(theta<=-M_PI)
        theta+=2*M_PI;

      if(theta>M_PI)
        theta-=2*M_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,PI),下
      if(theta<=-M_PI)
        theta+=2*M_PI;

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

      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)const
    {
      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)const
    {
      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);


    const float x() const {return m_x;};
    const float y() const {return m_y;};
    const 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);
    //返回值(-PI,PI]
    static float vector2yaw(float x,float y);

    static std::vector<Pose2d> generate_rectangle_vertexs(const Pose2d& pose,float w,float h){
      std::vector<Pose2d> poses;
      //std::cout<<" obs:"<<pose<<", oprator* "<<pose*Pose2d(w/2,h/2,pose.theta())<<",wh:"<<w<<","<<h<<std::endl;
      poses.push_back(pose*Pose2d(w/2,h/2,pose.theta()));
      poses.push_back(pose*Pose2d(w/2,-h/2,pose.theta()));
      poses.push_back(pose*Pose2d(-w/2,h/2,pose.theta()));
      poses.push_back(pose*Pose2d(-w/2,-h/2,pose.theta()));
      poses.push_back(pose*Pose2d(-w/2,0,pose.theta()));
      poses.push_back(pose*Pose2d(w/2,0,pose.theta()));
      poses.push_back(pose*Pose2d(0,-h/2.0,pose.theta()));
      poses.push_back(pose*Pose2d(0,h/2.0,pose.theta()));
      return poses;
    }

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




#endif //LIO_LIVOX_CPP_MPC_POSE2D_H_