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

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

Monitor_emqx::~Monitor_emqx()
{
  if(client_!= nullptr){
    client_->disconnect();
    delete client_;
    client_= nullptr;
  }
}

bool Monitor_emqx::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,StatuArrivedCallback,this);
  }
  return ret;

}

void Monitor_emqx::set_statu_arrived_callback(StatuCallback callback,void* context)
{
  StatuArrivedCallback_=callback;
  context_=context;
}
void Monitor_emqx::set_speed(SpeedType type,double v,double a)
{
  MqttMsg msg;
  NavMessage::Speed speed;
  heat_=(heat_++)%255;
  speed.set_h(heat_);
  speed.set_t(type);
  speed.set_v(v);
  speed.set_w(a);
  msg.fromProtoMessage(speed);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("set speed failed : emqx client disconnected...\n");
}

void Monitor_emqx::stop()
{
  MqttMsg msg;
  NavMessage::Speed speed;
  heat_=(heat_++)%255;
  speed.set_h(heat_);
  speed.set_t(eStop);
  speed.set_v(0);
  speed.set_w(0);
  msg.fromProtoMessage(speed);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("stop failed : emqx client disconnected...\n");
}

Monitor_emqx::Monitor_emqx(std::string nodeId,std::string pubTopic,std::string subTopic)
  :client_(nullptr),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic)
{
  heat_=0;
  StatuArrivedCallback_= nullptr;
  context_= nullptr;
}

void Monitor_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  Monitor_emqx* monitor=(Monitor_emqx*)context;
  NavMessage::AGVStatu statu;

  if(msg.toProtoMessage(statu))
  {
    if (monitor->StatuArrivedCallback_)
      monitor->StatuArrivedCallback_(statu.x(), statu.y(), statu.theta(), statu.v(), statu.vth(),monitor->context_);
  }
}