//
// Created by zx on 23-2-22.
//

#include "mqttmsg.h"
#include <string.h>
#include "message.pb.h"
#include <google/protobuf/util/json_util.h>
using namespace google::protobuf;
MqttMsg::MqttMsg()
{
  data_= nullptr;
  length_=0;
}
MqttMsg::MqttMsg(char* data,const int length)
{
  data_=(char*)malloc(length);
  memcpy(data_,data,length);
  length_=length;
}
MqttMsg::MqttMsg(const MqttMsg& msg)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  data_=(char*)malloc(msg.length_);
  memcpy(data_,msg.data_,msg.length_);
  length_=msg.length_;
}
MqttMsg& MqttMsg::operator=(const MqttMsg& msg)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  data_=(char*)malloc(msg.length_);
  memcpy(data_,msg.data_,msg.length_);
  length_=msg.length_;
  return *this;
}

char* MqttMsg::data()const{
  return data_;
}
int MqttMsg::length()const{
  return length_;
}




MqttMsg::~MqttMsg(){
  if(data_!= nullptr)
  {
    free(data_);
    data_ = nullptr;
  }
  length_=0;
}


void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);

  AGVStatu statu;
  statu.set_x(x);
  statu.set_y(y);
  statu.set_theta(theta);
  statu.set_v(v);
  statu.set_vth(vth);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(statu,&json_str,option);

  length_=json_str.length()+1;
  data_=(char*)malloc(length_);
  sprintf(data_,"%s",json_str.c_str());
  //memcpy(data_,json_str.c_str(),length_-1);
  data_[length_-1]=0;

}

bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;

  AGVStatu statu;
  util::Status ret;
  try
  {
    ret = util::JsonStringToMessage(data(), &statu);
  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
  }

  if(ret.ok())
  {
    x=statu.x();
    y=statu.y();
    theta=statu.theta();
    v=statu.v();
    vth=statu.vth();

  }

  return ret.ok();
}



void MqttMsg::fromSpeed(double v, double vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  ChangeSpeedCmd cmd;
  cmd.set_v(v);
  cmd.set_vth(vth);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);

  length_=json_str.size()+1;
  data_=(char*)malloc(length_);
  //sprintf(data_,"%s",json_str.c_str());
  memcpy(data_,json_str.c_str(),length_);
  data_[length_-1]='\0';
}

bool MqttMsg::toSpeed(double &v, double &vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;

  ChangeSpeedCmd cmd;
  util::Status ret;
  try
  {
    printf("%s\n",data());
    ret = util::JsonStringToMessage(data(), &cmd);
    printf("v:%f,  vth:%f\n",v,vth);
  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
  }

  if(ret.ok())
  {
    v=cmd.v();
    vth=cmd.vth();

  }

  return ret.ok();
}