//
// Created by zx on 22-12-1.
//
//
// Created by zx on 2020/9/9.
//
#include <iostream>
#include "trajectory.h"

Trajectory::Trajectory(){}

Trajectory::Trajectory(const std::vector<Pose2d>& points)
{
  m_pose_vector=points;
}

Pose2d& Trajectory::operator[](int index)
{
  return m_pose_vector[index];
}
Trajectory& Trajectory::operator+(const Trajectory& other)
{
  m_pose_vector.insert(m_pose_vector.end(),
                       other.m_pose_vector.begin(),other.m_pose_vector.end());
  return *this;
}

void Trajectory::push_point(const Pose2d& point)
{
  m_pose_vector.push_back(point);
}

/*
 * 根据梯度和方向计算角度,x轴正，逆时针为正
 */
double gradient2theta(double gradient,bool direction)
{
  //计算方向 在第几象限
  int Quadrant=1;
  if(gradient>0)
  {
    if(direction)
      Quadrant=1;
    else
      Quadrant=3;
  }
  else
  {
    if(direction)
      Quadrant=4;
    else
      Quadrant=2;
  }

  double angle=0;
  if(fabs(gradient)>=200.0)
  {
    angle=M_PI/2.0;
  }
  else
  {
    angle=atan(fabs(gradient));
  }
  // angle === (0,M_PI/2.0)
  if(Quadrant==1)
  {

  }
  if(Quadrant==2)
  {
    angle=M_PI-angle;
  }
  if(Quadrant==3)
  {
    angle+=M_PI;
  }
  if(Quadrant==4)
  {
    angle=-angle;
  }
  return angle;
}


double gen_axis_angle(const Pose2d& point1,const Pose2d& point2)
{

  double delta_x=point2.x()-point1.x();
  double delta_y=point2.y()-point1.y();
  double gradient=0;
  if(delta_x==0)
  {
    if(delta_y>0)
      gradient=200;
    else if(delta_y<0)
      gradient=-200;
    else
      return 0;
  }
  else
  {
    gradient=delta_y/delta_x;
  }

  return gradient2theta(gradient,delta_x>=0);
}


/*
 * 计算曲线P 在(x1,x2)的线积分
 * 默认以 （x2-x1）/100  为步长
 */
double integral_path(double x1,double x2,Eigen::MatrixXd P)
{
  double a=P(0,0);
  double b=P(1,0);
  double c=P(2,0);
  double d=P(3,0);
  double delta=(x2-x1)/100.0;

  double t=x1;
  double integral=0.0;
  //循环条件为 t位于 x1和x2 之间
  while((x2-t)*(t-x1)>=0)
  {
    t+=delta;
    double graidient=3.0*a*pow(t,2.0)+2.0*b*t+c;
    integral+=sqrt(delta*delta+graidient*graidient);
  }
  return integral;

}

Trajectory Trajectory::create_trajectory(const Pose2d& start,const Pose2d& end,int point_count)
{
  int extend_num=point_count/2;

  Trajectory trajectory;

  double angle =gen_axis_angle(start,end);

  Pose2d pose1=start.rotate(-angle);
  Pose2d pose2=end.rotate(-angle);

  double gradient1=pose1.gridient();
  double gradient2=pose2.gridient();

  //三阶多项式差值，生成 曲线
  double x0=pose1.x();
  double y0=pose1.y();
  double x1=pose2.x();
  double y1=pose2.y();

  Eigen::Matrix<double,4,4> A=Eigen::MatrixXd::Zero(4,4);    //系数矩阵A
  Eigen::Matrix<double,4,1> B=Eigen::MatrixXd::Zero(4,1);    //值矩阵B
  A(0,0)=pow(x0,3.0);
  A(0,1)=pow(x0,2.0);
  A(0,2)=x0;
  A(0,3)=1.0;

  A(1,0)=pow(x1,3.0);
  A(1,1)=pow(x1,2.0);
  A(1,2)=x1;
  A(1,3)=1.0;

  A(2,0)=3.0*pow(x0,2.0);
  A(2,1)=2.0*x0;
  A(2,2)=1.0;

  A(3,0)=3.0*pow(x1,2.0);
  A(3,1)=2.0*x1;
  A(3,2)=1.0;

  B(0,0)=y0;
  B(1,0)=y1;
  B(2,0)=gradient1;
  B(3,0)=gradient2;
  Eigen::MatrixXd P=A.inverse()*B;

  double a=P(0,0);
  double b=P(1,0);
  double c=P(2,0);
  double d=P(3,0);

  //步长
  double delta_x=(x1-x0)/float(point_count);
  //生成离散点,间距
  for(double x=x0-delta_x*extend_num;x<x1+delta_x*extend_num;x+=delta_x)
  {
    double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
    double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
    trajectory.push_point(Pose2d(x,y,gradient2theta(gradient)));
  }

  //将轨迹点反变换到原坐标系
  Trajectory trajectory_map;
  for(int i=0;i<trajectory.size();++i)
  {
    Pose2d point=trajectory[i];
    Pose2d new_point=point.rotate(angle);
    trajectory_map.push_point(new_point);
  }

  return trajectory_map;
}

Trajectory Trajectory::create_line_trajectory(const Pose2d& start,const Pose2d& end,float dt)
{

  Trajectory trajectory;
  double angle =gen_axis_angle(start,end);
  trajectory.push_point(Pose2d(start.x(),start.y(),angle));
  int point_count=int(Pose2d::distance(start,end)/dt);
  //生成离散点,间距
  Pose2d diff=end-start;
  float solution_x=diff.x()/float(point_count);
  float solution_y=diff.y()/float(point_count);
  for(int i=1;i<point_count;++i)
  {
    double x=start.x()+i*solution_x;
    double y=start.y()+i*solution_y;
    //printf(" point x :%f  y:%f\n",x,y);
    trajectory.push_point(Pose2d(x,y,angle));
  }
  trajectory.push_point(Pose2d(end.x(),end.y(),angle));
  return trajectory;
}


Trajectory Trajectory::path_smooth_slid(const std::vector<Pose2d>& path, int size)
{
  //判断参数合理
  if(path.size() <= 2*size+1)
    return path;

  Trajectory smoothed_path;
  std::vector<float> smoothed_path_x;
  std::vector<float> smoothed_path_y;
  std::vector<float> smoothed_path_theta;
  //处理xy坐标
  for(auto i = 0; i < path.size(); ++i)
  {
    if(i >= size && i < path.size() - size)
    {
      float x_sum = 0.0, y_sum =0.0;
      for(auto j = i - size; j < i + size + 1; ++j)
      {
        x_sum += path[j].x();
        y_sum += path[j].y();
      }
      smoothed_path_x.push_back(x_sum/float(2.0*size+1));
      smoothed_path_y.push_back(y_sum/float(2.0*size+1));
    }
    else
    {
      smoothed_path_x.push_back(path[i].x());
      smoothed_path_y.push_back(path[i].y());
    }
  }
  //处理theta角度
  for(auto i = 0; i < path.size(); ++i)
  {
    if(i >= size - 1 && i < path.size() - size)
    {
      Pose2d point1 = Pose2d(smoothed_path_x[i],smoothed_path_y[i],0.0);
      Pose2d point2 = Pose2d(smoothed_path_x[i+1],smoothed_path_y[i+1],0.0);
      double angle = gen_axis_angle(point1,point2);
      smoothed_path_theta.push_back((float)angle);
    }
    else
    {
      smoothed_path_theta.push_back(path[i].theta());
    }
  }
  //构建平滑后的返回路径
  for (int i = 0; i < path.size(); ++i)
  {
    smoothed_path.push_point(Pose2d(smoothed_path_x[i], smoothed_path_y[i], smoothed_path_theta[i]));
  }

  return smoothed_path;
}