//
// Created by zx on 23-3-14.
//

#include "RobotMonitorNode.h"
#include <unistd.h>

RobotMonitorNode::RobotMonitorNode(std::string nodeId,std::string pubTopic,std::string subTopic)
  :client_(0),thread_(0),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic),exit_(true)
{
  pose_=Pose2d(0,0,0);
  v_=0;
  vth_=0;
}
RobotMonitorNode::~RobotMonitorNode()
{
  if(thread_!= nullptr)
  {
    if(thread_->joinable())
    {
      exit_=true;
      thread_->join();
    }
    delete thread_;
    thread_= nullptr;
  }
  if(client_!= nullptr){
    client_->disconnect();
    delete client_;
    client_= nullptr;
  }
}

bool RobotMonitorNode::Connect(std::string ip,int port)
{
  if(client_!= nullptr)
  {
    client_->disconnect();
    delete client_;
  }
  client_=new Paho_client(nodeId_);
  bool ret= client_->connect(ip,port);
  if(ret)
  {
    while(!client_->isconnected()) usleep(1000);
    client_->subcribe(subTopic_,1,SpeedChangeCallback,this);
    if(thread_!= nullptr)
    {
      exit_=true;
      if(thread_->joinable())
        thread_->join();
      delete thread_;
    }
    exit_=false;
    thread_=new std::thread(&RobotMonitorNode::update,this);
  }
  return ret;

}

void RobotMonitorNode::update()
{
  auto last=std::chrono::steady_clock::now();
  while(1)
  {
    auto now=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(now - last);

    double time = double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
    float add_line=v_*time;
    float add_angular=vth_*time;
    Pose2d add_p(add_line,0,0);
    Pose2d addxy=add_p.rotate(pose_.theta());
    pose_=Pose2d(addxy.x()+pose_.x(),addxy.y()+pose_.y(),pose_.theta()+add_angular);


    if(client_!= nullptr)
    {
      MqttMsg msg;
      msg.fromStatu(pose_.x(), pose_.y(), pose_.theta(),v_,vth_);
      client_->publish(pubTopic_,1,msg);
      //printf("pub topic(%s):%s  length:%d\n",pubTopic_.c_str(),msg.data(),msg.length());
    }


    last=now;
    std::this_thread::sleep_for(std::chrono::milliseconds(30));
  }
}

void RobotMonitorNode::ChangeSpeed(double v ,double vth)
{
  printf(" node:%s change speed :(%.5f,%.5f)\n",nodeId_.c_str(),v,vth);
  v_=v;
  vth_=vth;
}

void RobotMonitorNode::SpeedChangeCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  RobotMonitorNode* robot=(RobotMonitorNode*)context;
  double v=0,vth=0;
  msg.toSpeed(v,vth);
  robot->ChangeSpeed(v,vth);
}