/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/
#include "mpc.h"
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>


using CppAD::AD;

namespace robot {
namespace common {


// 设置预测状态点的个数和两个相邻状态点之间的时间间隔
size_t N = 10;
double dt = 0.05; // 50ms

// 设置汽车头到车辆重心之间的距离
const double Lf = 2.67;

// 参考速度，为了避免车辆在行驶中停止
//double ref_v = 65;
double ref_cte = 0;
double ref_epsi = 0;

// solver使用的是一个向量存储所有的状态值，所以需要确定每种状态在向量中的开始位置
size_t x_start = 0;
size_t y_start = x_start + N;
size_t psi_start = y_start + N;
size_t v_start = psi_start + N;
size_t cte_start = v_start + N;
size_t epsi_start = cte_start + N;
size_t delta_start = epsi_start + N;
size_t a_start = delta_start + N -1;

class FG_eval {
public:

    // 参考路径方程的系数
    Eigen::VectorXd coeffs;
    double ref_v = 0;
    FG_eval(Eigen::VectorXd coeffs, double speed) {
        this->coeffs = coeffs;
        ref_v = speed;
    }

    typedef CPPAD_TESTVECTOR(AD<double>)ADvector;

    // 该函数的目的是定义约束，fg向量包含的是总损失和约束，vars向量包含的是状态值和驱动器的输入
     void operator()(ADvector& fg, const ADvector& vars) {
        // 任何cost都会加到fg[0]
        fg[0] = 0;
        // 使车辆轨迹和参考路径的误差最小，且使车辆的速度尽量接近参考速度
        for (int i = 0; i < N; i++) {
             fg[0] += 2000*CppAD::pow(vars[cte_start + i] - ref_cte, 2);
             fg[0] += 2000*CppAD::pow(vars[epsi_start + i] - ref_epsi, 2);
             fg[0] += CppAD::pow(vars[v_start + i] - ref_v, 2);
           }
        // 使车辆行驶更平稳，尽量减少每一次驱动器的输入大小
        // 注意：驱动器的输入是N-1个.最后一个点没有驱动器输入
        for (int i = 0; i < N - 1; i++) {
          fg[0] += 5*CppAD::pow(vars[delta_start + i], 2);
          fg[0] += 5*CppAD::pow(vars[a_start + i], 2);
        }
        // 为了使车辆运动更平滑，尽量减少相邻两次驱动器输入的差距
        // 注意：这个地方是N-2个
        for (int i = 0; i < N - 2; i++) {
          fg[0] += 200*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2);
          fg[0] += 10*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2);
        }

        // 设置fg的初始值为状态的初始值，这个地方为初始条件约束
        fg[1 + x_start] = vars[x_start];
        fg[1 + y_start] = vars[y_start];
        fg[1 + psi_start] = vars[psi_start];
        fg[1 + v_start] = vars[v_start];
        fg[1 + cte_start] = vars[cte_start];
        fg[1 + epsi_start] = vars[epsi_start];

        // 因为t=0初始条件约束已经有了，计算其他约束条件
        for (int t = 0; t < N -1; t++) {
            // t+1时刻的状态
            AD<double> x1 = vars[x_start + t + 1];
            AD<double> y1 = vars[y_start + t + 1];
            AD<double> psi1 = vars[psi_start + t + 1];
            AD<double> v1 = vars[v_start + t + 1];
            AD<double> cte1 = vars[cte_start + t + 1];
            AD<double> epsi1 = vars[epsi_start + t + 1];

            // t时刻的状态
            AD<double> x0 = vars[x_start + t ];
            AD<double> y0 = vars[y_start + t ];
            AD<double> psi0 = vars[psi_start + t ];
            AD<double> v0 = vars[v_start + t ];
            AD<double> cte0 = vars[cte_start + t ];
            AD<double> epsi0 = vars[epsi_start + t ];
            // t时刻的驱动器输入
            AD<double> delta0 = vars[delta_start + t ];
            AD<double> a0 = vars[a_start + t ];

            // t时刻参考路径的距离和角度值
            AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * x0 * x0 + coeffs[3] * x0 * x0 * x0;
            AD<double> psides0 = CppAD::atan(3*coeffs[3]* x0* x0 + 2*coeffs[2]* x0 + coeffs[1]);

            // 根据如上的状态值计算约束条件
            fg[2 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
            fg[2 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
            fg[2 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
            fg[2 + v_start + t] = v1 - (v0 + a0 * dt);
            fg[2 + cte_start + t] =
                cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
            fg[2 + epsi_start + t] =
                epsi1 - ((psi0 - psides0) - v0 * delta0 / Lf * dt);
        }
    }
};

// MPC class definition
MPC::MPC() {
}
MPC::~MPC() {
}

vector<double> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
  size_t i;
  typedef CPPAD_TESTVECTOR(double) Dvector;

  double x = x0[0];
  double y = x0[1];
  double psi = x0[2];
  double v = x0[3];
  double cte = x0[4];
  double epsi = x0[5];

  // 独立状态的个数，注意：驱动器的输入（N - 1）* 2
  size_t n_vars = N * 6 + (N - 1) * 2;
  // 约束条件的个数
  size_t n_constraints = N * 6;

    // 除了初始值，初始化每一个状态为0
  Dvector vars(n_vars);
   for (int i = 0; i < n_vars; i++) {
     vars[i] = 0;
   }
   vars[x_start] = x;
   vars[y_start] = y;
   vars[psi_start] = psi;
   vars[v_start] = v;
   vars[cte_start] = cte;
   vars[epsi_start] = epsi;

    // 设置每一个状态变量的最大和最小值
    // 【1】设置非驱动输入的最大和最小值
    // 【2】设置方向盘转动角度范围-25—25度
    // 【3】加速度的范围-—100%
   Dvector vars_lowerbound(n_vars);
   Dvector vars_upperbound(n_vars);
   for (int i = 0; i < delta_start; i++) {
       vars_lowerbound[i] = -1.0e19;
       vars_upperbound[i] = 1.0e19;
     }
   for (int i = delta_start; i < a_start; i++) {
       vars_lowerbound[i] = -0.436332;
       vars_upperbound[i] = 0.436332;
//     vars_lowerbound[i] = (delta_prev < -0.32) ? -0.436332 : delta_prev - 0.1;
//     vars_upperbound[i] = (delta_prev > 0.32) ? 0.436332 : delta_prev + 0.1;
   }
   for (int i = a_start; i < n_vars; i++) {
       vars_lowerbound[i] = -0.5; // -0.36
       vars_upperbound[i] = 0.5;
//     vars_lowerbound[i] = (a_prev < -0.31) ? -0.36 : a_prev - 0.05;; // -0.36
//     vars_upperbound[i] = (a_prev > 0.31) ? 0.36 : a_prev + 0.05;;  // 0.36
   }
   // 设置约束条件的的最大和最小值，除了初始状态其他约束都为0
   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;
   }
   constraints_lowerbound[x_start] = x;
   constraints_lowerbound[y_start] = y;
   constraints_lowerbound[psi_start] = psi;
   constraints_lowerbound[v_start] = v;
   constraints_lowerbound[cte_start] = cte;
   constraints_lowerbound[epsi_start] = epsi;

   constraints_upperbound[x_start] = x;
   constraints_upperbound[y_start] = y;
   constraints_upperbound[psi_start] = psi;
   constraints_upperbound[v_start] = v;
   constraints_upperbound[cte_start] = cte;
   constraints_upperbound[epsi_start] = epsi;

    // Object that computes objective and constraints
    FG_eval fg_eval(coeffs, ref_speed);

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

    // 计算这个问题
    CppAD::ipopt::solve_result<Dvector> solution;
    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
    //
    bool ok = true;
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

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

    vector<double> result;

    result.push_back(solution.x[delta_start]);
    result.push_back(solution.x[a_start]);

    for (int i = 0; i < N-1; i++)
    {
       result.push_back(solution.x[x_start + i + 1]);
       result.push_back(solution.x[y_start + i + 1]);
    }
    return result;
}

}  // namespace common
}  // namespace robot
