//
// 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"
#include "custom_type.h"

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,bool directY);


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_