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

#ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
#define LIO_LIVOX_CPP_MPC_NAVIGATION_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:
    enum ActionType
    {
        eReady=0,
        eRunning
    };
 public:
    Navigation();
    ~Navigation();
    //连接AGV emqx服务

    bool Init(const Navigation_parameter& parameter);

    void ResetPose(const Pose2d& pose);
    void ResetStatu(double v,double a);
    TimedLockData<Pose2d>& RealTimePose(){
      return timedPose_;
    }
    TimedLockData<double>& RealTimeV(){
      return timedV_;
    }
    TimedLockData<double>& RealTimeA(){
      return timedA_;
    }
    bool Start(const NavMessage::NavCmd& cmd);
    void Cancel();
    void Pause();

 protected:

    static void RobotStatuCallback(double x,double y,double theta,double v,double vth,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& target,const Pose2d& target_diff);
    bool exec_adjust_action(const Pose2d& target,const Pose2d& target_diff);
    bool mpc_once(const Trajectory& traj,std::vector<double>& out);
    void navigatting();

    /*
     * 发布导航模块状态
     */
    void publish_statu();

 protected:
    std::mutex  mtx_;
    bool exit_=false;
    std::thread* pubthread_= nullptr;
    bool inited_=false;
    Monitor_emqx* monitor_= nullptr;
    Terminator_emqx* terminator_= nullptr;

    TimedLockData<Pose2d> timedPose_;           //当前位姿
    TimedLockData<double> timedV_;              //底盘数据
    TimedLockData<double> timedA_;

    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;

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

    Navigation_parameter parameter_;
};


#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_