//
// 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::Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response)
{
  /*if(move_mode_!=Monitor_emqx::eMain)
  {
    printf(" navigation mode must set main,parameter:Pose2d\n");
    return false;
  }*/

  Navigation::Start(cmd,response);

}

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;
}

bool NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target) {
  if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
    printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
    return false;
  }

  NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();

  stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
  double acc_angular = 25 * M_PI / 180.0;
  double dt = 0.1;
  Pose2d rotated = Pose2d(space.x(),space.y(),space.theta());
  target.set_l(0.05);
  target.set_w(0.03);
  target.set_id(space.id());
  //后车先到,当前车进入2点,保持与后车一致的朝向
  if (brother.in_space() && brother.space_id() == space.id()) {
    rotated.mutable_theta() = brother.odom().theta();
  } else { //当前车先到,正向
    rotated.mutable_theta() = space.theta();
  }
  std::cout<<"===============================> RotateBeforeEnterSpace ,target:"<<rotated<<std::endl;
  double x = space.x() + wheelbase/2.0 * cos(rotated.theta());
  double y = space.y() + wheelbase/2.0 * sin(rotated.theta());
  target.set_x(x);
  target.set_y(y);
  target.set_theta(rotated.theta());

  while (cancel_ == false) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    Pose2d current = timedPose_.Get();
    double yawDiff = (rotated - current).theta();
    if (fabs(yawDiff) > 1 * M_PI / 180.0) {
      double theta = limit_gause(yawDiff, limit_rotate.min, limit_rotate.max);
      double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
      double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);

      SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, limit_angular);
      actionType_ = eRotation;
      printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
             timedA_.Get(), angular, limit_angular, yawDiff);

      continue;
    } else {
      if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
        printf(" RotateBeforeEnterSpace refer target completed\n");
        printf("---------------- update new target :%f %f %f \n",target.x(),target.y(),target.theta());
        return true;
      }
    }
  }
  return false;

}