//
// Created by gf on 23-7-21.
//

#ifndef LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_
#define LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_

#include <Eigen/Core>
#include "trajectory.h"
#include "custom_type.h"

class RotateMPC {
public:
    typedef struct {
        float shortest_radius;
        float dt;
        float acc_velocity;
        float acc_angular;
    } MPC_parameter;
public:
    RotateMPC(const Pose2d &obs, double obs_w, double obs_h, double min_velocity, double max_velocity);

    virtual ~RotateMPC();

    /*
     * 原地旋转MPC
     */
    virtual MpcError
    solve(Pose2d current_to_target, Eigen::VectorXd statu, MPC_parameter mpc_param, std::vector<double> &out);

protected:
    Pose2d obs_relative_pose_;    //障碍物相对小车的位姿
    double min_velocity_;
    double max_velocity_;
    double obs_w_;
    double obs_h_;

};

#endif //LIO_LIVOX_CPP_MPC_ROTATE_MPC_H_