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

#ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
#define LIO_LIVOX_CPP_MPC_NAVIGATION_H_

#include <glog/logging.h>
#include "../define/timedlockdata.hpp"
#include "pose2d.h"
#include "trajectory.h"
#include "monitor/monitor_emqx.h"
#include "monitor/terminator_emqx.h"
#include "parameter.pb.h"
#include <queue>
#include <thread>

class Navigation {
public:
    typedef struct {
        double min;
        double max;
    } stLimit;
    //舵轮位置
    enum RWheelPosition {
        eUnknow = 0,
        eX,   //超前
        eY,     //横移
        eR      //旋转位
    };
    enum ClampStatu {
        eInvalid = 0,
        eClosed = 1,
        eOpened = 2
    };

    enum MpcResult {
        eMpcSuccess = 0,
        eMpcFailed,
        eImpossibleToTarget
    };


public:
    Navigation();

    virtual ~Navigation();
    //连接AGV emqx服务

    bool Init(const NavParameter::Navigation_parameter &parameter);

    virtual void ResetPose(const Pose2d &pose);

    void ResetStatu(double v, double a);

    virtual void ResetClamp(ClampStatu statu);

    virtual void ResetLifter(LifterStatus status);

    TimedLockData<Pose2d> &RealTimePose() {
        return timedPose_;
    }

    TimedLockData<double> &RealTimeV() {
        return timedV_;
    }

    TimedLockData<double> &RealTimeA() {
        return timedA_;
    }

    virtual void Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);

    /* 手动旋转 */
    virtual void ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response);

    void Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);

    bool Stop();

    bool SlowlyStop();

    virtual void SwitchMode(int mode, float wheelBase);

    bool PoseTimeout();

protected:
    virtual void HandleAgvStatu(const MqttMsg &msg);

    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);

    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance);

    static void RobotPoseCallback(const MqttMsg &msg, void *context);

    static void RobotSpeedCallback(const MqttMsg &msg, void *context);

    static void BrotherAgvStatuCallback(const MqttMsg &msg, void *context);


    bool IsArrived(NavMessage::PathNode targetNode);

    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY);

    /*
     * 移动到目标点，不能到达则旋转
     */
    bool MoveToTarget(NavMessage::PathNode node, stLimit limit_v, stLimit limit_w, bool autoDirect, bool enable_rotate);

    /*
     * autoDirect:自动选择 纵向还是横向巡线
     */
    MpcResult MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect);

    /*
     * 旋转摆正，朝向目标点,
     * anyDirect: 任意一个朝向对准目标点
     */
    virtual bool RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect);

    virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);

    /*
     * 按照路径点导航
     */
    bool execute_nodes(NavMessage::NewAction action);

    /*
     * 进出库
     */
    bool execute_InOut_space(NavMessage::NewAction action);

    /*
     * 根据自身和配对车辆状态，调整入库姿态，先到的进入车位1点，后车先到的倒车入库，后到的车与先到的车保持朝向一致
     * space:输入车位点,
     * target:输出目标点
     */
    virtual Navigation::MpcResult
    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);

    virtual bool clamp_close();

    virtual bool clamp_open();

    virtual bool lifter_rise();

    virtual bool lifter_down();

    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY = false);

    /* 原地旋转mpc
     *
     * */
    bool
    Rotation_mpc_once(Pose2d current_to_target, stLimit limit_wmg, std::vector<double> &out, bool all_direct = true);

    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);

    /*
     * 计算以当前点到目标点的曲率
     */
    float compute_cuv(const Pose2d &target);

    /*
     * 发布导航模块状态
     */
    static void pubStatuThreadFuc(void *p);

    virtual void publish_statu(NavMessage::NavStatu &statu);

protected:
    std::mutex mtx_;
    bool exit_ = false;
    ActionType actionType_ = eReady;
    std::thread *pubthread_ = nullptr;
    bool inited_ = false;
    Monitor_emqx *monitor_ = nullptr;
    Terminator_emqx *terminator_ = nullptr;  //本机emqx连接
    Terminator_emqx *brotherEmqx_ = nullptr;  //本机emqx连接

    TimedLockData<Pose2d> timedPose_;           //当前位姿
    TimedLockData<double> timedV_;              //底盘数据
    TimedLockData<double> timedA_;
    TimedLockData<ClampStatu> timed_clamp_;     //加持杆
    TimedLockData<LifterStatus> timed_lifter_;    //提升机构

    TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
    TimedLockData<double> timedBrotherV_;              //底盘数据
    TimedLockData<double> timedBrotherA_;
    TimedLockData<NavMessage::NavStatu> timedBrotherNavStatu_;


    NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
    std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
    bool running_ = false;
    bool pause_ = false;
    bool cancel_ = false;

    int move_mode_ = eSingle;

    bool isInSpace_; //是否在车位或者正在进入车位
    std::string space_id_;
    TimedLockData<Trajectory> selected_traj_;
    TimedLockData<Trajectory> predict_traj_;

    NavParameter::Navigation_parameter parameter_;
    RWheelPosition RWheel_position_;
};

double limit_gause(double x, double min, double max);

double limit(double x, double min, double max);

double next_speed(double speed, double target_speed, double acc, double dt = 0.1);

#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_