#include  "MpcDiffModel.h"
using CppAD::AD;

static double ref_v = 2.0;
static double max_vel = 2.0;

class FG_eval_model {
 public:
  // Fitted polynomial coefficients
  Eigen::VectorXd coeffs;
  FG_eval_model(Eigen::VectorXd coeffs) { this->coeffs = coeffs;}

  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;
    
    // 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 cte_cost_weight = 10000;
    const int epsi_cost_weight = 5000;
    const int v_cost_weight = 1;
    // const int delta_cost_weight = 10;
    const int a_cost_weight = 5;
    // const int delta_change_cost_weight = 100;
    const int a_change_cost_weight = 5;

    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    // Cost for CTE, psi error and velocity
    for (int t = 0; t < MpcDiffModel::N; t++) {
      fg[0] += cte_cost_weight * CppAD::pow(vars[MpcDiffModel::cte_start + t], 2);
      fg[0] += epsi_cost_weight * CppAD::pow(vars[MpcDiffModel::epsi_start + t], 2);
      fg[0] += v_cost_weight * CppAD::pow(vars[MpcDiffModel::v_left_start + t] - ref_v, 2);
      fg[0] += v_cost_weight * CppAD::pow(vars[MpcDiffModel::v_right_start + t] - ref_v, 2);
      // std::cout<<"v l r : "<<vars[v_left_start + t]<<", "<<vars[v_right_start + t]<<std::endl;
    }
    
    // Costs for steering (delta) and acceleration (a)
    for (int t = 0; t < MpcDiffModel::N-1; t++) {
      // fg[0] += delta_cost_weight * CppAD::pow(vars[delta_start + t], 2);
      fg[0] += a_cost_weight * CppAD::pow(vars[MpcDiffModel::a_left_start + t], 2);
      fg[0] += a_cost_weight * CppAD::pow(vars[MpcDiffModel::a_right_start + t], 2);
      // std::cout<<"a l r : "<<vars[a_left_start + t]<<", "<<vars[a_right_start + t]<<std::endl;
    }
    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    
    // Costs related to the change in steering and acceleration (makes the ride smoother)
    for (int t = 0; t < MpcDiffModel::N-2; t++) {
      // fg[0] += delta_change_cost_weight * pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
      fg[0] += a_change_cost_weight * pow(vars[MpcDiffModel::a_left_start + t + 1] - vars[MpcDiffModel::a_left_start + t], 2);
      fg[0] += a_change_cost_weight * pow(vars[MpcDiffModel::a_right_start + t + 1] - vars[MpcDiffModel::a_right_start + t], 2);
    }
    // std::cout<<"fg0: "<<fg[0]<<std::endl;
    
    // Setup Model Constraints

    // Initial constraints
    // We add 1 to each of the starting indices due to cost being located at index 0 of `fg`.
    // This bumps up the position of all the other values.
    fg[1 + MpcDiffModel::x_start] = vars[MpcDiffModel::x_start];
    fg[1 + MpcDiffModel::y_start] = vars[MpcDiffModel::y_start];
    fg[1 + MpcDiffModel::psi_start] = vars[MpcDiffModel::psi_start];
    fg[1 + MpcDiffModel::v_left_start] = vars[MpcDiffModel::v_left_start];
    fg[1 + MpcDiffModel::v_right_start] = vars[MpcDiffModel::v_right_start];
    fg[1 + MpcDiffModel::cte_start] = vars[MpcDiffModel::cte_start];
    fg[1 + MpcDiffModel::epsi_start] = vars[MpcDiffModel::epsi_start];
    // std::cout << "112,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    // fg[1 + a_left_start] = vars[a_left_start];
    // std::cout << "113,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    // fg[1 + a_right_start] = vars[a_right_start];
    // std::cout << "114,,,fg,vars: " <<1 + a_left_start<<", "<<1 + a_right_start<< std::endl;
    
    // The rest of the constraints
    for (int t = 1; t < MpcDiffModel::N; t++) {
      // State at time t + 1
      AD<double> x1 = vars[MpcDiffModel::x_start + t];
      AD<double> y1 = vars[MpcDiffModel::y_start + t];
      AD<double> psi1 = vars[MpcDiffModel::psi_start + t];
      AD<double> v_left1 = vars[MpcDiffModel::v_left_start + t];
      AD<double> v_right1 = vars[MpcDiffModel::v_right_start + t];
      AD<double> cte1 = vars[MpcDiffModel::cte_start + t];
      AD<double> epsi1 = vars[MpcDiffModel::epsi_start + t];
      
      // State at time t
      AD<double> x0 = vars[MpcDiffModel::x_start + t - 1];
      AD<double> y0 = vars[MpcDiffModel::y_start + t - 1];
      AD<double> psi0 = vars[MpcDiffModel::psi_start + t - 1];
      AD<double> v_left0 = vars[MpcDiffModel::v_left_start + t - 1];
      AD<double> v_right0 = vars[MpcDiffModel::v_right_start + t - 1];
      AD<double> cte0 = vars[MpcDiffModel::cte_start + t - 1];
      AD<double> epsi0 = vars[MpcDiffModel::epsi_start + t - 1];
      
      // Actuator constraints at time t only
      AD<double> a_left0 = vars[MpcDiffModel::a_left_start + t - 1];
      AD<double> a_right0 = vars[MpcDiffModel::a_right_start + t - 1];
      
      AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * pow(x0, 2) + coeffs[3] * pow(x0, 3);
      AD<double> psi_des0 = CppAD::atan(coeffs[1] + 2*coeffs[2]*x0 + 3*coeffs[3]*pow(x0,2));
      
      // Setting up the rest of the model constraints
      AD<double> v0 = (v_left0+v_right0)/2.0;
      AD<double> v_diff0 = v_right0 - v_left0;
      fg[1 + MpcDiffModel::x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * MpcDiffModel::dt);
      fg[1 + MpcDiffModel::y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * MpcDiffModel::dt);
      fg[1 + MpcDiffModel::psi_start + t] = psi1 - (psi0 - v_diff0 / MpcDiffModel::Lf * MpcDiffModel::dt);
      // fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
      fg[1 + MpcDiffModel::v_left_start + t] = v_left1 - (v_left0 + a_left0 * MpcDiffModel::dt);
      fg[1 + MpcDiffModel::v_right_start + t] = v_right1 - (v_right0 + a_right0 * MpcDiffModel::dt);
      fg[1 + MpcDiffModel::cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * MpcDiffModel::dt));// 预测路径误差 - (y方向与路径差 + 当前角度误差带来的y方向移动)
      fg[1 + MpcDiffModel::epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v_diff0 / MpcDiffModel::Lf * MpcDiffModel::dt);// 预测角度误差 - (当前与目标角度差 - 当前角度偏转或补偿)
    }    
  }
};

MpcDiffModel::MpcDiffModel()
{
  // ref_v = 2.0;
  v_left = 0;
  v_right = 0;
  a_left = 0;
  a_right = 0;
  last_theta = 0;

  // for (int i = -30; i < 100; i++)
  // {
  //   double x = i * (15.0 / 100);
  //   double y = -0.005 * x * x * x + 0.0525 * x * x + 0.001 * x+1;
  //   ptsx.push_back(x);
  //   ptsy.push_back(y);
  //   // std::cout<<"x,y: "<<x<<", "<<y<<std::endl;
  // }
}

MpcDiffModel::~MpcDiffModel()
{
}

// 求y
double MpcDiffModel::polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// 求参数
Eigen::VectorXd MpcDiffModel::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

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

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

// 输入局部路径，当前位姿，速度与角速度
// 输出用于下发的速度与角速度
bool MpcDiffModel::Solve(double ref_velociity, std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth)
{
  ref_v = ref_velociity;
  //max_vel = fmin(fabs(ref_v),fabs(max_vel));
  // std::cout << "model 000 v: " << v << ", " << vth << std::endl;
  double theta_diff = curr_pose.getRotation().getAngle() - last_theta;
  v_left = v - vth*Lf/2.0;//(theta_diff * Lf) / (2 * dt);
  v_right = v + vth*Lf/2.0;//(theta_diff * Lf) / (2 * dt);
  Eigen::VectorXd ptsx_car(ref_poses.size());
  Eigen::VectorXd ptsy_car(ref_poses.size());

  for (int i = 0; i < ref_poses.size(); i++)
  {
    ptsx_car[i] = ref_poses[i].getOrigin().getX();
    ptsy_car[i] = ref_poses[i].getOrigin().getY();
    // std::cout<<"x,y: "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
  }

  //拟合求斜率与y方向误差
  auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
  // std::cout<<"coeffs: "<<coeffs<<std::endl;
  // if(std::isnan(coeffs[0])||std::isnan(coeffs[1])||std::isnan(coeffs[2])||std::isnan(coeffs[3]))
  //   return;
  double cte = polyeval(coeffs, 0);
  double etheta = atan(coeffs[1]);

  // Predict state after latency
  // x, y and psi are all zero after transformation above
  double pred_px = 0.0 + v * dt; // Since psi is zero, cos(0) = 1, can leave out
  const double pred_py = 0.0;    // Since sin(0) = 0, y stays as 0 (y + v * 0 * dt)
  double pred_theta = 0.0 + theta_diff;
  double pred_v_left = v_left + a_left * dt;
  double pred_v_right = v_right + a_right * dt;
  double pred_cte = cte + v * sin(etheta) * dt;
  double pred_etheta = etheta - theta_diff;

  bool ok = true;
  typedef CPPAD_TESTVECTOR(double) Dvector;

  size_t n_vars = N * 7 + (N - 1) * 2;

  size_t n_constraints = N * 7;

  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 < v_left_start; i++) {
    vars_lowerbound[i] = -1.0e6;
    vars_upperbound[i] = 1.0e6;
  }

  for (int i = v_left_start; i < cte_start; i++) {
    vars_lowerbound[i] = -max_vel;
    vars_upperbound[i] = max_vel;
  }

  for (int i = cte_start; i < a_left_start; i++) {
    vars_lowerbound[i] = -1.0e6;
    vars_upperbound[i] = 1.0e6;
  }
  
  for (int i = a_left_start; i < n_vars; i++) {
    vars_lowerbound[i] = -6.5;
    vars_upperbound[i] = 6.5;
  }

  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;
  }
  
  // Start lower and upper limits at current values
  constraints_lowerbound[x_start] = pred_px;
  constraints_lowerbound[y_start] = pred_py;
  constraints_lowerbound[psi_start] = pred_theta;
  constraints_lowerbound[v_left_start] = pred_v_left;
  constraints_lowerbound[v_right_start] = pred_v_right;
  constraints_lowerbound[cte_start] = pred_cte;
  constraints_lowerbound[epsi_start] = pred_etheta;
  
  constraints_upperbound[x_start] = pred_px;
  constraints_upperbound[y_start] = pred_py;
  constraints_upperbound[psi_start] = pred_theta;
  constraints_upperbound[v_left_start] = pred_v_left;
  constraints_upperbound[v_right_start] = pred_v_right;
  constraints_upperbound[cte_start] = pred_cte;
  constraints_upperbound[epsi_start] = pred_etheta;

  // object that computes objective and constraints
  FG_eval_model fg_eval(coeffs);

  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
  // std::cout<<vars.size()<<", "<<vars_lowerbound.size()<<", "<<vars_upperbound.size()<<", "<<constraints_lowerbound.size()<<", "<<constraints_upperbound.size()<<", "<<std::endl;
  CppAD::ipopt::solve<Dvector, FG_eval_model>(
      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 <<"model   status "<<ok<< ", Cost " << cost << std::endl;

  //保存状态，根据优化结果获取需下发 v vth
  last_theta = curr_pose.getRotation().getAngle();
  std::vector<double> solved;
  if (ok)
  {
    solved.push_back(solution.x[a_left_start]);
    solved.push_back(solution.x[a_right_start]);
    for (int i = 0; i < N; ++i)
    {
      solved.push_back(solution.x[x_start + i]);
      solved.push_back(solution.x[y_start + i]);
    }
    // std::cout<<"return solved"<<std::endl;
  }else
  {
    return false;
  }

  //更新 v vth
  a_left = solved[0];
  a_right = solved[1];
  // std::cout<<"v_curr: "<<v_left<<", "<<v_right<<std::endl;
  v_left += a_left * dt;
  v_right += a_right * dt;
  std::cout<<"model a: "<<a_left<<", "<<a_right<<std::endl;
  // std::cout << "model   v: " << v_left << ", " << v_right << std::endl;
  v = (v_left + v_right) / 2.0;
  vth = -(v_right - v_left) / Lf; //turtlesim uses left hand angle, if angle less than 0, it means turn left.
  std::cout << "model   v: " << v << ", " << vth << ", " << ref_v << std::endl;
}
