//
// 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;
  int n=int(m_theta/(2*M_PI));
  if(m_theta<=-M_PI)
  {
    m_theta += 2*M_PI*(n+1);
  }
  if(m_theta>M_PI)
  {
    m_theta -= 2*M_PI*(n+1);
  }
}

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 ) //2 3象限
    yaw= M_PI-yaw;

  if(yaw>M_PI)
    yaw-=M_PI*2;
  if(yaw<=-M_PI)
    yaw+=M_PI*2;

  return 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);
  }
  //std::cout<<"---------target:"<<target_pose<<std::endl;
  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<=-M_PI)
    new_theta=new_theta+2*M_PI;

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

  return Pose2d(x,y,new_theta);
}

Pose2d Pose2d::rotate(float ox,float oy,float theta)const{

  Pose2d translate(x()-ox,y()-oy,m_theta);//平移
  Pose2d rotated=translate.rotate(theta);

  Pose2d ret(rotated.x()+ox,rotated.y()+oy,rotated.theta());
  return ret;
}