//
// 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>
/*
 * 带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;
    }
    Pose2d operator-(const Pose2d& pose)const
    {
      float x=m_x-pose.x();
      float y=m_y-pose.y();
      float theta=m_theta-pose.theta();
      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();
      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_