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

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


Monitor_emqx::Monitor_emqx(std::string nodeId)
    : Terminator_emqx(nodeId)
{
  heat_=0;
}

Monitor_emqx::~Monitor_emqx()
{

}


void Monitor_emqx::set_speedcmd_topic(std::string speedcmd)
{
  speedcmd_topic_=speedcmd;
}

void Monitor_emqx::clamp_close(ActionMode mode)
{
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(eClampClode);

  speed.set_m(mode);
  speed.set_end(1);
  msg.fromProtoMessage(speed);
  Publish(speedcmd_topic_,msg);
}

void Monitor_emqx::clamp_open(ActionMode mode)
{
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(eClampOpen);

  speed.set_m(mode);
  speed.set_end(1);
  msg.fromProtoMessage(speed);
  Publish(speedcmd_topic_,msg);
}

void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a,double L)
{
  if(mode==eMain && type==eMPC &&fabs(L)<1e-3)
  {
    printf(" Main agv mpc must set wheelBase\n ");
    return;
  }
  double w=fabs(a)>0.001?a:0.0;
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(type);
  speed.set_v(v);
  speed.set_w(w);
  speed.set_l(L);

  speed.set_m(mode);
  speed.set_end(1);
  msg.fromProtoMessage(speed);
  Publish(speedcmd_topic_,msg);

}
void Monitor_emqx::set_speed(ActionMode mode,ActionType type,double v,double a)
{
  double w=fabs(a)>0.0001?a:0.0;
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(type);
  speed.set_v(v);
  speed.set_w(w);
  speed.set_m(mode);
  speed.set_end(1);
  msg.fromProtoMessage(speed);
  Publish(speedcmd_topic_,msg);

}

void Monitor_emqx::stop()
{
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(eStop);
  speed.set_v(0);
  speed.set_w(0);
  speed.set_end(1);
  msg.fromProtoMessage(speed);

  Publish(speedcmd_topic_,msg);
}