//
// 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(int mode)
{
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(eClampClose);

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

void Monitor_emqx::clamp_open(int 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::lifter_rise(int mode)
{
    MqttMsg msg;
    NavMessage::ToAgvCmd speed;
    heat_=(heat_+1)%255;
    speed.set_h(heat_);
    speed.set_t(eLifterRise);

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

void Monitor_emqx::lifter_down(int mode)
{
    MqttMsg msg;
    NavMessage::ToAgvCmd speed;
    heat_=(heat_+1)%255;
    speed.set_h(heat_);
    speed.set_t(eLifterDown);

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

void Monitor_emqx::set_speed(int mode,ActionType type,double v,double a,double L)
{
  if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
  {
    printf(" Main agv mpc must set wheelBase\n ");
    return;
  }
  double w=fabs(a)>0.001?a:0.0;
  double velocity=fabs(v)>0.001?v:0;
  MqttMsg msg;
  NavMessage::ToAgvCmd speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(type);
  speed.set_v(velocity);
  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(int 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);
}

void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v, double w, double L, int P, double D) {
//    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
//    {
//        printf(" Main agv mpc must set wheelBase\n ");
//        return;
//    }
    double w_correct=fabs(w)>0.001?w:0.0;
    double velocity=fabs(v)>0.001?v:0;
    MqttMsg msg;
    NavMessage::ToAgvCmd agvCmd;
    heat_=(heat_+1)%255;
    agvCmd.set_h(heat_);
    agvCmd.set_m(mode);
    agvCmd.set_t(type);
    agvCmd.set_v(velocity);
    agvCmd.set_w(w_correct);
    agvCmd.set_l(L);
    agvCmd.set_p(P);
    agvCmd.set_d(D);

    agvCmd.set_end(1);
    msg.fromProtoMessage(agvCmd);
    Publish(speedcmd_topic_,msg);
}