//
// 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_h1(heat_);
  speed.set_t1(eClampClose);

  speed.set_m1(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_h1(heat_);
  speed.set_t1(eClampOpen);

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

void Monitor_emqx::clamp_fully_open(int mode)
{
    MqttMsg msg;
    NavMessage::ToAgvCmd speed;
    heat_=(heat_+1)%255;
    speed.set_h1(heat_);
    speed.set_t1(eClampFullyOpen);

    speed.set_m1(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_h1(heat_);
    speed.set_t1(eLifterRise);

    speed.set_m1(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_h1(heat_);
    speed.set_t1(eLifterDown);

    speed.set_m1(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_h1(heat_);
  speed.set_t1(type);
  speed.set_v1(velocity);
  speed.set_w1(w);
  speed.set_l1(L);

  speed.set_m1(mode);
  speed.set_end(1);
  printf("real down(v:%f, w:%f)\n",velocity, w);
  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_h1(heat_);
  speed.set_t1(type);
  speed.set_v1(v);
  speed.set_w1(w);
  speed.set_v2(v);
  speed.set_w2(w);
  speed.set_v3(v);
  speed.set_w3(w);
  speed.set_m1(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_h1(heat_);
  speed.set_t1(eStop);
  speed.set_v1(0);
  speed.set_w1(0);
  speed.set_v2(0);
  speed.set_w2(0);
  speed.set_v3(0);
  speed.set_w3(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;
//    }
    int count = 3;
    double w_correct[count];
    double velocity[count];
    for (int i = 0; i < count; ++i) {
        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
    }
    MqttMsg msg;
    NavMessage::ToAgvCmd agvCmd;
    heat_=(heat_+1)%255;
    agvCmd.set_h1(heat_);
    agvCmd.set_m1(mode);
    agvCmd.set_t1(type);
    agvCmd.set_v1(velocity[0]);
    agvCmd.set_w1(w_correct[0]);
    agvCmd.set_v2(velocity[1]);
    agvCmd.set_w2(w_correct[1]);
    agvCmd.set_v3(velocity[2]);
    agvCmd.set_w3(w_correct[2]);
    agvCmd.set_l1(L);
    agvCmd.set_p1(P);
    agvCmd.set_d1(D);

    agvCmd.set_end(1);
    msg.fromProtoMessage(agvCmd);
    Publish(speedcmd_topic_,msg);
}
//
//void Monitor_emqx::set_ToAgvCmd(int mode, ActionType type, double v[], double w[], double diff_yaw[], double L, int P, double D) {
////    if(mode == eDouble && type == eVertical && fabs(L) < 1e-3)
////    {
////        printf(" Main agv mpc must set wheelBase\n ");
////        return;
////    }
//    int count = 3;
//    double w_correct[count];
//    double velocity[count];
//    for (int i = 0; i < count; ++i) {
//        w_correct[i] =fabs(w[i])>0.0001?w[i]:0.0;
//        velocity[i] =fabs(v[i])>0.0001?v[i]:0.0;
//    }
//    for (int i = 0; i < 2; ++i) {
//        diff_yaw[i] =fabs(diff_yaw[i])>0.0001?diff_yaw[i]:0.0;
//    }
//    MqttMsg msg;
//    NavMessage::ToAgvCmd agvCmd;
//    heat_=(heat_+1)%255;
//    agvCmd.set_h1(heat_);
//    agvCmd.set_m1(mode);
//    agvCmd.set_t1(type);
//    agvCmd.set_v1(velocity[0]);
//    agvCmd.set_w1(w_correct[0]);
//    agvCmd.set_v2(velocity[1]);
//    agvCmd.set_w2(w_correct[1]);
//    agvCmd.set_v3(velocity[2]);
//    agvCmd.set_w3(w_correct[2]);
//    agvCmd.set_l1(L);
//    agvCmd.set_p1(P);
//    agvCmd.set_y1(diff_yaw[0]);
//    agvCmd.set_y2(diff_yaw[1]);
//    agvCmd.set_d1(D);
//
//    agvCmd.set_end(1);
//    msg.fromProtoMessage(agvCmd);
//    Publish(speedcmd_topic_,msg);
//}