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

#ifndef LIO_LIVOX_CPP_MPC_LOADED_MPC_H_
#define LIO_LIVOX_CPP_MPC_LOADED_MPC_H_

#include <Eigen/Core>
#include "trajectory.h"
enum MpcError{
    success=0,
    no_solution=1,
    failed
};
class LoadedMPC
{
public:
    typedef struct {
        float shortest_radius;
        float dt;
        float acc_velocity;
        float acc_angular;
    }MPC_parameter;
 public:
    LoadedMPC(const Pose2d& obs,double obs_w,double obs_h,double min_velocity,double max_velocity);
    virtual ~LoadedMPC();
    virtual MpcError solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd statu,MPC_parameter mpc_param,
                       std::vector<double>& out,Trajectory& select_traj,Trajectory& optimize_trajectory);

 protected:
    /*
     * 根据当前点和轨迹，选择前方 point_num
     * point:当前点，target：目标点
     */
    static bool filte_Path(const Pose2d& point,Pose2d target,
                           Trajectory trajectory,std::vector<Pose2d>& poses,int point_num=10);
    /*
     * 根据路径点，拟合曲线
     */
    static Eigen::VectorXd fit_path(const std::vector<Pose2d>& trajectory);
    Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
    double min_velocity_;
    double max_velocity_;
    double obs_w_;
    double obs_h_;

};

#endif //LIO_LIVOX_CPP_MPC_LOADED_MPC_H_