RoboidControl-cpp/NetworkSync.cpp

#include "NetworkSync.h"
#include "Messages.h"

// #define RC_DEBUG 1

#ifdef RC_DEBUG
#include <Arduino.h>
#if ESP32
#define SERIALPORT Serial
#else
#define SERIALPORT Serial
#endif
#endif

#include "ControlCore/Messages.h"
#include "LinearAlgebra/Angle.h"
#include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h"
#include "float16/float16.h"

#include <string.h>

NetworkSync::NetworkSync(Roboid *roboid) {
  this->roboid = roboid;
  this->networkId = 0;
}

#include <Arduino.h>

void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
  // printf("Received msgId %d, length %d\n", buffer[0], bytecount);

  networkPerception->ProcessPacket(roboid, buffer, bytecount);

  switch (buffer[0]) {
  case NetworkIdMsg:
    ReceiveNetworkId();
    break;
  case CustomMsg:
    ReceiveCustom(bytecount);
    break;
  }
}

void NetworkSync::PublishClient() {
  unsigned char ix = 0;
  buffer[ix++] = ClientMsg;
  buffer[ix++] = this->networkId;
  PublishBuffer(ix);

#ifdef RC_DEBUG
  SERIALPORT.printf("Publish Device\n");
#endif
}

void NetworkSync::ReceiveNetworkId() {
  this->networkId = buffer[1];
#ifdef RC_DEBUG
  SERIALPORT.printf("Received network Id %d\n", this->networkId);
#endif
  SendThingInfo(roboid, true);
  printf("completed\n");
}

// void NetworkSync::SendInvestigate(InterestingThing *thing) {
//   unsigned char ix = 0;
//   buffer[ix++] = InvestigateMsg;
//   buffer[ix++] = thing->networkId;
//   buffer[ix++] = thing->id;
//   SendBuffer(ix);
// #ifdef RC_DEBUG
//   printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
// #endif
// }

void NetworkSync::ReceiveCustom(unsigned char packetSize) {
  unsigned char ix = 1; // first byte is the msgId
  unsigned char networkId = buffer[ix++];
  unsigned char thingId = buffer[ix++];
  // unsigned char len = buffer[ix++];
  unsigned char len = packetSize - 3;
  printf("Custom size = %d\n", len);
  if (len > 0) {
    unsigned char *bytes = new unsigned char[len];
    // printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
    for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
      bytes[bytesIx] = buffer[ix++];

    // networkId is not used right now, we assume networkId == 0
    Thing *thing = roboid->FindChild(thingId);
    printf("Found thing %s\n", thing->name);
    if (thing != nullptr)
      thing->ProcessBytes(bytes);

    delete bytes;
  }
}

void NetworkSync::PublishState(Roboid *roboid) {
  SendPose(roboid);
  PublishPerception(roboid);
}

void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
  if (thing == nullptr)
    return;

  SendThing(thing);
  SendName(thing);
  SendModel(thing);
  SendCustom(thing);
  SendPose(thing, true, false);

  if (recurse) {
    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
      Thing *child = thing->GetChild(childIx);
      if (child != nullptr)
        SendThingInfo(child, true);
    }
  }
}

void NetworkSync::SendThing(Thing *thing) {
  if (thing == nullptr)
    return;

  // unsigned char ix = 0;
  // buffer[ix++] = ThingMsg;
  // buffer[ix++] = this->networkId;
  // buffer[ix++] = thing->id;
  // buffer[ix++] = (unsigned char)thing->type;
  // Thing *parentThing = thing->GetParent();
  // if (parentThing != nullptr)
  //   buffer[ix++] = parentThing->id;
  // else
  //   buffer[ix++] = 0x00;
  // SendBuffer(ix);

  Thing *parent = thing->GetParent();
  Passer::Control::ThingMsg msg =
      Passer::Control::ThingMsg(this->networkId, thing->id, thing->type,
                                parent == nullptr ? 0 : parent->id);
  SendBuffer(msg.Serialize(this->buffer));

#ifdef RC_DEBUG
  printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
#endif
}

void NetworkSync::SendName(Thing *thing) {
  if (thing->name == nullptr)
    return;

  unsigned char len = strlen(thing->name);
  if (len > 255)
    return;

  // unsigned char ix = 0;
  // buffer[ix++] = NameMsg;
  // buffer[ix++] = this->networkId;
  // buffer[ix++] = thing->id;

  // buffer[ix++] = len;
  // for (unsigned char nameIx = 0; nameIx < len; nameIx++)
  //   buffer[ix++] = thing->name[nameIx];

  // SendBuffer(ix);
  Passer::Control::NameMsg msg =
      Passer::Control::NameMsg(this->networkId, thing->id, thing->name, len);
  SendBuffer(msg.Serialize(this->buffer));

  // Passer::Control::NameMsg::Send(this, thing);

#ifdef RC_DEBUG
  SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
                    thing->name);
#endif
}

void NetworkSync::SendModel(Thing *thing) {
  if (thing->modelUrl == nullptr)
    return;

  unsigned char len = strlen(thing->modelUrl);
  if (len > 255)
    return;

  // unsigned char ix = 0;
  // buffer[ix++] = ModelMsg;
  // buffer[ix++] = this->networkId;
  // buffer[ix++] = thing->id; // objectId
  // // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
  // // Messages::SendSpherical16(buffer, &ix, s);
  // Messages::SendFloat16(buffer, &ix, thing->modelScale);

  // buffer[ix++] = len;
  // for (int urlIx = 0; urlIx < len; urlIx++)
  //   buffer[ix++] = thing->modelUrl[urlIx];

  // SendBuffer(ix);
  Passer::Control::ModelUrlMsg msg = Passer::Control::ModelUrlMsg(
      this->networkId, thing->id, len, thing->modelUrl, thing->modelScale);
  SendBuffer(msg.Serialize(this->buffer));
}

void NetworkSync::SendCustom(Thing *thing) {
  unsigned char ix = 0;
  buffer[ix++] = NetworkSync::CustomMsg;
  buffer[ix++] = this->networkId;
  buffer[ix++] = thing->id;
  thing->SendBytes(buffer, &ix);

  if (ix > 3) // When ix <= 3 then there is no custom data
    SendBuffer(ix);
}

void NetworkSync::SendDestroyThing(InterestingThing *thing) {
  if (networkId == 0) // We're not connected to a site yet
    return;

  unsigned char ix = 0;
  buffer[ix++] = DestroyMsg;
  buffer[ix++] = this->networkId;
  buffer[ix++] = thing->id;
  SendBuffer(ix);

#if RC_DEBUG
  printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
#endif
}

void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
  if (this->networkId == 0) // We're not connected to a site yet
    return;

  thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
  thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
  if (force || thing->positionUpdated || thing->orientationUpdated) {
    unsigned char ix = 0;
    buffer[ix++] = PoseMsg;
    buffer[ix++] = this->networkId;
    buffer[ix++] = thing->id;
    unsigned char pattern = 0;
    if (force || thing->positionUpdated)
      pattern |= Pose_Position;
    if (force || thing->orientationUpdated)
      pattern |= Pose_Orientation;
    buffer[ix++] = pattern;
    Messages::SendSpherical16(buffer, &ix, thing->position);
    Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
    SendBuffer(ix);

    thing->positionUpdated = false;
    thing->orientationUpdated = false;

#if RC_DEBUG
    if (thing->id == 0) {
      Vector3 v = thing->position.ToVector3();
      printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
             this->networkId, thing->id, thing->position.distance,
             thing->position.direction.horizontal.InDegrees(), v.Right(),
             v.Up(), v.Forward(),
             thing->orientation.swing.horizontal.InDegrees());
    }
#endif
  }

  if (recurse) {
    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
      Thing *child = thing->GetChild(childIx);
      if (child != nullptr)
        SendPose(thing->GetChild(childIx), force, recurse);
    }
  }
}

void NetworkSync::PublishState(Sensor *sensor) {
  float *value = (float *)sensor->GetValue();
  if (value == nullptr)
    return;

  unsigned char ix = 0;
  buffer[ix++] = StateMsg;
  buffer[ix++] = sensor->type;
  Messages::SendFloat16(buffer, &ix, *value);
  PublishBuffer(ix);
}

void NetworkSync::PublishPerception(Roboid *roboid) {
  Perception *perception = roboid->perception;
  if (perception == nullptr)
    return;

  for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
       sensorIx++) {
    Sensor *sensor = perception->sensors[sensorIx];
    if (sensor == nullptr)
      continue;
    PublishState(sensor);
  }
  PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
}

void NetworkSync::PublishTrackedObjects(Roboid *roboid,
                                        InterestingThing **objects) {
  if (networkId == 0) // We're not connected to a site yet
    return;

  int n = 0;
  for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
    InterestingThing *obj = objects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->confidence > 0)
      PublishTrackedObject(roboid, obj);

    n++;
  }
}

void NetworkSync::PublishTrackedObject(Roboid *roboid,
                                       InterestingThing *thing) {
  if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
    return;
  }
  // printf("publish tracked\n");
  /*
    Spherical16 roboidPosition = roboid->GetPosition();
    SwingTwist16 roboidOrientation = roboid->GetOrientation();

    Spherical16 worldPosition =
        roboidPosition + roboidOrientation * object->position;
    SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
  */

  SwingTwist16 inv_originOrientation;
  Spherical16 originPosition;
  if (roboid->worldOrigin == nullptr) {
    inv_originOrientation = SwingTwist16::identity;
    originPosition = Spherical16::zero;
  } else {
    inv_originOrientation =
        SwingTwist16::Inverse(roboid->worldOrigin->orientation);
    originPosition = roboid->worldOrigin->position;
  }

  // SwingTwist16 inv_originOrientation =
  //     SwingTwist16::Inverse(roboid->worldOrigin->orientation);
  // Spherical16 originPosition = roboid->worldOrigin->position;

  SwingTwist16 worldOrientation = inv_originOrientation * thing->orientation;
  Spherical16 worldPosition =
      inv_originOrientation * (thing->position - originPosition);

  unsigned char ix = 0;
  buffer[ix++] = PoseMsg; // Position2DMsg;
  buffer[ix++] = this->networkId;
  buffer[ix++] = thing->id; // objectId;
  buffer[ix++] = Pose_Position | Pose_Orientation;
  Messages::SendSpherical16(buffer, &ix, worldPosition);
  Messages::SendSwingTwist(buffer, &ix, worldOrientation);
  SendBuffer(ix);

#if RC_DEBUG
  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);
#endif

  thing->updated = false;
}

void NetworkSync::SendText(const char *s) {
  unsigned char length;
  for (length = 0; length < 253; length++) {
    if (s[length] == '\0')
      break;
  }
  if (length >= 253)
    return;

  unsigned char ix = 0;
  buffer[ix++] = 0xB0;
  buffer[ix++] = length;
  for (int urlIx = 0; urlIx < length; urlIx++)
    buffer[ix++] = s[urlIx];

  SendBuffer(ix);
#ifdef RC_DEBUG
  printf("Sent Text %s\n", s);
#endif
}

#if ESP32
#include <Arduino.h>
#endif
void NetworkSync::SendInt(const int x) {
  String s = String(x);
  char length = s.length();

  unsigned char ix = 0;
  buffer[ix++] = 0xB0;
  buffer[ix++] = length;
  for (int urlIx = 0; urlIx < length; urlIx++)
    buffer[ix++] = s[urlIx];

  SendBuffer(ix);
}

// Low-level functions

// bool NetworkSync::SendBuffer(unsigned char bufferSize) { return false; }

// bool NetworkSync::PublishBuffer(unsigned char bufferSize) { return false; }

// void NetworkSync::SendCBuffer(unsigned char bufferSize, unsigned char
// *buffer) {
//}