//
// Created by zx on 23-5-8.
//

#ifndef NAVIGATION_NAVIGATION_MAIN_H
#define NAVIGATION_NAVIGATION_MAIN_H
#include "navigation.h"

class NavigationMain:public Navigation {

public:
    NavigationMain();
    ~NavigationMain();

    virtual void ResetPose(const Pose2d& pose);
    virtual void Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response);
    virtual void publish_statu(NavMessage::NavStatu& statu);


    void ResetOtherClamp(ClampStatu statu);
    virtual void HandleAgvStatu(const MqttMsg& msg);
    virtual void SwitchMode(Monitor_emqx::ActionMode mode,float wheelBase){
      if(move_mode_==mode)
        return;
      if(mode==Monitor_emqx::eMain)
      {
        if(timedBrotherPose_.timeout()==true || timedPose_.timeout()==true)
        {
          std::cout<<"Brother/self pose is timeout can not set MainAGV pose"<<std::endl;
          return;
        }
        //Pose2d transform(-wheelBase_/2.0,0,0);
        //Navigation::ResetPose(pose * transform);
        Pose2d brother=timedBrotherPose_.Get();
        Pose2d pose=timedPose_.Get();
        Pose2d diff=Pose2d::abs(Pose2d::relativePose(brother,pose));

        if(diff.x()>3.4 || diff.x()<2.3 || diff.y()>0.2 || diff.theta()>5*M_PI/180.0)
        {
          std::cout<<" distance with two agv is too far diff: "<<diff<<std::endl;
          return;
        }
        printf(" Switch MoveMode --> main\n");
      }
      wheelBase_=wheelBase;
      move_mode_=mode;

    }


protected:
    virtual bool RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target);
    virtual void SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                             double v,double angular);
    virtual bool clamp_close();
    virtual bool clamp_open();
    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu);
protected:
    float  wheelBase_;   //两节agv的位姿与单节的位姿变换
    TimedLockData<int> timed_other_clamp_;
};

#endif //NAVIGATION_NAVIGATION_MAIN_H