//
// 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 ActionType
    {
        eReady=0,
        eRotation,
        eHorizon,
        eVertical,
        eClampOpen,
        eClampClose
    };
    //舵轮位置
    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);
    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);
    void Cancel(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response);
    bool Stop();
    bool SlowlyStop();
    virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase);
    bool PoseTimeout();

 protected:
    virtual void HandleAgvStatu(const MqttMsg& msg);
    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                             double v,double angular);

    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);
    /*
     * 按照路径点导航
     */
    bool execute_nodes(NavMessage::NewAction action);
    /*
     * 进出库
     */
    bool execute_InOut_space(NavMessage::NewAction action);
    /*
     * 根据自身和配对车辆状态，调整入库姿态，先到的进入车位1点，后车先到的倒车入库，后到的车与先到的车保持朝向一致
     * space:输入车位点,
     * target:输出目标点
     */
    virtual bool RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target);

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

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

    Monitor_emqx::ActionMode move_mode_=Monitor_emqx::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_