#include "make_trajectory.h"
#include <limits>

double get_pose_angle_z(tf::Transform pos)
{
    double yaw,a,b;
    pos.getBasis().getEulerZYX(yaw,a,b);
    return yaw;
}

double get_gradient_by_tf(tf::Transform pos)
{
    double angle=get_pose_angle_z(pos);
    double gradient=tanf(angle);
    if(fabs(gradient)>200)
        return 200.0*(gradient/fabs(gradient));
    return gradient;
}

tf::Transform get_tf(double x,double y,double gradient)
{
    double angle=0;
    if(fabs(gradient)>200.0)
    {
        angle=M_PI/2.0*(gradient/fabs(gradient));
    }
    else
    {
        angle=atan(fabs(gradient));
        angle=angle*(gradient/fabs(gradient));
    }
    tf::Matrix3x3 rotation;
    rotation.setRPY(0,0,angle);
    tf::Transform tf;
    tf.setBasis(rotation);
    tf.setOrigin(tf::Vector3(x,y,0));
    return tf;
}

tf::Transform transform_tf(tf::Transform pos,tf::Transform tf)
{
    pos=pos*tf;
    return pos;
}

tf::Pose Rotate_pose(tf::Pose pose,double angle)
{
    //先平移，后旋转
    double x=pose.getOrigin().getX();
    double y=pose.getOrigin().getY();

    double cs=cos(angle);
    double sn=sin(angle);
    double x1=cs*x+sn*y;
    double y1=-sn*x+cs*y;
    tf::Pose newpose;

    tf::Quaternion quat;
    double theta,a,b;
    pose.getBasis().getEulerZYX(theta,a,b);
    quat.setEulerZYX(theta-angle,a,b);
    newpose.setRotation(quat);
    newpose.setOrigin(tf::Vector3(x1,y1,0));
    return newpose;
}



///////////////////////////////////////////////////////////////////////////////////////////////////////////

MakeTrajectory::MakeTrajectory()
{
    left_tf_=tf::Transform(tf::Quaternion(0,0,0));
    right_tf_=tf::Transform(tf::Quaternion(0,0,0));
    
}
bool MakeTrajectory::TransformTrajectory(tf::Transform tf)
{

    return true;
}
tf::Transform MakeTrajectory::operator[](int n)
{
    boost::mutex::scoped_lock lock(mutex_trajectory_);
    return trajectory_[n];

}
bool MakeTrajectory::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,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;
    line_param_=P;

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

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

    //计算两轮间距/2
    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
                            pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
    distance+=0.00000001;

    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,3.0)+b*pow(x,2.0)+c*x+d;
        double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;

        //计算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, 3.0) + b * pow(x_last, 2.0) + c * x_last + d;
        double gradient_last = 3.0 * a * pow(x_last, 2.0) + 2.0 * b * x_last + c;
        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;
}
double MakeTrajectory::gen_axis_angle()
{

    double x=position1_.getOrigin().getX()-position0_.getOrigin().getX();
    double y=position1_.getOrigin().getY()-position0_.getOrigin().getY();
    tf::Vector3 var(x,y,0);
    double angle=atan2(y,x);

    //std::cout<<" var.angle : "<<angle*180.0/M_PI<<std::endl;
    return angle;

}

void MakeTrajectory::SetLRTransform(tf::Transform left,tf::Transform right)
{
    left_tf_=left;
    right_tf_=right;
}

bool MakeTrajectory::IsPoseInTrajectory(tf::Transform pose)
{
    boost::mutex::scoped_lock lock(mutex_trajectory_);
    if(trajectory_.size()==0)
        return false;

    double angle0=get_pose_angle_z(pose);
    double min_distance=std::numeric_limits<double>::max();
    tf::Transform min_point;
    for(int i=0;i<trajectory_.size();++i)
    {
        tf::Transform tf=trajectory_[i];
        double distance=sqrt(pow(pose.getOrigin().getX()-tf.getOrigin().getX(),2.0)+
                            pow(pose.getOrigin().getY()-tf.getOrigin().getY(),2.0));
        if(distance < min_distance)
        {
            min_distance=distance;
            min_point=tf;
        }
    }
    if(min_distance==std::numeric_limits<double>::max())
        return false;
    double angle1=get_pose_angle_z(min_point);
    double angle_distance=fabs(angle1-angle0);
    
    if(min_distance>0.05 || angle_distance*180.0/M_PI>10.0)
    {
        std::cout<<"distance :"<<min_distance<<"   angle distance : "<<angle_distance*180.0/M_PI<<std::endl;
        return false;
    }

    return true;
}

double MakeTrajectory::threta_gradient(double x)
{
    double a=line_param_(0,0);
    double b=line_param_(1,0);
    double c=line_param_(2,0);
    double d=line_param_(3,0);
    double S=1+pow(3*a*pow(x,2.0)+2*b*x+c,2.0);
    double s=6*a*x+2*b;
    return s/S;
}