//
// Created by zx on 23-4-11.
//

#include "terminator_emqx.h"
#include "unistd.h"
Terminator_emqx::~Terminator_emqx()
{
  if(client_!= nullptr){
    client_->disconnect();
    delete client_;
    client_= nullptr;
  }
}

bool Terminator_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 Terminator_emqx::SendNavCmd(const NavMessage::NavCmd& cmd)
{
  MqttMsg msg;
  msg.fromProtoMessage(cmd);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("Send NavCmd failed : emqx client disconnected...\n");
}

void Terminator_emqx::set_statu_arrived(NavStatuCallback callback, void *context)
{
  StatuArrivedCallback_=callback;
  context_=context;
}


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

void Terminator_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  Terminator_emqx* terminator=(Terminator_emqx*)context;
  double x=0,y=0,theta=0,v=0,vth=0;
  NavMessage::NavStatu statu;
  if(msg.toProtoMessage(statu))
  {
    if (terminator->StatuArrivedCallback_)
      terminator->StatuArrivedCallback_(statu,terminator->context_);
  }
}