//
// 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 = 25;                  //优化考虑后面多少步

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;              //搜索条件参数
    FG_eval_half_agv(Eigen::VectorXd coeffs,Eigen::VectorXd statu,Eigen::VectorXd condition) {
      m_coeffs=coeffs;
      m_statu=statu;
      m_condition=condition;
    }

    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);
        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]);
            fg[1 + nobs + N*i+t] = CppAD::pow(ra/obs_w,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)
{
  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=0.5/radius;//fabs(line_velocity) / radius;

  std::vector<Pose2d> filte_poses;
  if (filte_Path(pose_agv, target, trajectory, filte_poses, 10) == 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 ref_velocity = Pose2d::distance(pose_agv, target) / 2.5;
  if(ref_velocity<0.1)
    ref_velocity=0.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;
  }
  //// acc ndlt
  for (int i = ndlt; i < ndlt + N; i++)
  {
    constraints_lowerbound[i] = -max_acc_dlt;
    constraints_upperbound[i] = max_acc_dlt;
  }

  // 与障碍物保持距离的约束
  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);

  // 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;

  solution.status == CppAD::ipopt::solve_result<Dvector>::success; // 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=solution.x[nv];
  double solve_angular=solution.x[ndlt];

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