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

#include "navigation.h"
#include "loaded_mpc.h"
#include "../define/TimerRecord.h"

Navigation::~Navigation()
{
  if(terminator_)
    delete terminator_;
  if(monitor_)
    delete monitor_;
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
      thread_->join();
    delete thread_;
  }
}

bool Navigation::Init(const Navigation_parameter& parameter)
{
  Emqx_parameter agv_p=parameter.agv_emqx();
  if(monitor_== nullptr) {
    monitor_ = new Monitor_emqx(agv_p.nodeid(), agv_p.pubtopic(), agv_p.subtopic());
    monitor_->set_statu_arrived_callback(Navigation::RobotStatuCallback, this);
    if(monitor_->Connect(agv_p.ip(),agv_p.port())==false)
    {
      printf(" agv emqx connected failed\n");
      return false;
    }
  }

  Emqx_parameter terminal_p=parameter.terminal_emqx();
  if(terminator_== nullptr) {
    terminator_ = new Terminator_emqx(terminal_p.nodeid(), terminal_p.pubtopic(), terminal_p.subtopic());
    terminator_->set_cmd_arrived(Navigation::NavCmdCallback,this);
    if(terminator_->Connect(terminal_p.ip(),terminal_p.port())==false)
    {
      printf(" terminator emqx connected failed\n");
      return false;
    }
  }
  inited_=true;
  return true;

}

void Navigation::publish_statu(ActionType type,float velocity,float angular)
{
  NavStatu statu;
  if(running_)
  {
    statu.set_action_type(type);
    statu.set_isrunning(running_);
    std::queue<Trajectory> trajectorys_=global_trajectorys_;
    while(trajectorys_.empty()==false)
    {
      Trajectory traj=trajectorys_.front();
      TrajectoryMsg* trajMsg=statu.add_trajs();
      for(int i=0;i<traj.size();++i)
      {
        Pose2dMsg* pose=trajMsg->add_points();
        pose->set_x(traj[i].x());
        pose->set_y(traj[i].y());
        pose->set_theta(traj[i].theta());
      }
      trajectorys_.pop();
    }
    statu.set_velocity(velocity);
    statu.set_angular(angular);
    if(type==eMPC)
    {
      //发布mpc 预选点
      if(selected_traj_.timeout()==false) {
        for (int i = 0; i < selected_traj_.Get().size(); ++i){
          Pose2d pt=selected_traj_.Get()[i];
          Pose2dMsg* pose=statu.mutable_select_traj()->add_points();
          pose->set_x(pt.x());
          pose->set_y(pt.y());
          pose->set_theta(pt.theta());
        }
      }
      //发布 mpc 预测点
      if(predict_traj_.timeout()==false) {
        for (int i = 0; i < predict_traj_.Get().size(); ++i){
          Pose2d pt=predict_traj_.Get()[i];
          Pose2dMsg* pose=statu.mutable_predict_traj()->add_points();
          pose->set_x(pt.x());
          pose->set_y(pt.y());
          pose->set_theta(pt.theta());
        }
      }
    }
  }
  terminator_->publish_nav_statu(statu);
}

void Navigation::ResetStatu(double v,double a)
{
  timedV_.reset(v,0.5);
  timedA_.reset(a,0.5);
}

void Navigation::ResetPose(const Pose2d& pose)
{
  timedPose_.reset(pose,0.5);
}

bool Navigation::Start(std::queue<Trajectory> global_trajectorys)
{
  if(running_==true)
  {
    if(pause_)
    {
      pause_=false;
      return true;
    }
    printf(" navigation is running pls cancel before\n");
    return false;
  }
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
      thread_->join();
    delete thread_;
  }

    //add 先检查当前点与起点的距离
    global_trajectorys_=global_trajectorys;
    thread_=new std::thread(&Navigation::navigatting,this);
    return true;

}
void Navigation::Cancel()
{
  cancel_=true;

}
void Navigation::Pause()
{
  monitor_->stop();
  pause_=true;
}

Navigation::Navigation()
{
  thread_= nullptr;
  monitor_= nullptr;
  terminator_= nullptr;
}


void Navigation::NavCmdCallback(const NavCmd& cmd,void* context)
{
  Navigation* navigator=(Navigation*)context;

  if(cmd.action()==3)
  {
    navigator->Cancel();
    return ;
  }
  if(cmd.action()==2) {
    navigator->pause_=false;
    return ;
  }
  if(cmd.action()==1) {
    navigator->Pause();
    return ;
  }

  std::queue<Trajectory> trajs;
  for(int i=0;i<cmd.trajs_size();++i)
  {
    Trajectory traj;
    for(int j=0;j<cmd.trajs(i).points_size();++j)
    {
      Pose2dMsg pt_msg=cmd.trajs(i).points(j);
      Pose2d pt2d(pt_msg.x(),pt_msg.y(),pt_msg.theta());
      traj.push_point(pt2d);
    }
    trajs.push(traj);
  }
  navigator->Start(trajs);
}
void Navigation::RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context)
{
  Navigation* navigator=(Navigation*)context;
  navigator->ResetPose(Pose2d(x,y,theta));
  navigator->ResetStatu(v,vth);
}

bool Navigation::moveToPoint(const Pose2d& pose,const Pose2d& distance)
{
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return false;
  }
  Pose2d thresh=Pose2d::abs(distance);
  bool yCompleted=false;
  bool xCompleted=false;
  bool aCompleted=false;
  while(cancel_==false)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (pause_ == true)
    {
      //发送暂停消息
      continue;
    }
    if (timedPose_.timeout() == true)
    {
      printf(" navigation Error:Pose is timeout \n");
      break;
    }

    //计算目标pose在当前pose下的pose
    Pose2d diff = Pose2d::PoseByPose(pose,timedPose_.Get());
    if (Pose2d::abs(diff).y() < thresh.y())
      yCompleted=true;
    else if(Pose2d::abs(diff).y()>2.0*thresh.y())
      yCompleted=false;

    if (Pose2d::abs(diff).x() < thresh.x())
      xCompleted=true;
    else if(Pose2d::abs(diff).x() >2.0* thresh.x())
      xCompleted=false;

    if (Pose2d::abs(diff).theta() < thresh.theta())
      aCompleted=true;
    else
      aCompleted=false;

    if(yCompleted&&xCompleted&&aCompleted)
    {
      monitor_->stop();
      return true;
    }

    //先旋转使得Y=0摆直，再移动x，再X最后旋转
    if(yCompleted==false)
    {
      double theta=atan(diff.y()/diff.x());

      double maxtheta=20.*M_PI/180.0;
      double mintheta=5.*M_PI/180.0;
      if(abs(theta)<mintheta)
      {
        theta=(theta>0)?mintheta:-mintheta;
      }
      if(abs(theta)>maxtheta)
      {
        theta=(theta>0)?maxtheta:-maxtheta;
      }
      publish_statu(eRotate,0,theta);
      monitor_->rotate(theta);
      printf("旋转 diff.y:%.5f\n",diff.y());
      continue;

    }
    if(xCompleted==false){
      double v=diff.x()/10.0;
      double maxv=1.9;
      double minv=0.1;
      if(abs(v)<minv)
      {
        v=(v>0)?minv:-minv;
      }
      if(abs(v)>maxv)
      {
        v=(v>0)?maxv:-maxv;
      }
      publish_statu(eLine,v,0);
      monitor_->set_speed(v,0);
      printf("x移动 diff.x():%.5f\n",diff.x());
      continue;
    }
    if(aCompleted==false){
      double theta=diff.theta()/5.0;
      if(std::abs(diff.theta())>M_PI)
        theta=-theta;
      double maxtheta=20.*M_PI/180.0;
      double mintheta=5.*M_PI/180.0;
      if(abs(theta)<mintheta)
      {
        theta=(theta>0)?mintheta:-mintheta;
      }
      if(abs(theta)>maxtheta)
      {
        theta=(theta>0)?maxtheta:-maxtheta;
      }
      publish_statu(eRotate,0,theta);
      monitor_->rotate(theta);
      printf("旋转angular:%.5f diff.theta():%.5f\n",theta,diff.theta());
    }

  }
  if(cancel_==true)
    return false;
  return true;
}

bool Navigation::mpc_once(const Trajectory& traj,std::vector<double>& out)
{
  if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
  {
    printf(" MPC once Error:Pose/V/A is timeout \n");
    return false;
  }
  if(traj.size()==0)
  {
    printf("traj size ==0\n");
    return false;
  }

  Pose2d pose=timedPose_.Get();
  double velocity=timedV_.Get();    //从plc获取状态
  double angular=timedA_.Get();

  LoadedMPC MPC(2.7);
  Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态

  statu[0]=pose.x();
  statu[1]=pose.y();
  statu[2]=pose.theta();
  statu[3]=velocity;
  statu[4]=angular;

  Trajectory optimize_trajectory;
  Trajectory selected_trajectory;
  bool ret= MPC.solve(traj,traj[traj.size()-1],statu,out,selected_trajectory,optimize_trajectory);
  predict_traj_.reset(optimize_trajectory,1);
  selected_traj_.reset(selected_trajectory,1);
  return ret;
}

bool Navigation::IsArrived(const Pose2d& cur,double speed,const Pose2d& target,const Pose2d& diff)
{
    if(Pose2d::abs(cur-target)<diff)
    {
        if(fabs(speed)<0.1)
            return true;
    }
    return false;
}

bool Navigation::execute_edge(const Trajectory& child,const Pose2d& head_diff,const Pose2d& target_diff)
{
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return false;
  }

  if(child.size()==0)
    return true;
  Pose2d head=child[0];
  Pose2d tail=child[child.size()-1];

  Trajectory traj=child;
  bool init_start_pose=false;
  while(cancel_==false)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if(pause_==true)
    {
      //发送暂停消息
      continue;
    }
    if(timedPose_.timeout()==true)
    {
      printf(" navigation Error:Pose is timeout \n");
      break;
    }
    //判断是否到达终点
    Pose2d diff=Pose2d::abs(timedPose_.Get()-tail);
    if(IsArrived(timedPose_.Get(),timedV_.Get(),tail,target_diff))
    {
        monitor_->stop();
        printf(" edge navigation completed !!!\n");
        return true;
    }else if(diff.x()<target_diff.x() && diff.y()<target_diff.y()){
      monitor_->stop();
        moveToPoint(tail,target_diff);
    }
    //std::cout<<"diff:"<<Pose2d::abs(timedPose_.Get()-tail)<<std::endl;

    //如果与起点差距过大先运动到起点
    if(false==init_start_pose)
    {
      init_start_pose= moveToPoint(head, head_diff);
      continue;
    }
    else{
      //一次变速
      std::vector<double> out;
      bool ret;
      TimerRecord::Execute([&,this](){
        ret=mpc_once(traj,out);
      },"mpc_once");
      if(ret==false)
      {
        monitor_->stop();
        return false;
      }
      publish_statu(eMPC,out[0],out[1]);
      monitor_->set_speed(out[0],out[1]);
    }

  }
  return false;
}

void Navigation::navigatting()
{
  if(inited_==false) {
    printf(" navigation has not inited\n");
    return ;
  }
  pause_=false;
  cancel_=false;
  running_=true;
  printf(" navigation beg...\n");

#if 0
  Trajectory gtraj;
  while(global_trajectorys_.empty()==false)
  {
    gtraj=gtraj+global_trajectorys_.front();
    global_trajectorys_.pop();
  }
  execute_edge(gtraj);
#else
  double v=0,a=0;
  Pose2d head_diff(0.1,0.1,3/180.0*M_PI);
  Pose2d target_diff(0.03,0.05,0.5/180.0*M_PI);
  while(global_trajectorys_.empty()==false && cancel_==false)
  {
    Trajectory traj=global_trajectorys_.front();
    if(execute_edge(traj,head_diff,target_diff))
    {
      global_trajectorys_.pop();
    }
    else
    {
      break;
    }
  }
#endif
  monitor_->stop();
  if(cancel_==true)
    printf(" navigation canceled\n");
  printf(" navigation end...\n");
  if(global_trajectorys_.size()==0)
    printf("navigation completed!!!\n");
  running_=false;
}