//
// Created by zx on 2019/8/31.
//

#include "FourthTrajectory.h"

FourthTrajectory::FourthTrajectory()
{

}
FourthTrajectory::~FourthTrajectory()
{}

bool FourthTrajectory::Make(tf::Transform t0, tf::Transform t1, int extend_num)
{
    position0_=t0;
    position1_=t1;

    double angle =gen_axis_angle();

    tf::Transform pose1=Rotate_pose(position0_,angle);
    tf::Transform pose2=Rotate_pose(position1_,angle);
    double gradient1=get_gradient_by_tf(pose1);
    double gradient2=get_gradient_by_tf(pose2);

    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
    //三阶多项式差值，生成 曲线
    double x0=pose1.getOrigin().getX();
    double y0=pose1.getOrigin().getY();
    double x1=pose2.getOrigin().getX();
    double y1=pose2.getOrigin().getY();

    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;

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

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

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

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

    A(4,0)=12*pow(x1,2.0);
    A(4,1)=6.0*x1;
    A(4,2)=2.0;

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

    double a=P(0,0);
    double b=P(1,0);
    double c=P(2,0);
    double d=P(3,0);
    double e=P(4,0);
    //生成离散点,间距 step (m)

    boost::mutex::scoped_lock lock(mutex_trajectory_);
    trajectory_.clear();

    double delta=0.05;
    double x=x0;
    // added by yct
    int extend_front_count = 0;
    int extend_end_count = 0;
    int sign = 1;
    //modified by yct
    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
    {
        double y=a*pow(x,4.0)+b*pow(x,3.0)+c*x*x+d*x+e;
        double gradient=4.0*a*pow(x,3.0)+3.0*b*x*x+2.0*c*x+d;

        //计算x处的角度变化率
        /*double gradtheta=threta_gradient(x);
        if(fabs(gradtheta)>1.0/distance)
        {
            trajectory_.clear();
            return false;
        }*/

        tf::Transform pose;
        pose.setOrigin(tf::Vector3(x,y,0));
        pose=get_tf(x,y,gradient);

        // modified by yct
        // 开头逆向移动x到扩展头，再开始入队.
        if (extend_num > 0)
        {
            if(extend_front_count <= extend_num)
            {
                extend_front_count++;
                sign = -1;
            }else{
                sign = 1;
                trajectory_.push_back(pose);
                if(x>=x1 && extend_end_count <= extend_num){
                    extend_end_count++;
                }
            }
        }
        else{
            trajectory_.push_back(pose);
        }

        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
        // modified by yct
        x+=delta * sign;
    }

    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
    if (extend_num <= 0)
    {
        double x_last = x1;
        double y_last = a * pow(x_last, 4.0) + b * pow(x_last, 3.0) + c * x_last*x_last + d*x_last+e;
        double gradient_last = 4.0 * a * pow(x_last, 3.0) + 3.0 * b * x_last*x_last + 2.0*c*x_last+d;
        tf::Transform pose_last;
        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
        pose_last = get_tf(x_last, y_last, gradient_last);
        trajectory_.push_back(pose_last);
    }

    //将轨迹点反变换到原坐标系
    //tf::Transform tf_inv=axis_transform_.inverse();
    for(int i=0;i<trajectory_.size();++i)
    {
        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
    }
    return true;
}
