//
// 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,
        eRunning
    };
    enum ClampStatu
    {
        eInvalid=0,
        eClosed=1,
        eOpened=2
    };

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

    bool Init(const 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 bool Start(const NavMessage::NavCmd& cmd);
    void Cancel();
    void Pause();
    bool Stop();


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

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

    static bool IsArrived(const Pose2d& cur,double velocity,double angular,const Pose2d& target,
                          const Pose2d& diff);
    bool execute_along_action(const Pose2d& begin,const Pose2d& target,const Pose2d& target_diff,stLimit limit_v);
    bool execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
                            stLimit limit_v,stLimit limit_h,stLimit limit_rotate);
    virtual bool clamp_close();
    virtual bool clamp_open();
    bool mpc_once(const Trajectory& traj,stLimit limit_v,std::vector<double>& out);
    bool mpc_possible_to_end(const Pose2d& target,const Pose2d& diff);
    void navigatting();

    virtual NavMessage::RobotStatu CreateRobotStatuMsg();

    /*
     * 发布导航模块状态
     */
    static void pubStatuThreadFuc(void* p);
    virtual void publish_statu(NavMessage::NavStatu& statu);

 protected:
    std::mutex  mtx_;
    bool exit_=false;
    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_;

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

    Monitor_emqx::ActionMode move_mode_=Monitor_emqx::eSingle;

    TimedLockData<Trajectory> selected_traj_;
    TimedLockData<Trajectory> predict_traj_;

    Navigation_parameter parameter_;
};


#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_