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

#include "CppAD5Trajectory.h"
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include "Eigen/Core"


using CppAD::AD;

class FG5_eval {
public:
    double m_x0,m_x1;
    FG5_eval(double x0,double x1) {
        m_x0=x0;
        m_x1=x1;
    }

    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
    void operator()(ADvector& fg, const ADvector& vars) {
        AD<double> a=vars[0];
        AD<double> b=vars[1];
        AD<double> c=vars[2];
        AD<double> d=vars[3];
        AD<double> e=vars[4];
        AD<double> f=vars[5];

        /*AD<double> Fx1=20*a*a*CppAD::pow(m_x1,5)+30*a*b*CppAD::pow(m_x1,4)+(20*a*c+b*b)*CppAD::pow(m_x1,3)
            +(18*b*c+10*a*d)*CppAD::pow(m_x1,2)+(9*c*c+12*b*d)*m_x1+d*log(m_x1);
        AD<double> Fx0=20*a*a*CppAD::pow(m_x0,5)+30*a*b*CppAD::pow(m_x0,4)+(20*a*c+b*b)*CppAD::pow(m_x0,3)
            +(18*b*c+10*a*d)*CppAD::pow(m_x0,2)+(9*c*c+12*b*d)*m_x0+d*CppAD::log(m_x0);*/
        AD<double> Fx1=100/7.0*a*a*CppAD::pow(m_x1,7)+20*a*b*CppAD::pow(m_x1,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x1,5)
            +(9*b*c+5*a*d)*CppAD::pow(m_x1,4)+(3*c*c+4*b*d)*CppAD::pow(m_x1,3)+3*c*d*m_x1*m_x1+d*d;
        AD<double> Fx0=100/7.0*a*a*CppAD::pow(m_x0,7)+20*a*b*CppAD::pow(m_x0,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x0,5)
            +(9*b*c+5*a*d)*CppAD::pow(m_x0,4)+(3*c*c+4*b*d)*CppAD::pow(m_x0,3)+3*c*d*m_x0*m_x0+d*d;
        fg[0]=Fx1-Fx0;      //||二阶导数|| * ||x-x1|| 在(xo,x1)上积分

        fg[1]=a*CppAD::pow(m_x0,5)+b*CppAD::pow(m_x0,4)+c*CppAD::pow(m_x0,3)+d*CppAD::pow(m_x0,2)+e*m_x0+f; // ==y0
        fg[2]=a*CppAD::pow(m_x1,5)+b*CppAD::pow(m_x1,4)+c*CppAD::pow(m_x1,3)+d*CppAD::pow(m_x1,2)+e*m_x1+f; // ==y1

        fg[3]=5*a*CppAD::pow(m_x0,4)+4*b*CppAD::pow(m_x0,3)+3*c*CppAD::pow(m_x0,2)+2*d*m_x0+e ;  // gradient0
        fg[4]=5*a*CppAD::pow(m_x1,4)+4*b*CppAD::pow(m_x1,3)+3*c*CppAD::pow(m_x1,2)+2*d*m_x1+e ;  // gradient0

        fg[5]=10*a*CppAD::pow(m_x1,3)+6*b*CppAD::pow(m_x1,2)+3*c*m_x1+d ;         //  0



    }
};



CppAD5Trajectory::CppAD5Trajectory(){}
CppAD5Trajectory::~CppAD5Trajectory(){}
bool CppAD5Trajectory::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);

    if(Solve(t0,t1)==false) return false;

    std::cout<<"  solve param :"<<m_p;
    boost::mutex::scoped_lock lock(mutex_trajectory_);
    double x0=pose1.getOrigin().getX();
    double y0=pose1.getOrigin().getY();
    double x1=pose2.getOrigin().getX();
    double y1=pose2.getOrigin().getY();
    trajectory_.clear();
    double a=m_p[0];
    double b=m_p[1];
    double c=m_p[2];
    double d=m_p[3];
    double e=m_p[4];
    double f=m_p[5];
    //计算两轮间距/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,5.0)+b*pow(x,4.0)+c*pow(x,3.0)+d*pow(x,2.0)+e*x+f;
        double gradient=5.0*a*pow(x,4.0)+4.0*b*pow(x,3.0)+3.0*c*x*x+2.0*d*x+e;

        //计算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, 5.0) + b * pow(x_last, 4.0) + c * pow(x_last,3.0)
            + d*pow(x_last,2.0)+e*x_last+f;
        double gradient_last = 5.0 * a * pow(x_last, 4.0) + 4.0 * b * pow(x_last,3.0)
            + 3.0*c*pow(x_last,2.0)+2.0*d*x_last+e;
        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;
}

bool CppAD5Trajectory::Solve(tf::Pose t0,tf::Pose t1)
{
    bool ok=true;
    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();

    typedef CPPAD_TESTVECTOR(double) Dvector;
    size_t n_vars = 6;
    size_t n_constraints = 5;

    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars_lowerbound[i] = -1.0e19;
        vars_upperbound[i] = 1.0e19;
    }

    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);

    constraints_lowerbound[0] = y0;
    constraints_upperbound[0] = y0;

    constraints_lowerbound[1] = y1;
    constraints_upperbound[1] = y1;

    constraints_lowerbound[2] = gradient1;
    constraints_upperbound[2] = gradient1;

    constraints_lowerbound[3] = gradient2;
    constraints_upperbound[3] = gradient2;

    constraints_lowerbound[4] = 0;
    constraints_upperbound[4] = 0;

    // object that computes objective and constraints
    FG5_eval fg_eval(x0,x1);

    std::string options;
    options += "Integer print_level  0\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Numeric max_cpu_time          0.5\n";

    CppAD::ipopt::solve_result<Dvector> solution;
    std::cout<<" options :"<<options;
    CppAD::ipopt::solve<Dvector, FG5_eval>(
        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
        constraints_upperbound, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    std::cout<<" ok:"<<solution.status<<std::endl;
    auto cost = solution.obj_value;

    if(ok)
    {
        Eigen::VectorXd p(6);
        p<<solution.x[0],solution.x[1],solution.x[2],solution.x[3],solution.x[4],solution.x[5];
        std::cout<<" solve p : "<<p;
        m_p=p;
    }
    return ok;

}


