RoboidControl-cpp/Participants/ParticipantUDP.cpp

#include "ParticipantUDP.h"

#include "Participant.h"
#include "Thing.h"

#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"
#include "Posix/PosixParticipant.h"
#include "Windows/WindowsParticipant.h"

#include "Things/DifferentialDrive.h"
#include "Things/DistanceSensor.h"
#include "Things/TouchSensor.h"

#include <string.h>

namespace RoboidControl {

#pragma region ParticipantRegistry

ParticipantRegistry ParticipantUDPGeneric::registry;

RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
                                               unsigned int port) {
#if !defined(NO_STD)
  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
    if (participant == nullptr)
      continue;
    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
        participant->port == port) {
      // std::cout << "found participant " << participant->ipAddress << ":"
      //           << (int)participant->port << std::endl;
      return participant;
    }
  }
  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
            << std::endl;
#endif
  return nullptr;
}

RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
#if !defined(NO_STD)
  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
    if (participant == nullptr)
      continue;
    if (participant->networkId == participantId)
      return participant;
  }
  std::cout << "Could not find participant " << (int)participantId << std::endl;
#endif
  return nullptr;
}

RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
                                               unsigned int port) {
  RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
  Add(participant);
  return participant;
}

void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
  Participant* foundParticipant = Get(participant->networkId);
  // Get(participant->ipAddress, participant->port);

  if (foundParticipant == nullptr) {
#if defined(NO_STD)
    // this->things[this->thingCount++] = thing;
#else
    ParticipantRegistry::participants.push_back(participant);
#endif
    //   std::cout << "Add participant " << participant->ipAddress << ":"
    //             << participant->port << "[" << (int)participant->networkId
    //             << "]\n";
    //   std::cout << "participants " <<
    //   ParticipantRegistry::participants.size()
    //             << "\n";
    // } else {
    //   std::cout << "Did not add, existing participant " <<
    //   participant->ipAddress
    //             << ":" << participant->port << "[" <<
    //             (int)participant->networkId
    //             << "]\n";
  }
}

void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
  // participants.remove(participant);
}

#if defined(NO_STD)
RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
  return ParticipantRegistry::participants;
}
#else
const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
  return ParticipantRegistry::participants;
}
#endif

#pragma endregion ParticipantRegistry

RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
  // make a copy of the ip address string
  int addressLength = (int)strlen(ipAddress);
  int stringLength = addressLength + 1;
  char* addressString = new char[stringLength];
#if defined(_WIN32) || defined(_WIN64)
  strncpy_s(addressString, stringLength, ipAddress,
            addressLength);  // Leave space for null terminator
#else
  strncpy(addressString, ipAddress, addressLength);
#endif
  addressString[addressLength] = '\0';

  this->ipAddress = addressString;
  this->port = port;
}

bool RemoteParticipantUDP::Send(IMessage* msg) {
  // No message is actually sent, because this class has no networking
  // implementation
  return false;
}

#pragma region Init

ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
    : RemoteParticipantUDP("127.0.0.1", port) {
  this->name = "ParticipantUDP";
  this->remoteSite = nullptr;
  if (this->port == 0)
    this->isIsolated = true;
  registry.Add(this);

  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
  this->root->owner = this;
  this->root->name = "UDP Root";
  std::cout << "P2 " << (int)this->root << std::endl;
  this->Add(this->root);

  Participant::ReplaceLocalParticipant(*this);
}

ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
                                             int port,
                                             int localPort)
    : RemoteParticipantUDP("127.0.0.1", localPort) {
  this->name = "ParticipantUDP";
  if (this->port == 0)
    this->isIsolated = true;
  else
    this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
  registry.Add(this);

  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
  this->root->owner = this;
  this->root->name = "UDP Root";
  std::cout << "P1 " << (int)this->root << std::endl;
  this->Add(this->root);

  Participant::ReplaceLocalParticipant(*this);
}

void ParticipantUDPGeneric::begin() {
  if (this->isIsolated || this->remoteSite == nullptr)
    return;

  SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
}

#pragma endregion Init

#pragma region Update

// The update order
// 1. receive external messages
// 2. update the state
// 3. send out the updated messages
void ParticipantUDPGeneric::Update(bool recurse) {
  unsigned long currentTimeMs = Thing::GetTimeMs();

  if (this->isIsolated == false) {
    if (this->connected == false)
      begin();

    if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
      ParticipantMsg* msg = new ParticipantMsg(this->networkId);

      if (this->remoteSite == nullptr)
        this->Publish(msg);
      else
        this->Send(msg);

      delete msg;
      this->nextPublishMe = currentTimeMs + this->publishInterval;
    }

    this->ReceiveUDP();
  }

  UpdateMyThings();
  UpdateOtherThings();
}

void ParticipantUDPGeneric::UpdateMyThings() {
  for (Thing* thing : this->things) {
    if (thing == nullptr)  // || thing->GetParent() != nullptr)
      continue;

      // Why don't we do recursive?
    // Because when a thing creates a thing in the update,
    // that new thing is not sent out (because of hierarchyChanged)
    // before it is updated itself: it is immediatedly updated!
    thing->Update(false);

    if (thing->terminate)
      this->Remove(thing);
  }
}

void ParticipantUDPGeneric::UpdateOtherThings() {
#if defined(NO_STD)
  Participant** participants = Participant::registry.GetAll();
  for (int ix = 0; ix < Participant::registry.count; ix++) {
    Participant* participant = participants[ix];
#else
  for (Participant* participant : registry.GetAll()) {
#endif
    if (participant == nullptr || participant == this)
      continue;

    // Call only the Participant version of the Update.
    // This is to deal with the function where one of the (remote)
    // participants is actually a local participant
    participant->Participant::Update();
    if (this->isIsolated)
      continue;

    for (Thing* thing : participant->things) {
      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
      participant->Send(poseMsg);
      delete poseMsg;
      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
      participant->Send(binaryMsg);
      delete binaryMsg;
    }
  }
}

// Update
#pragma endregion

#pragma region Send

void ParticipantUDPGeneric::SendThingInfo(Participant* remoteParticipant,
                                          Thing* thing) {
  // std::cout << "Send thing info [" << (int)thing->id << "] \n";
  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
  remoteParticipant->Send(thingMsg);
  delete thingMsg;
  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
  remoteParticipant->Send(nameMsg);
  delete nameMsg;
  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
  remoteParticipant->Send(modelMsg);
  delete modelMsg;
  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
  remoteParticipant->Send(poseMsg);
  delete poseMsg;
  BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
  remoteParticipant->Send(binaryMsg);
  delete binaryMsg;
}

bool ParticipantUDPGeneric::Send(IMessage* msg) {
  if (this->remoteSite != nullptr)
    return this->remoteSite->Send(msg);

  return true;
}

void ParticipantUDPGeneric::PublishThingInfo(Thing* thing) {
  // std::cout << "Publish thing info" << thing->networkId << "\n";
  //  Strange, when publishing, the network id is irrelevant, because it is
  //  connected to a specific site...
  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
  this->Publish(thingMsg);
  delete thingMsg;
  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
  this->Publish(nameMsg);
  delete nameMsg;
  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
  this->Publish(modelMsg);
  delete modelMsg;
  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
  this->Publish(poseMsg);
  delete poseMsg;
  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
  this->Publish(customMsg);
  delete customMsg;
}

// Send
#pragma endregion

#pragma region Receive

void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
                                        char* senderIpAddress,
                                        unsigned int senderPort) {
  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
  //           << std::endl;
  RemoteParticipantUDP* sender =
      this->registry.Get(senderIpAddress, senderPort);
  if (sender == nullptr) {
    sender = this->registry.Add(senderIpAddress, senderPort);
#if !defined(NO_STD)
    // std::cout << "New remote participant " << sender->ipAddress << ":"
    //           << sender->port << std::endl;
#endif
  }

  ReceiveData(packetSize, sender);
}

void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
                                        RemoteParticipantUDP* sender) {
  unsigned char msgId = this->buffer[0];
  // std::cout << "receive msg " << (int)msgId << "\n";
  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
  switch (msgId) {
    case ParticipantMsg::id: {
      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
      bufferSize -= msg->length;
      Process(sender, msg);
      delete msg;
    } break;
    case NetworkIdMsg::id: {
      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
      bufferSize -= msg->length;
      Process(sender, msg);
      delete msg;
    } break;
    case InvestigateMsg::id: {
      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
      Process(sender, msg);
      delete msg;
    } break;
    case ThingMsg::id: {
      ThingMsg* msg = new ThingMsg(this->buffer);
      bufferSize -= msg->length;
      Process(sender, msg);
      delete msg;
    } break;
    case NameMsg::id: {
      NameMsg* msg = new NameMsg(this->buffer);
      bufferSize -= msg->length + msg->nameLength;
      Process(sender, msg);
      delete msg;
    } break;
    case ModelUrlMsg::id: {
      ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
      bufferSize -= msg->length + msg->urlLength;
      Process(sender, msg);
      delete msg;
    } break;
    case PoseMsg::id: {
      PoseMsg* msg = new PoseMsg(this->buffer);
      bufferSize -= msg->length;
      Process(sender, msg);
      delete msg;
    } break;
    case BinaryMsg::id: {
      BinaryMsg* msg = new BinaryMsg(this->buffer);
      bufferSize -= msg->length + msg->dataLength;
      Process(sender, msg);
      delete msg;
    } break;
    case TextMsg::id: {
      TextMsg* msg = new TextMsg(this->buffer);
      bufferSize -= msg->length + msg->textLength;
      Process(sender, msg);
      delete msg;
    } break;
    case DestroyMsg::id: {
      DestroyMsg* msg = new DestroyMsg(this->buffer);
      bufferSize -= msg->length;
      Process(sender, msg);
      delete msg;
    } break;
  };

// Check if the buffer has been read completely
#if !defined(NO_STD)
  if (bufferSize > 0)
    std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    ParticipantMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
            << "\n";
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    NetworkIdMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
            << " -> " << (int)msg->networkId << "\n";
#endif

  if (this->networkId != msg->networkId) {
    this->networkId = msg->networkId;

    std::cout << this->things.size() << " things\n";
    for (Thing* thing : this->things)
      this->SendThingInfo(sender, thing);
  }
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    InvestigateMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "]\n";
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    ThingMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
            << (int)msg->parentId << "\n";
#endif
  RemoteParticipantUDP* owner = registry.Get(msg->networkId);
  if (owner == nullptr) {
    owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
    owner->networkId = msg->networkId;
    registry.Add(owner);
  }

  Thing* thing = owner->Get(msg->networkId, msg->thingId);
  if (thing == nullptr) {
    bool isRemote = (sender->networkId != owner->networkId);
    thing = ProcessNewThing(owner, msg, isRemote);
    thing->id = msg->thingId;
    thing->type = msg->thingType;
    thing->isRemote = isRemote;
  }

  if (msg->parentId != 0) {
    thing->SetParent(owner->Get(msg->networkId, msg->parentId));
    if (thing->GetParent() == nullptr)
      std::cout << "Could not find parent" << std::endl;
  } else
    thing->SetParent(nullptr);
}

Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
                                              ThingMsg* msg,
                                              bool isRemote) {
  switch (msg->thingType) {
    case Thing::Type::DistanceSensor:
      return new DistanceSensor(owner->root);
    case Thing::Type::TouchSensor:
      return new TouchSensor(owner->root);
    case Thing::Type::DifferentialDrive:
      return new DifferentialDrive(owner->root);
    default:
      return new Thing(owner->root);
  }
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    NameMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
            << (int)msg->thingId << "] ";
#endif

  Thing* thing = sender->Get(msg->networkId, msg->thingId);
  if (thing != nullptr) {
    int nameLength = msg->nameLength;
    int stringLen = nameLength + 1;
    char* thingName = new char[stringLen];
#if defined(_WIN32) || defined(_WIN64)
    strncpy_s(thingName, stringLen, msg->name,
              stringLen - 1);  // Leave space for null terminator
#else
    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
    // ESP-IDF)
    strncpy(thingName, msg->name,
            nameLength);  // Leave space for null terminator
#endif
    thingName[nameLength] = '\0';
    thing->SetName(thingName);

#if !defined(NO_STD)
    std::cout << thing->GetName();
#endif
  }
#if !defined(NO_STD)
  std::cout << std::endl;
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    ModelUrlMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "]\n";
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    PoseMsg* msg) {
#if !defined(DEBUG) && !defined(NO_STD)
  std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
#endif
  Participant* owner = registry.Get(msg->networkId);
  if (owner == nullptr)
    return;

  Thing* thing = owner->Get(msg->networkId, msg->thingId);
  if (thing == nullptr)
    return;

  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
    thing->SetPosition(msg->position);
  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
    thing->SetOrientation(msg->orientation);
  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
    thing->SetLinearVelocity(msg->linearVelocity);
  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
    thing->SetAngularVelocity(msg->angularVelocity);
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    BinaryMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "]\n";
#endif

  Participant* owner = registry.Get(msg->networkId);
  if (owner != nullptr) {
    Thing* thing = owner->Get(msg->networkId, msg->thingId);
    if (thing != nullptr)
      thing->ProcessBinary(msg->data);
#if !defined(NO_STD)
    else {
#if defined(DEBUG)
      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
                << (int)msg->thingId << "]";
#endif
    }
#endif
  }
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    TextMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
            << (int)msg->text << "\n";
#endif
}

void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
                                    DestroyMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
            << (int)msg->thingId << "]\n";
#endif

  Thing* thing = sender->Get(msg->networkId, msg->thingId);
  if (thing != nullptr)
    this->Remove(thing);
}

// Receive
#pragma endregion

}  // namespace RoboidControl