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

#include "navigation_main.h"


NavigationMain::NavigationMain(){
  move_mode_=Monitor_emqx::eSingle;
  wheelBase_=0;
}
NavigationMain::~NavigationMain(){

}

void NavigationMain::ResetPose(const Pose2d& pose){
  if(move_mode_==Monitor_emqx::eMain) {
    if(timedBrotherPose_.timeout()==true)
    {
      std::cout<<"Brother 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 diff=Pose2d::abs(Pose2d::relativePose(brother,pose));

    if(diff.x()>3.6 || diff.x()<2.2 || diff.y()>0.3 || diff.theta()>5*M_PI/180.0)
    {
      std::cout<<" distance with two agv is too far diff: "<<diff<<std::endl;
      return;
    }
    Pose2d abs_diff=pose-brother;
    //计算两车朝向的方向
    float theta=Pose2d::vector2yaw(abs_diff.x(),abs_diff.y());
    Pose2d agv=Pose2d((pose.x()+brother.x())/2.0,(pose.y()+brother.y())/2.0,theta);

    Navigation::ResetPose(agv);
  }
  else
    Navigation::ResetPose(pose);
}

void NavigationMain::publish_statu(NavMessage::NavStatu& statu)
{
  statu.set_main_agv(true);
  Navigation::publish_statu(statu);
}

void NavigationMain::HandleNavCmd(const NavMessage::NavCmd& cmd)
{
  if(cmd.action()==4)
  {
    if(cmd.newactions_size()==0)
    {
      printf(" Error main \n");
      return;
    }
    NavMessage::NewAction action=cmd.newactions(0);
    if(action.wheelbase()<2.4 || action.wheelbase()>3.5)
    {
      printf("Failed to Switch MoveMode --> main,wheelbase invalid :%f\n",action.wheelbase());
      return;
    }
    SwitchMode(Monitor_emqx::eMain,action.wheelbase());
    return ;
  }
  if(cmd.action()==5)
  {
    printf(" Switch MoveMode --> single\n");
    SwitchMode(Monitor_emqx::eSingle,0);
    return ;
  }
  else
  {
    Navigation::HandleNavCmd(cmd);
  }
}

bool NavigationMain::Start(const NavMessage::NavCmd& cmd)
{
  /*if(move_mode_!=Monitor_emqx::eMain)
  {
    printf(" navigation mode must set main,parameter:Pose2d\n");
    return false;
  }*/

  return Navigation::Start(cmd);

}

void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
                double v,double angular)
{
  if(monitor_)
  {
    monitor_->set_speed(mode,type,v,angular,wheelBase_);
  }

}

bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
{
  if(Navigation::CreateRobotStatuMsg(robotStatu)) {
    robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
    return true;
  }
  //std::cout<<agvStatu.DebugString()<<std::endl;
  return false;
}

void NavigationMain::ResetOtherClamp(ClampStatu statu)
{
  timed_other_clamp_.reset(statu,1);
}

void NavigationMain::HandleAgvStatu(const MqttMsg& msg)
{
  NavMessage::AgvStatu speed;
  if(msg.toProtoMessage(speed))
  {
    ResetStatu(speed.v(),speed.w());
    ResetClamp((ClampStatu)speed.clamp());
    ResetOtherClamp((ClampStatu)speed.clamp_other());
    //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
  }
}

bool NavigationMain::clamp_close()
{
  if(move_mode_==Monitor_emqx::eSingle)
    return Navigation::clamp_close();
  printf("双车夹持\n");
  if(monitor_) {
    monitor_->clamp_close(move_mode_);
    while (exit_ == false) {
      if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
        printf("timed clamp is timeout\n");
        return false;
      }
      if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get()==eClosed) {
        printf("双车夹持completed!!!\n");
        return true;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      monitor_->clamp_close(move_mode_);
    }
    return false;
  }
  return false;
}
bool NavigationMain::clamp_open() {
  if(move_mode_==Monitor_emqx::eSingle)
    return Navigation::clamp_open();

  if(monitor_) {
    printf("双车松夹持\n");
    monitor_->clamp_open(move_mode_);
    while(exit_==false)
    {
      if(timed_clamp_.timeout()||timed_other_clamp_.timeout())
      {
        printf("timed clamp is timeout\n");
        return false;
      }
      if(timed_clamp_.Get()==eOpened&&timed_other_clamp_.Get()==eOpened) {
        printf("双车松夹持completed!!!\n");
        return true;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
      monitor_->clamp_open(move_mode_);
    }
    return false;
  }
  return false;
}