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

#include "pose2d.h"

Pose2d::Pose2d()
        :m_x(0),m_y(0),m_theta(0)
{
}
Pose2d::Pose2d(float x,float y,float theta)
        :m_x(x),m_y(y),m_theta(theta)
{
}

Pose2d::~Pose2d()
{}

std::ostream& operator<<(std::ostream &out,const Pose2d& point)
{
  out<<"[x:"<<point.x()<<", y:"<<point.y()<<", theta:"<<point.theta()*180.0/M_PI<<"°]";
  return out;
}
const float Pose2d::gridient() const
{
  double gradient=tanf(m_theta);
  if(fabs(gradient)>200)
    return 200.0*(gradient/fabs(gradient));
  return gradient;
}

float Pose2d::vector2yaw(float x,float y)
{
  float r=sqrt(x*x+y*y);
  if(r<1e-8)
    return 0;
  float yaw=asin(y/r);
  if(x>=0)  //1,4象限
    return yaw;
  if (x<0 ) //2 3象限
    return M_PI-yaw;

}

Pose2d Pose2d::relativePose(const Pose2d& target_pose,const Pose2d& axis_pose)
{
  Pose2d diff=target_pose-axis_pose;
  Pose2d nPose=diff.rotate(-axis_pose.theta());

  float new_theta=diff.theta();
  //转换到 [-pi, pi]下
  int n=int(new_theta/(2*M_PI));
  if(new_theta<-M_PI)
  {
    new_theta += 2*M_PI*(n+1);
  }
  if(new_theta>M_PI)
  {
    new_theta -= 2*M_PI*(n+1);
  }

  return Pose2d(nPose.x(),nPose.y(),new_theta);
}

float Pose2d::distance(const Pose2d& pose1,const Pose2d& pose2)
{
  Pose2d offset=pose1-pose2;
  return sqrt(offset.x()*offset.x()+offset.y()*offset.y());
}

Pose2d Pose2d::rotate(float theta)const
{
  double cs=cos(theta);
  double sn=sin(theta);
  float x=cs*this->x()-sn*this->y();
  float y=sn*this->x()+cs*this->y();
  float new_theta=this->theta()+theta;


  if(theta<0)
    new_theta=new_theta+2*M_PI;

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

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

  /*//转换到 [-pi/2, pi/2]下
  int n=int(new_theta/(M_PI));
  if(new_theta<-M_PI/2)
  {
    new_theta += M_PI*(n+1);
  }
  if(new_theta>M_PI/2)
  {
    new_theta -= M_PI*(n+1);
  }*/

  return Pose2d(x,y,new_theta);
}

Pose2d Pose2d::rotate(float ox,float oy,float theta)const{
  Pose2d translate(x()-ox,y()-oy,theta);//平移
  Pose2d rotated=rotate(theta);
  Pose2d ret(rotated.x()+x(),rotated.y()+y(),rotated.theta());
  return ret;
}