RoboidControl-cpp/ControlCore/Messages.cpp

#include "Messages.h"

#include "LowLevelMessages.h"
#include "string.h"

#pragma region IMessage

unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }

bool IMessage::SendMsg(Participant *client, IMessage msg) {
  // return SendMsg(client, client.buffer, );nameLength
  return client->SendBuffer(msg.Serialize(client->buffer));
}

#pragma endregion

#pragma region Investigate

InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
  this->networkId = networkId;
  this->thingId = thingId;
}

unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  return ix;
}

bool InvestigateMsg::Send(Participant *client, unsigned char networkId,
                          unsigned char thingId) {
  InvestigateMsg msg = InvestigateMsg(networkId, thingId);
  return SendMsg(client, msg);
}

// Investigate
#pragma endregion

#pragma region Thing

ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
                   unsigned char thingType, unsigned char parentId) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->thingType = thingType;
  this->parentId = parentId;
}

unsigned char ThingMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->thingType;
  buffer[ix++] = this->parentId;
  return ix;
}

bool ThingMsg::Send(Participant *client, unsigned char networkId,
                    unsigned char thingId, unsigned char thingType,
                    unsigned char parentId) {
  ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
  return SendMsg(client, msg);
}

// Thing
#pragma endregion

#pragma region Name

NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
                 const char *name, unsigned char nameLength) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->name = name;
  this->nameLength = nameLength;
}

unsigned char NameMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->nameLength;
  for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
    buffer[ix++] = this->name[nameIx];

  return ix;
}

bool NameMsg::Send(Participant *client, CoreThing *thing,
                   unsigned char nameLength) {
  if (thing->name == nullptr)
    return true; // nothing sent, but still a success!

  if (strlen(thing->name) == 0)
    return true;

  NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name, nameLength);
  return SendMsg(client, msg);
}

// Name
#pragma endregion

#pragma region ModelUrl

ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
                         unsigned char urlLength, const char *url,
                         float scale) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->urlLength = urlLength;
  this->url = url;
  this->scale = scale;
}

unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
  buffer[ix++] = this->urlLength;
  for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
    buffer[ix++] = url[urlIx];
  return ix;
}

// Model Url
#pragma endregion

#pragma region PoseMsg

#include <Arduino.h>
PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
                 unsigned char poseType, Spherical16 position,
                 SwingTwist16 orientation) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->position = position;
  this->orientation = orientation;
  this->poseType = poseType;
}

unsigned char PoseMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = PoseMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->poseType;
  LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
  printf("send spherical %f %f\n", this->position.distance,
         this->position.direction.horizontal.InDegrees());
  LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
  return ix;
}

// Pose
#pragma endregion