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

    void ResetOtherLifter(LifterStatus status);

    virtual void HandleAgvStatu(const MqttMsg &msg);

    virtual bool SwitchMode(int mode, float wheelBase) {
        printf("change mode to:%d\n", mode);
        wheelBase_ = wheelBase;
        if (move_mode_ == mode){
            return true;
        }

        if (mode == eDouble) {
            if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
                std::cout << "Brother/self pose is timeout can not set MainAGV pose" << std::endl;
                return false;
            }
            //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.6 || diff.x() < 2.2 || diff.y() > 0.5 || diff.theta() > 5 * M_PI / 180.0) {
                std::cout << " distance with two agv is too far diff: " << diff << std::endl;
                return false;
            }

            Pose2d self_pose = timedPose_.Get();
            Pose2d brother_pose = timedBrotherPose_.Get();
            Pose2d brother_in_self = Pose2d::relativePose(brother_pose, self_pose);

            if (brother_in_self.x() <= 0)
                mode = mode | eMainInForward;
            else
                mode = mode | eMainInBackward;
            if (brother_in_self.theta() >= -M_PI / 2 && brother_in_self.theta() <= M_PI / 2)
                mode = mode | eChildSameToMain;
            else
                mode = mode | eChildOppositeToMain;

            printf(" Switch MoveMode --> main(%d), wheelBase: %f\n", mode, wheelBase);
        }

        move_mode_ = mode;
        return true;
    };

protected:
    virtual Navigation::MpcResult
    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
                           NavMessage::PathNode &target,int clampLifterAction=0);

    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[]);
    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
                             int space_id, double distance,double Y1=0.0,double Y2=0.0);

    virtual bool clamp_close();

    virtual bool clamp_half_open();

    virtual bool lifter_rise();

    virtual bool lifter_down();

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

protected:

};

#endif //NAVIGATION_NAVIGATION_MAIN_H