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

using CppAD::AD;

// Set the timestep length and duration
// Currently tuned to predict 1 second worth
size_t N = 10;


// The solver takes all the state variables and actuator
// variables in a singular vector. Thus, we should to establish
// when one variable starts and another ends to make our lifes easier.
size_t nx = 0;
size_t ny = nx + N;
size_t nth = ny + N;
size_t nv = nth + N;
size_t nvth = nv + N;
size_t nacc = nvth + N;
size_t naccth = nacc + N - 1;

class FG_eval {
public:
    // Fitted polynomial coefficients
    Eigen::VectorXd coeffs;
    double m_ref_v,m_dt;
    double m_v,m_vth;
    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
        this->coeffs = coeffs;
        m_v=v;
        m_vth=vth;
        m_ref_v=ref_v;
        m_dt=dt;
    }

    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
    void operator()(ADvector& fg, const ADvector& vars) {
        // Implementing MPC below
        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
        // The cost is stored is the first element of `fg`.
        // Any additions to the cost should be added to `fg[0]`.
        fg[0] = 0;
        double dt=m_dt;
        // Reference State Cost
        // Below defines the cost related the reference state and
        // any anything you think may be beneficial.

        // Weights for how "important" each cost is - can be tuned
        const int y_cost_weight = 2500;
        const int th_cost_weight = 1000;
        const int v_cost_weight = 1;
        const int vth_cost_weight = 10;
        const int a_cost_weight = 1;
        const int ath_cost_weight=10;

        // Cost for CTE, psi error and velocity
        for (int t = 0; t < N; t++)
        {
            // y坐标差
            AD<double> xt = vars[nx + t];
            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t] - fx, 2);

            // theta, v, vth cost
            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t] - fth, 2);
            fg[0] += v_cost_weight * CppAD::pow(vars[nv + t] - m_ref_v, 2);
            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
        }

        // Costs for steering (delta) and acceleration (a)
        for (int t = 0; t < N - 1; t++)
        {
            // a, ath cost
            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t + 1] - vars[nv + t], 2);
            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t + 1] - vars[nvth + t], 2);
        }

        /////////////////////
        // x, y, theta路径约束
        fg[1 + nx] = vars[nx];// - vars[nv] * dt;
        fg[1 + ny] = vars[ny];
        fg[1 + nth] = vars[nth];// - vars[nvth] * dt;
        fg[1 + nv] = vars[nv];
        fg[1 + nvth] = vars[nvth];

        // The rest of the constraints
        for (int t = 1; t < N; t++)
        {
            // State at time t + 1
            AD<double> x1 = vars[nx + t];
            AD<double> y1 = vars[ny + t];
            AD<double> th1 = vars[nth + t];
            AD<double> v1 = vars[nv + t];
            AD<double> vth1 = vars[nvth + t];

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

            AD<double> acc0 = vars[nacc + t - 1];
            AD<double> accth0 = vars[naccth + t -1];

            // Actuator constraints at time t only

            // 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 + vth0 * dt);
            fg[1 + nv + t] = v1 - (v0 + acc0 * dt);
            fg[1 + nvth + t] = vth1 - (vth0 + accth0 * dt);
        }

        // fg[1 + nv] = (vars[nv] - m_v) / dt;
        // fg[1 + nvth] = (vars[nvth] - m_vth) / dt;
        // for (int t = 1; t < N; ++t)
        // {
        //     fg[1 + nv + t] = (vars[nv + t] - vars[nv + t - 1]) / dt;
        //     fg[1 + nvth + t] = (vars[nvth + t] - vars[nvth + t - 1]) / dt;
        // }

        ////  add
    }
};

//
// MPC class definition implementation.
//
MonitorMPC::MonitorMPC() {}
MonitorMPC::~MonitorMPC() {}

bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
    bool ok = true;
    typedef CPPAD_TESTVECTOR(double) Dvector;

    // State vector holds all current values neede for vars below
    double v = state[0];
    double vth = state[1];
    double ref_v = state[2];
    double max_v=state[3];
    double dt=state[4];

    // std::cout<<"params: "<<state.transpose()<<std::endl;
    // Setting the number of model variables (includes both states and inputs).
    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
    size_t n_vars = N * 7 - 2;
    // Setting the number of constraints
    size_t n_constraints = N * 5;

    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }

    for (int i = 0; i < N; ++i)
    {
        vars[nv + i] = v;
        vars[nvth + i] = vth;
    }
    // vars[nv] = v;
    // vars[nvth] = vth;

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Sets lower and upper limits for variables.
    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    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_v;
        vars_upperbound[i] = max_v;
    }
    // vars_lowerbound[nv] = v-0.01;
    // vars_upperbound[nv] = v+0.01;

    ////limint vth
    for (int i = nvth; i < nvth+N; i++) {
        vars_lowerbound[i] = -0.03;
        vars_upperbound[i] = 0.03;
    }
    // vars_lowerbound[nvth] = vth-0.01;
    // vars_upperbound[nvth] = vth+0.01;

    // acc
    for (int i = nacc; i < nacc+N-1; i++) {
        vars_lowerbound[i] = -5;
        vars_upperbound[i] = 5;
    }

    // acc th
    for (int i = naccth; i < naccth+N-1; i++) {
        vars_lowerbound[i] = -0.5;
        vars_upperbound[i] = 0.5;
    }

    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    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;
    }
    // 对传入v vth进行限制
    if(v < vars_lowerbound[nv])
        v = vars_lowerbound[nv];
    if(v > vars_upperbound[nv])
        v = vars_upperbound[nv];

    if(vth < vars_lowerbound[nvth])
        vth = vars_lowerbound[nvth];
    if(vth > vars_upperbound[nvth])
        vth = vars_upperbound[nvth];
    // v vth初值约束
    constraints_lowerbound[nv] = v;
    constraints_upperbound[nv] = v;
    constraints_lowerbound[nvth] = vth;
    constraints_upperbound[nvth] = vth;
	
    // // acc v
    // for (int i = nv; i < nv+N; i++) {
    //     constraints_lowerbound[i] = -0.001;
    //     constraints_upperbound[i] = 0.001;
    // }
    // //// acc vth
    // for (int i = nvth; i < nvth+N; i++) {
    //     constraints_lowerbound[i] = -0.001;
    //     constraints_upperbound[i] = 0.001;
    // }


    // object that computes objective and constraints
    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);

    //
    // NOTE: You don't have to worry about these options
    //
    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.
    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>(
        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;

    // Cost
    auto cost = solution.obj_value;
    //std::cout << "Cost " << cost << std::endl;

    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
    out.clear();
    m_trajectory.clear();
    out.push_back(solution.x[nv+1]);
    out.push_back(solution.x[nvth+1]);
    for (int i = 0; i < N; ++i) {
        out.push_back(solution.x[nx + i]);
        out.push_back(solution.x[ny + i]);
        tf::Pose pose;
        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
        m_trajectory.push_back(pose);
    }
    //printf(" cost : %.3f\n",cost);
    // printf("v0 vth0 v1 vth1 acc0 accth1: %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\n",solution.x[nv],solution.x[nvth],solution.x[nv+1],solution.x[nvth],solution.x[nacc],solution.x[naccth]);
    return ok;

}
