#include <csignal>

#include "pangolinViewer.h"

#include "dijkstra/dijkstra.h"
#include "define/timedlockdata.hpp"
#include "define/TimerRecord.h"
#include <glog/logging.h>
#include "dijkstra/trajectory.h"
#include "emqx/terminator_emqx.h"
#include "tool/pathcreator.h"
#include "tool/proto_tool.h"
#include <queue>
#include "map.pb.h"

Terminator_emqx* g_terminator= nullptr;
//路径相关
std::mutex pathMutex;
std::vector<int> g_shortest_path;
PathMap DijkstraMap;
bool g_auto_test=false;
int auto_test_count=0;
int next_begin_node=0;
int end_node=0;

bool last_nav_statu_completed=true;

//控制相关
TimedLockData<NavMessage::NavStatu> timed_navStatu_;
TimedLockData<NavMessage::AGVStatu> timed_AgvStatu_;
TimedLockData<NavMessage::AGVStatu> timed_AgvBrotherStatu_;

//创建单车由指定点进入车位导航指令
bool CreateIntoSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
//创建单车出车位去指定点导航指令
bool CreateOutSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);
//创建单车出车位去指定点导航指令
bool CreateTestNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd);

bool NavIsCompleted()
{
  if(timed_navStatu_.timeout()==false)
  {
    if(timed_navStatu_.Get().unfinished_actions_size()>0)
    {
      return false;
    }
    return true;
  }
  return false;
}

bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs);

bool InitMap(std::string config_file);

bool OnDijkstraBtn(int beg,int end,float& distance,
                   std::vector<int>& shortestPath)
{
  if(beg==0 || end==0)
    return false;
  next_begin_node=beg;
  end_node=end;
  bool ret= DijkstraMap.GetShortestPath(beg, end, shortestPath, distance);
  if(ret)
  {
    pathMutex.lock();
    g_shortest_path= shortestPath;
    std::reverse(g_shortest_path.begin(),g_shortest_path.end());
    g_shortest_path.push_back(end_node);
    pathMutex.unlock();
  }
  return ret;
}
void navigation_start() {

  if (NavIsCompleted() == false) {
    printf("Last nav has not completed!!\n");
    return;
  }

  if (next_begin_node == 0) {
    printf(" Last node == 0 ,not allow to start nav\n");
    return;
  }

  std::vector<int> t_shortestPath;
  float distance = -1;
  bool ret = DijkstraMap.GetShortestPath(next_begin_node, end_node, t_shortestPath, distance);
  if (ret) {
    pathMutex.lock();
    g_shortest_path = t_shortestPath;
    std::reverse(g_shortest_path.begin(), g_shortest_path.end());
    g_shortest_path.push_back(end_node);
    pathMutex.unlock();
  }

  auto_test_count++;
  //发布指令
  NavMessage::NavCmd cmd;
  if (false == CreateIntoSpaceNavCmd(g_shortest_path, cmd))
    return;
  std::cout << " ------------------------------------new NavCmd ---------------------------" << std::endl;
  printf("----第 %d 次测试---- from %d ---- %d -------begin navigatting...-----\n\n",
         auto_test_count,next_begin_node,end_node);

  if (g_terminator) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }

}

void navigation_pause()
{
  NavMessage::NavCmd cmd;
  cmd.set_action(1);  //暂停
  if(g_terminator) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }
}
void navigation_stop()
{
  g_auto_test=false;
  if(NavIsCompleted()==false)
  {
    printf("Last nav has not completed!! set last node = -1\n");
    next_begin_node=0;
  }

  NavMessage::NavCmd cmd;
  cmd.set_action(3);  //取消导航
  if(g_terminator) {
    MqttMsg msg;
    msg.fromProtoMessage(cmd);
    g_terminator->Publish("agv1_child/nav_cmd", msg);
  }
}

void navigation_autoTest()
{
  g_auto_test=true;
}
void pangolinSpinOnce(PangolinViewer* viewer) {
  ///导航结束，修改起点为上一任务终点,重置终点为0
  if(last_nav_statu_completed==false) {
    if (NavIsCompleted() == true) {
      if (end_node != 0) {
        next_begin_node = end_node;
        //随机终点
        int randid=0; //[1-8]
        while(randid==0 || randid==next_begin_node) {
          randid = rand() % 8 + 1;
          usleep(1000*1000);
        }
        printf(" nav completed ...... reset beg:%d  rand end:%d\n",next_begin_node,randid);
        end_node = randid;
        float distance;
        std::vector<int> shortestPath;

        if(OnDijkstraBtn(next_begin_node,end_node,distance,shortestPath))
          viewer->ResetPathInfo(next_begin_node, end_node,distance);
        last_nav_statu_completed=true;
        if(g_auto_test)
          navigation_start();
      }
    }
  }

  if (timed_AgvStatu_.timeout() == false) {
    NavMessage::AGVStatu statu = timed_AgvStatu_.Get();

    Pose2d pose2d(statu.x(), statu.y(), statu.theta());
    Eigen::Matrix3d rotation_matrix3;
    rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

    Eigen::Matrix4d pose;
    pose.topLeftCorner(3, 3) = rotation_matrix3;
    pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
    viewer->ShowRealtimeStatu(pose, statu.v(), statu.vth());
  }
  //显示另一节agv状态
  if (timed_AgvBrotherStatu_.timeout() == false) {
    NavMessage::AGVStatu statu = timed_AgvBrotherStatu_.Get();
    Pose2d pose2d(statu.x(), statu.y(), statu.theta());
    Eigen::Matrix3d rotation_matrix3;
    rotation_matrix3 = Eigen::AngleAxisd(pose2d.theta(), Eigen::Vector3d::UnitZ()) *
                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                       Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

    Eigen::Matrix4d pose;
    pose.topLeftCorner(3, 3) = rotation_matrix3;
    pose.topRightCorner(3, 1) = Eigen::Vector3d(pose2d.x(), pose2d.y(), 0);
    viewer->ShowBrotherStatu(pose, statu.v(), statu.vth());
  }
  //绘制导航实时状态
  if (timed_navStatu_.timeout() == false) {
    NavMessage::NavStatu statu = timed_navStatu_.Get();
    Trajectory select_traj;
    Trajectory predict_traj;
    for (int i = 0; i < statu.selected_traj().poses_size(); ++i) {
      select_traj.push_point(Pose2d(statu.selected_traj().poses(i).x(),
                                    statu.selected_traj().poses(i).y(),
                                    statu.selected_traj().poses(i).theta()));
    }
    for (int i = 0; i < statu.predict_traj().poses_size(); ++i) {
      predict_traj.push_point(Pose2d(statu.predict_traj().poses(i).x(),
                                     statu.predict_traj().poses(i).y(),
                                     statu.predict_traj().poses(i).theta()));
    }
    viewer->DrawTrajectory(select_traj, 1, 0, 0, 12);
    viewer->DrawTrajectory(predict_traj, 0, 1, 1, 15);
  }

  //绘制dijkstra地图及最短路径
  viewer->DrawDijkastraMap(DijkstraMap, g_shortest_path);

}

void AgvStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::AGVStatu statu;
  if(msg.toProtoMessage(statu))
    timed_AgvStatu_.reset(statu,0.5);

}
void AgvBrotherStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::AGVStatu statu;
  if(msg.toProtoMessage(statu))
    timed_AgvBrotherStatu_.reset(statu,0.5);

}
void NavStatuArrived(const MqttMsg& msg,void* context)
{
  NavMessage::NavStatu statu;
  if(msg.toProtoMessage(statu))
    timed_navStatu_.reset(statu,1.0);
  if(last_nav_statu_completed==true)
  {
    if(NavIsCompleted()==false)
      last_nav_statu_completed=false;
  }

}

int main(int argc, char** argv)
{
  srand((unsigned int)time(nullptr));
  if(argc<2)
  {
    printf(" argc must >2  exe mapfile\n");
    return -1;
  }
  std::string config_file=argv[1];
  if(InitMap(config_file)==false)
    return -1;


  //"agv1/nav_statu"
  g_terminator=new Terminator_emqx("UI_nav-child");

  if(g_terminator->Connect("192.168.0.70",1883)==false)
  {
    printf(" AGV emqx connect failed\n");
    return -1;
  }
  g_terminator->AddCallback("agv1_child/nav_statu",NavStatuArrived, nullptr);
  g_terminator->AddCallback("agv1_child/agv_statu",AgvStatuArrived, nullptr);
  g_terminator->AddCallback("agv1/agv_statu",AgvBrotherStatuArrived, nullptr);


  PangolinViewer* viewer=PangolinViewer::CreateViewer();
  viewer->SetCallbacks(pangolinSpinOnce,OnDijkstraBtn);
  viewer->SetNavigationCallbacks(navigation_start,navigation_pause,navigation_stop);
  viewer->SetAutoTestNavCallback(navigation_autoTest);
  viewer->Join();

  TimerRecord::PrintAll();

  delete viewer;
  delete g_terminator;
  return 0;
}

bool InitMap(std::string config_file)
{
  MapMessage::Map proto_map;
  if(proto_tool::read_proto_param(config_file,proto_map)==false)
  {
    printf(" read map proto failed\n");
    return false;
  }

  std::map<int,int> exist_id;
  for(int i=0;i<proto_map.nodes_size();++i)
  {
    MapMessage::Node node=proto_map.nodes(i);
    if(exist_id.find(node.id())!=exist_id.end())
    {
      printf("Can not add vertex, Node Id :%d exist\n",node.id());
      return false;
    }
    DijkstraMap.AddVertex(node.id(),node.x(),node.y());
    exist_id[node.id()]=0;
  }

  for(int i=0;i<proto_map.edges_size();++i)
  {
    MapMessage::Edge edge=proto_map.edges(i);
    if(exist_id.find(edge.id1())!=exist_id.end() && exist_id.find(edge.id2())!=exist_id.end())
    {
      if(false==DijkstraMap.AddEdge(edge.id1(),edge.id2(),edge.directed()))
        return false;
    }
    else
    {
      printf(" Cant add edge id1 or id2 (%d %d) is not exist\n",edge.id1(),edge.id2());
      return false;
    }
  }
  return true;
}

bool Create_trajectoris(PathMap& map,std::vector<int> nodes_id,std::queue<Trajectory>& trajs)
{
  if(nodes_id.size()<2)
    return false;
  for(int i=0;i<nodes_id.size()-1;++i)
  {

    float x1,y1,x2,y2;
    map.GetVertexPos(nodes_id[i],x1,y1);
    map.GetVertexPos(nodes_id[i+1],x2,y2);

    Pose2d start(x1,y1,0);
    Pose2d end(x2,y2,0);
    Trajectory traj=Trajectory::create_line_trajectory(start,end,0.05);
    if(traj.size()>1)
      trajs.push(traj);
    else
      return false;

  }
  return true;
}

bool CreateIntoSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
{

  if(nodes.size()==0)
    return false;

  cmd.set_action(0);  //开始新导航
  char key[255]={0};
  static int N=1111;
  sprintf(key,"abcd-efg-%d",N++);
  cmd.set_key(key);

  NavMessage::Pose2d adjustdiff,node_mpcdiff,enddiff;
  adjustdiff.set_x(0.1);
  adjustdiff.set_y(0.05);
  adjustdiff.set_theta(0.5*M_PI/180.0);

  node_mpcdiff.set_x(0.1);
  node_mpcdiff.set_y(0.1);
  node_mpcdiff.set_theta(3*M_PI/180.0);

  enddiff.set_x(0.1);
  enddiff.set_y(0.1);
  enddiff.set_theta(5*M_PI/180.0);


  //调整到起点，朝向与当前朝向一致
  if(timed_AgvStatu_.timeout())
  {
    printf(" agv statu is timeout can not get current direction\n");
    return false;
  }
  float current_theta=timed_AgvStatu_.Get().theta();
  float x,y;
  DijkstraMap.GetVertexPos(nodes[0],x,y);

  NavMessage::SpeedLimit v_limit,angular_limit,horize_limit;
  v_limit.set_min(0.1);
  v_limit.set_max(0.2);
  horize_limit.set_min(0.05);
  horize_limit.set_max(0.2);
  angular_limit.set_min(1);
  angular_limit.set_max(25.0);

  NavMessage::SpeedLimit mpc_x_limit,mpc_angular_limit;
  mpc_x_limit.set_min(0.05);
  mpc_x_limit.set_max(1.5);

  mpc_angular_limit.set_min(0*M_PI/180.0);
  mpc_angular_limit.set_max(3*M_PI/180.0);

  NavMessage::Action act_head;
  act_head.set_type(1);
  act_head.mutable_target()->set_x(x);
  act_head.mutable_target()->set_y(y);
  act_head.mutable_target()->set_theta(current_theta);
  act_head.mutable_target_diff()->CopyFrom(adjustdiff);

  act_head.mutable_velocity_limit()->CopyFrom(v_limit);
  act_head.mutable_horize_limit()->CopyFrom(horize_limit);
  act_head.mutable_angular_limit()->CopyFrom(angular_limit);
  cmd.add_actions()->CopyFrom(act_head);

  Pose2d begin;
  for(int i=1;i<nodes.size();++i)
  {
    //调整到上一点位姿，朝向当前点
    float lx,ly;
    DijkstraMap.GetVertexPos(nodes[i-1],lx,ly);
    DijkstraMap.GetVertexPos(nodes[i],x,y);
    float theta=Trajectory::gen_axis_angle(Pose2d(lx,ly,0),Pose2d(x,y,0));
    begin=Pose2d(lx,ly,theta);
    NavMessage::Action act_adjust;
    act_adjust.set_type(1);
    act_adjust.mutable_target()->set_x(lx);
    act_adjust.mutable_target()->set_y(ly);
    act_adjust.mutable_target()->set_theta(theta);
    act_adjust.mutable_target_diff()->CopyFrom(adjustdiff);
    act_adjust.mutable_velocity_limit()->CopyFrom(v_limit);
    act_adjust.mutable_horize_limit()->CopyFrom(horize_limit);
    act_adjust.mutable_angular_limit()->CopyFrom(angular_limit);
    cmd.add_actions()->CopyFrom(act_adjust);

    //MPC 控制
    NavMessage::Action act_along;
    act_along.set_type(2);
    act_along.mutable_begin()->set_x(begin.x());
    act_along.mutable_begin()->set_y(begin.y());
    act_along.mutable_begin()->set_theta(begin.theta());
    act_along.mutable_target()->set_x(x);
    act_along.mutable_target()->set_y(y);
    act_along.mutable_target()->set_theta(theta);
    if(i==nodes.size()-1) //最后一个节点
      act_along.mutable_target_diff()->CopyFrom(enddiff);
    else
      act_along.mutable_target_diff()->CopyFrom(node_mpcdiff);
    act_along.mutable_velocity_limit()->CopyFrom(mpc_x_limit);
    act_along.mutable_angular_limit()->CopyFrom(mpc_angular_limit);
    cmd.add_actions()->CopyFrom(act_along);
  }
  return true;

}

bool CreateOutSpaceNavCmd(std::vector<int> nodes,NavMessage::NavCmd& cmd)
{

  if(nodes.size()==0)
    return false;

  cmd.set_action(0);  //开始新导航
  char key[255]={0};
  static int N=1111;
  sprintf(key,"abcd-efg-%d",N++);
  cmd.set_key(key);

  NavMessage::Pose2d headdiff,enddiff;
  headdiff.set_x(0.1);
  headdiff.set_y(0.1);
  headdiff.set_theta(1.0*M_PI/180.0);

  enddiff.set_x(0.03);
  enddiff.set_y(0.05);
  enddiff.set_theta(1.0*M_PI/180.0);


  if(timed_AgvStatu_.timeout())
  {
    printf(" agv statu is timeout can not get current direction\n");
    return false;
  }
  float current_theta=timed_AgvStatu_.Get().theta();
  float x,y;
  DijkstraMap.GetVertexPos(nodes[0],x,y);

  NavMessage::SpeedLimit v_limit,angular_limit,horize_limit;
  v_limit.set_min(0.1);
  v_limit.set_max(0.2);
  horize_limit.set_min(0.05);
  horize_limit.set_max(0.2);
  angular_limit.set_min(1);
  angular_limit.set_max(30);

  NavMessage::SpeedLimit mpc_x_limit,mpc_angular_limit;
  mpc_x_limit.set_min(0.05);
  mpc_x_limit.set_max(1.0);

  mpc_angular_limit.set_min(0*M_PI/180.0);
  mpc_angular_limit.set_max(3*M_PI/180.0);

  NavMessage::Action act_head;
  act_head.set_type(1);
  act_head.mutable_target()->set_x(x);
  act_head.mutable_target()->set_y(y);
  act_head.mutable_target()->set_theta(current_theta);
  act_head.mutable_target_diff()->CopyFrom(headdiff);

  act_head.mutable_velocity_limit()->CopyFrom(v_limit);
  act_head.mutable_horize_limit()->CopyFrom(horize_limit);
  act_head.mutable_angular_limit()->CopyFrom(angular_limit);
  cmd.add_actions()->CopyFrom(act_head);

  Pose2d begin;
  for(int i=1;i<nodes.size();++i)
  {

    //调整到上一点位姿，朝向当前点
    float lx,ly;
    DijkstraMap.GetVertexPos(nodes[i-1],lx,ly);
    DijkstraMap.GetVertexPos(nodes[i],x,y);
    float theta=Trajectory::gen_axis_angle(Pose2d(lx,ly,0),Pose2d(x,y,0));
    //第一点倒退出车位
    if(i==1)
      theta=Trajectory::gen_axis_angle(Pose2d(x,y,0),Pose2d(lx,ly,0));

    begin=Pose2d(lx,ly,theta);
    NavMessage::Action act_adjust;
    act_adjust.set_type(1);
    act_adjust.mutable_target()->set_x(lx);
    act_adjust.mutable_target()->set_y(ly);
    act_adjust.mutable_target()->set_theta(theta);
    act_adjust.mutable_target_diff()->CopyFrom(headdiff);
    act_adjust.mutable_velocity_limit()->CopyFrom(v_limit);
    act_adjust.mutable_horize_limit()->CopyFrom(horize_limit);
    act_adjust.mutable_angular_limit()->CopyFrom(angular_limit);
    cmd.add_actions()->CopyFrom(act_adjust);

    //MPC 控制
    NavMessage::Action act_along;
    act_along.set_type(2);
    act_along.mutable_begin()->set_x(begin.x());
    act_along.mutable_begin()->set_y(begin.y());
    act_along.mutable_begin()->set_theta(begin.theta());
    act_along.mutable_target()->set_x(x);
    act_along.mutable_target()->set_y(y);
    act_along.mutable_target()->set_theta(theta);
    if(i==nodes.size()-1) //最后一个节点
      act_along.mutable_target_diff()->CopyFrom(enddiff);
    else
      act_along.mutable_target_diff()->CopyFrom(headdiff);
    act_along.mutable_velocity_limit()->CopyFrom(mpc_x_limit);
    act_along.mutable_angular_limit()->CopyFrom(mpc_angular_limit);
    cmd.add_actions()->CopyFrom(act_along);
  }
  return true;

}

bool CreateTestNavCmd(int begin,int space,int outnode,NavMessage::NavCmd& cmd)
{
  //去车位生成路径
  std::vector<int> t_shortestPath;
  float distance = -1;
  bool ret = DijkstraMap.GetShortestPath(begin, space, t_shortestPath, distance);
  if (ret) {
    pathMutex.lock();
    g_shortest_path = t_shortestPath;
    std::reverse(g_shortest_path.begin(), g_shortest_path.end());
    g_shortest_path.push_back(end_node);
    pathMutex.unlock();
  }
  //生成去车位指令
  NavMessage::NavCmd toCmd;


  return false;

}