//
// Created by zx on 22-12-1.
//

#include "loaded_mpc.h"
#include <chrono>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

size_t N = 15;                  //优化考虑后面多少步
size_t delay = 2;  // 预判发送到执行的延迟，即几个周期的时间
size_t down_count = 3;//下发前多少步

size_t nx = 0;
size_t ny = nx + N;
size_t nth = ny + N;
size_t nv = nth + N;
size_t ndlt = nv + N;

size_t nobs = ndlt + N;

class FG_eval_half_agv {
public:
    // Fitted polynomial coefficients
    Eigen::VectorXd m_coeffs;                 //曲线方程
    Eigen::VectorXd m_statu;                  //当前状态
    Eigen::VectorXd m_condition;              //搜索条件参数
    bool directY_;
    FG_eval_half_agv(Eigen::VectorXd coeffs, Eigen::VectorXd statu, Eigen::VectorXd condition,bool directY) {
        m_coeffs = coeffs;
        m_statu = statu;
        m_condition = condition;
        directY_=directY;
    }

    typedef CPPAD_TESTVECTOR(CppAD::AD<double>) ADvector;

    void operator()(ADvector &fg, const ADvector &vars) {

        fg[0] = 0;
        double dt = m_condition[0];
        double ref_v = m_condition[1];
        double obs_w = m_condition[2];
        double obs_h = m_condition[3];
        double target_x = m_condition[4];
        double target_y = m_condition[5];

        double v = m_statu[0];
        double delta = m_statu[1];

        // Weights for how "important" each cost is - can be tuned
        const double y_cost_weight = 1000;
        const double th_cost_weight = 4000;
        const double v_cost_weight = 5000;
        const double vth_cost_weight = 1000;
        const double a_cost_weight = 1;
        const double ath_cost_weight = 10;
        const double obs_distance_weight = 5000.0;

        // Cost for CTE, psi error and velocity
        for (int t = 0; t < N; t++) {
            CppAD::AD<double> xt = vars[nx + t];
            CppAD::AD<double> fx = m_coeffs[0] + m_coeffs[1] * xt + m_coeffs[2] * pow(xt, 2) + m_coeffs[3] * pow(xt, 3);
            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t] - fx, 2);
            //fg[0]+=v_cost_weight*(CppAD::pow(vars[nx+t]-target_x,2)+CppAD::pow(vars[ny+t]-target_y,2));
            //目标速度loss
            fg[0] += v_cost_weight * CppAD::pow(vars[nv + t] - ref_v, 2);


        }

        for (int t = 0; t < N - 1; t++) {
            //角速度，加速度，角加速度 weight loss
            fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
            if(t > N / 3){//前中后三段角速度权重1：2：3
                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
            }
            if(t > N / 3 * 2){//前中后三段角速度权重1：2：3
                fg[0] += vth_cost_weight * CppAD::pow(vars[ndlt + t], 2);
            }
            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t + 1] - vars[nv + t], 2);
            fg[0] += ath_cost_weight * CppAD::pow(vars[ndlt + t + 1] - vars[ndlt + t], 2);
        }

        /////////////////////
        fg[1 + nx] = vars[nx] - vars[nv] * dt;
        fg[1 + ny] = vars[ny];
        //CppAD::AD<double> w0=vars[nv]/wheelbase*CppAD::tan(vars[ndlt]);
        fg[1 + nth] = vars[nth] - vars[ndlt] * dt;

        //位姿约束
        for (int t = 1; t < N; t++) {
            // State at time t + 1
            CppAD::AD<double> x1 = vars[nx + t];
            CppAD::AD<double> y1 = vars[ny + t];
            CppAD::AD<double> th1 = vars[nth + t];

            // State at time t
            CppAD::AD<double> x0 = vars[nx + t - 1];
            CppAD::AD<double> y0 = vars[ny + t - 1];
            CppAD::AD<double> th0 = vars[nth + t - 1];
            CppAD::AD<double> v0 = vars[nv + t - 1];

            // Setting up the rest of the model constraints
            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
            fg[1 + nth + t] = th1 - (th0 + vars[ndlt + t - 1] * dt);

        }
        //加速度和dlt约束
        fg[1 + nv] = (vars[nv] - v) / dt;
        fg[1 + ndlt] = (vars[ndlt] - delta) / dt;
        for (int t = 1; t < N; ++t) {
            fg[1 + nv + t] = (vars[nv + t] - vars[nv + t - 1]) / dt;
            fg[1 + ndlt + t] = (vars[ndlt + t] - vars[ndlt + t - 1]) / dt;
        }


        if (m_statu.size() == 2 + 16) {
            //与障碍物的距离
            for (int i = 0; i < 8; ++i) {
                double ox = m_statu[2 + i * 2];
                double oy = m_statu[2 + i * 2 + 1];
                for (int t = 0; t < N; ++t) {
                    /*第i点的矩形方程为:   {[(x-xi)cos(Θi)-(y-yi)sin(Θi)]/1.35}**8  +
                     *                  {[(x-xi)sin(Θi)+(y-yi)cos(Θi)]/0.75}**8  = 1.0
                     *
                     * */
                    CppAD::AD<double> ra = (ox - vars[nx + t]) * CppAD::cos(vars[nth + t]) -
                                           (oy - vars[ny + t]) * CppAD::sin(vars[nth + t]);
                    CppAD::AD<double> rb = (ox - vars[nx + t]) * CppAD::sin(vars[nth + t]) +
                                           (oy - vars[ny + t]) * CppAD::cos(vars[nth + t]);
                    if(!directY_)
                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / obs_w, 8) + CppAD::pow(rb / (obs_h*0.76), 8);
                    else
                        fg[1 + nobs + N * i + t] = CppAD::pow(ra / (obs_w*0.76), 8) + CppAD::pow(rb / obs_h, 8);
                }
            }
        }

    }
};

LoadedMPC::LoadedMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity) {
    min_velocity_ = min_velocity;
    max_velocity_ = max_velocity;
    obs_relative_pose_ = obs;
    obs_w_ = obs_w;
    obs_h_ = obs_h;
}

LoadedMPC::~LoadedMPC() {}

MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target, Eigen::VectorXd statu, MPC_parameter mpc_param,
                          std::vector<double> &out, Trajectory &select_traj,
                          Trajectory &optimize_trajectory,bool directY) {
    auto start = std::chrono::steady_clock::now();
    // State vector holds all current values neede for vars below
    Pose2d pose_agv = Pose2d(statu[0], statu[1], statu[2]);
    double line_velocity = statu[3];
    double wmg = statu[4];

    //纠正角速度/线速度，使其满足最小转弯半径
    double angular = wmg;
    double radius = mpc_param.shortest_radius * (1.0 / sqrt(line_velocity * line_velocity + 1e-10));
    if ((line_velocity * line_velocity) / (wmg * wmg + 1e-9) < (radius * radius)) {
        angular = fabs(line_velocity) / radius;
        if (wmg < 0) angular = -angular;
    }
    double max_wmg = fabs(line_velocity) / radius;//0.5 / radius;

    std::vector<Pose2d> filte_poses;
    if (filte_Path(pose_agv, target, trajectory, filte_poses, 20) == false) {
        printf("filte path failed ...\n");
        return failed;
    }
    select_traj = Trajectory(filte_poses);

    //将选中点移动到小车坐标系
    std::vector<Pose2d> transform_poses;
    for (int i = 0; i < filte_poses.size(); i++) {
        double x = filte_poses[i].x() - pose_agv.x();
        double y = filte_poses[i].y() - pose_agv.y();
        transform_poses.push_back(Pose2d(x, y, 0).rotate(-pose_agv.theta()));
    }

    Eigen::VectorXd coef = fit_path(transform_poses);

    //优化
    typedef CPPAD_TESTVECTOR(double) Dvector;

    //根据当前点和目标点距离,计算目标速度
    double dis=Pose2d::distance(pose_agv, target);
    double ref_velocity = 1.0/(1+exp(-4.*(dis-1)));

    //目标点与起点的连线 朝向与启动朝向 > M_PI/2.0

    Pose2d targetPoseInAGV = Pose2d::relativePose(target, pose_agv);
    //std::cout<<"target:"<<target<<", agv:"<<pose_agv<<", relative:"<<targetPoseInAGV<<std::endl;
    if (targetPoseInAGV.x() < 0)
        ref_velocity = -ref_velocity;

    double dt = mpc_param.dt;

    //printf("min_v:%f   max_v:%f\n",min_velocity_,max_velocity_);
    double max_dlt = max_wmg;//5*M_PI/180.0;
    double max_acc_line_velocity = mpc_param.acc_velocity;
    double max_acc_dlt = mpc_param.acc_angular * M_PI / 180.0;

    size_t n_vars = N * 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;
    }

    //// limit  v
    for (int i = nv; i < nv + N; i++) {
        vars_lowerbound[i] = -max_velocity_;
        vars_upperbound[i] = max_velocity_;
    }

    ////limint dlt
    for (int i = ndlt; i < ndlt + N; i++) {
        vars_lowerbound[i] = -max_dlt;
        vars_upperbound[i] = max_dlt;
    }

    // Lower and upper limits for the constraints
    size_t n_constraints = N * 5;
    /*
     * 障碍物是否进入 碰撞检测范围内
     */
    float distance = Pose2d::distance(obs_relative_pose_, Pose2d(0, 0, 0));
    bool find_obs = false;
    if (distance < 20) {
        printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
        find_obs = true;
    }
    if (find_obs) n_constraints = N * (5 + 8);

    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (int i = 0; i < n_constraints; i++) {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    //// acc v
    for (int i = nv; i < nv + N; i++) {
        constraints_lowerbound[i] = -max_acc_line_velocity;
        constraints_upperbound[i] = max_acc_line_velocity;
        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
        if (i < nv+delay) {
            constraints_lowerbound[i] = 0;
            constraints_upperbound[i] = 0;
        }
    }
    //// acc ndlt
    for (int i = ndlt; i < ndlt + N; i++) {
        constraints_lowerbound[i] = -max_acc_dlt;
        constraints_upperbound[i] = max_acc_dlt;
        //延迟处理,前delay个周期内认定为匀速运动(加速度为0)
        if (i < ndlt+delay){
            constraints_lowerbound[i] = 0;
            constraints_upperbound[i] = 0;
        }
    }

    // 与障碍物保持距离的约束
    if (find_obs) {
        for (int i = nobs; i < nobs + 8 * N; ++i) {
            constraints_lowerbound[i] = 1.0;
            constraints_upperbound[i] = 1e19;
        }
    }

    //限制最小转弯半径，
    /*for(int i=nwmg;i<nwmg+N;++i)
    {
      constraints_lowerbound[i] = 0;
      constraints_upperbound[i] = 1.0/(radius*radius);
    }*/

    if (line_velocity > max_velocity_) {
        line_velocity = max_velocity_;
    }
    if (line_velocity < -max_velocity_) {
        line_velocity = -max_velocity_;
    }
    if (angular > max_dlt) {
        angular = max_dlt;
    }
    if (angular < -max_dlt) {
        angular = -max_dlt;
    }

    Eigen::VectorXd statu_velocity(2);
    if (find_obs) {
        statu_velocity = Eigen::VectorXd(2 + 16);

        std::vector<Pose2d> vertexs = Pose2d::generate_rectangle_vertexs(obs_relative_pose_, obs_w_ * 2, obs_h_ * 2);
        for (int i = 0; i < vertexs.size(); ++i) {
            //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
            statu_velocity[2 + i * 2] = vertexs[i].x();
            statu_velocity[2 + i * 2 + 1] = vertexs[i].y();
        }
    }
    statu_velocity[0] = line_velocity;
    statu_velocity[1] = angular;

    Eigen::VectorXd condition(6);
    condition << dt, ref_velocity, obs_w_, obs_h_, targetPoseInAGV.x(), targetPoseInAGV.y();
    FG_eval_half_agv fg_eval(coef, statu_velocity, condition,directY);

    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    options += "Numeric max_cpu_time          0.5\n";
    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval_half_agv>(
            options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
            constraints_upperbound, fg_eval, solution);

    auto now = std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - start);
    double time =
            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

    // invalid_number_detected
    if (solution.status != CppAD::ipopt::solve_result<Dvector>::success) {
        printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) max_v:%f\n", solution.status, line_velocity,
               wmg, angular, max_velocity_);
        if (solution.status == CppAD::ipopt::solve_result<Dvector>::local_infeasibility)
            return no_solution;
        return failed;
    }

    // Cost
    auto cost = solution.obj_value;

    out.clear();

    double solve_velocity[down_count];
    double solve_angular[down_count];

    for (int i = 0; i < down_count; ++i) {
        solve_velocity[i] = solution.x[nv + delay + i];
        solve_angular[i] = solution.x[ndlt + delay + i];


        //纠正角速度/线速度，使其满足最小转弯半径
        double correct_angular = solve_angular[i];
        if ((solve_velocity[i] * solve_velocity[i]) / (solve_angular[i] * solve_angular[i] + 1e-9) < (radius * radius)) {
            correct_angular = fabs(line_velocity) / radius;
            if (solve_angular[i] < 0) correct_angular = -correct_angular;
        }
        ///-----
        printf(" P[%d],input:%.4f %.5f(%.5f) output:%.4f %.5f(%.5f) ref:%.3f max_v:%f time:%.3f\n",
               i, line_velocity, wmg, angular, solve_velocity[i], solve_angular[i], correct_angular,
               ref_velocity, max_velocity_, time);

        /*if(solve_velocity>=0 && solve_velocity<min_velocity_)
          solve_velocity=min_velocity_;
        if(solve_velocity<0 && solve_velocity>-min_velocity_)
          solve_velocity=-min_velocity_;*/

        out.push_back(solve_velocity[i]);
        out.push_back(correct_angular);
    }


    //计算预测轨迹
    optimize_trajectory.clear();
    for (int i = 0; i < N; ++i) {
        Pose2d pose(solution.x[nx + i], solution.x[ny + i], solution.x[nth + i]);
        optimize_trajectory.push_point(pose_agv + pose.rotate(pose_agv.theta()));
    }
    return success;
}

bool LoadedMPC::filte_Path(const Pose2d &point, Pose2d target, Trajectory trajectory,
                           std::vector<Pose2d> &poses, int point_num) {
    double gradient = 0;
    if (fabs((target - point).x()) == 0)
        gradient = 200.0;
    else
        gradient = (target - point).y() / (target - point).x();

    double theta = gradient2theta(gradient, target.x() - point.x() >= 0);

    if (trajectory.size() < 2)
        return false;

    poses.clear();
    for (int i = 0; i < trajectory.size(); i++) {
        // 平移加反向旋转到小车坐标系
        Pose2d offset = trajectory[i] - point;
        double x = offset.x();
        double y = offset.y();
        double trans_x = x * cos(-theta) - y * sin(-theta);
        double trans_y = x * sin(-theta) + y * cos(-theta);
        if (trans_x >= 0 && poses.size() < point_num) {
            // 旋转到原坐标系
            float nx = trans_x * cos(theta) - trans_y * sin(theta);
            float ny = trans_x * sin(theta) + trans_y * cos(theta);
            Pose2d pose(nx + point.x(), ny + point.y(), trajectory[i].theta());
            poses.push_back(pose);
        }
    }
    int size = poses.size();
    if (size < point_num) {
        //一个点都没有，则以当前点开始，反向取点
        float dl = -0.1;
        Pose2d last = point;
        if (size >= 1) {
            //从最后点到最后一点均匀选取
            last = poses[size - 1]; //
            dl = 0.1;
        }
        float dx = cos(last.theta()) * dl;
        float dy = sin(last.theta()) * dl;
        for (int i = 1; i < point_num - size + 1; ++i)
            poses.push_back(Pose2d(last.x() + dx * i, last.y() + dy * i, last.theta()));
    }
    return true;
}

Eigen::VectorXd LoadedMPC::fit_path(const std::vector<Pose2d> &trajectory) {
    int order = 3;
    assert(order >= 1 && order <= trajectory.size() - 1);
    Eigen::MatrixXd A(trajectory.size(), order + 1);
    Eigen::VectorXd yvals(trajectory.size());
    for (int i = 0; i < trajectory.size(); i++) {
        A(i, 0) = 1.0;
        yvals[i] = trajectory[i].y();
    }

    for (int j = 0; j < trajectory.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * trajectory[j].x();
        }
    }


    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}