RoboidControl-cpp/Participants/ParticipantUDP.cpp

#include "ParticipantUDP.h"

#include "Thing.h"

#include "Arduino/ArduinoParticipant.h"
#include "EspIdf/EspIdfParticipant.h"

#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.h>
#include "Windows/WindowsParticipant.h"
#pragma comment(lib, "ws2_32.lib")

#elif defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h>  // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#include <chrono>
#include "Posix/PosixParticipant.h"
#endif

#include <string.h>

namespace RoboidControl {

ParticipantUDP::ParticipantUDP(int port) {
  this->ipAddress = "0.0.0.0";
  this->port = port;
  this->remoteSite = nullptr;
  if (this->port == 0)
    this->isIsolated = true;
}

ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
    : Participant("127.0.0.1", localPort) {
  if (this->port == 0)
    this->isIsolated = true;
  else
    this->remoteSite = new Participant(ipAddress, port);
}

static ParticipantUDP* isolatedParticipant = nullptr;

ParticipantUDP* ParticipantUDP::Isolated() {
  if (isolatedParticipant == nullptr)
    isolatedParticipant = new ParticipantUDP(0);
  return isolatedParticipant;
}

void ParticipantUDP::begin() {
  if (this->isIsolated)
    return;

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

void ParticipantUDP::SetupUDP(int localPort,
                              const char* remoteIpAddress,
                              int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
  Windows::ParticipantUDP* thisWindows =
      static_cast<Windows::ParticipantUDP*>(this);
  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__)
  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO)
  Arduino::ParticipantUDP* thisArduino =
      static_cast<Arduino::ParticipantUDP*>(this);
  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(IDF_VER)
  EspIdf::ParticipantUDP* thisEspIdf =
      static_cast<EspIdf::ParticipantUDP*>(this);
  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
#endif
  this->connected = true;
}

void ParticipantUDP::Update(unsigned long currentTimeMs) {
  if (currentTimeMs == 0)
    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(this->remoteSite, msg);
      delete msg;

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

    this->ReceiveUDP();
  }

  for (Thing* thing : this->things) {
    if (thing == nullptr)
      continue;

    if (this->isIsolated == false) {
      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
      this->Send(thing->owner, poseMsg);
      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
      this->Send(thing->owner, binaryMsg);
      delete poseMsg;
    }
    thing->Update(currentTimeMs, true);
  }
}

void ParticipantUDP::ReceiveUDP() {
#if defined(_WIN32) || defined(_WIN64)
  Windows::ParticipantUDP* thisWindows =
      static_cast<Windows::ParticipantUDP*>(this);
  thisWindows->Receive();
#elif defined(__unix__) || defined(__APPLE__)
  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
  thisPosix->Receive();
#elif defined(ARDUINO)
  Arduino::ParticipantUDP* thisArduino =
      static_cast<Arduino::ParticipantUDP*>(this);
  thisArduino->Receive();
#elif defined(IDF_VER)
  EspIdf::ParticipantUDP* thisEspIdf =
      static_cast<EspIdf::ParticipantUDP*>(this);
  thisEspIdf->Receive();
#endif
}

Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
  for (Participant* sender : this->senders) {
    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
      return sender;
  }
  return nullptr;
}

Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
  Participant* participant = new Participant(ipAddress, port);
#if defined(NO_STD)
  participant->networkId = this->senderCount;
  this->senders[this->senderCount++] = participant;
#else
  participant->networkId = (unsigned char)this->senders.size();
  this->senders.push_back(participant);
#endif
  return participant;
}

#pragma region Send

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

bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
    return true;

#if defined(_WIN32) || defined(_WIN64)
  Windows::ParticipantUDP* thisWindows =
      static_cast<Windows::ParticipantUDP*>(this);
  return thisWindows->Send(remoteParticipant, bufferSize);
#elif defined(__unix__) || defined(__APPLE__)
  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
  return thisPosix->Send(remoteParticipant, bufferSize);
#elif defined(ARDUINO)
  Arduino::ParticipantUDP* thisArduino =
      static_cast<Arduino::ParticipantUDP*>(this);
  return thisArduino->Send(remoteParticipant, bufferSize);
#elif defined(IDF_VER)
  EspIdf::ParticipantUDP* thisEspIdf =
      static_cast<EspIdf::ParticipantUDP*>(this);
  return thisEspIdf->Send(remoteParticipant, bufferSize);
#else
  return false;
#endif
}

void ParticipantUDP::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;
}

bool ParticipantUDP::Publish(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
  Windows::ParticipantUDP* thisWindows =
      static_cast<Windows::ParticipantUDP*>(this);
  return thisWindows->Publish(msg);
#elif defined(__unix__) || defined(__APPLE__)
  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
  return thisPosix->Publish(msg);
#elif defined(ARDUINO)
  Arduino::ParticipantUDP* thisArduino =
      static_cast<Arduino::ParticipantUDP*>(this);
  return thisArduino->Publish(msg);
#elif defined(IDF_VER)
  EspIdf::ParticipantUDP* thisEspIdf =
      static_cast<EspIdf::ParticipantUDP*>(this);
  return thisEspIdf->Publish(msg);
#else
  return false;
#endif
}

// Send
#pragma endregion

#pragma region Receive

void ParticipantUDP::ReceiveData(unsigned char packetSize,
                                 char* senderIpAddress,
                                 unsigned int senderPort) {
  Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
  if (sender == nullptr) {
    sender = this->AddParticipant(senderIpAddress, senderPort);
    std::cout << "New remote participant " << sender->ipAddress << ":"
              << sender->port << std::endl;
  }

  ReceiveData(packetSize, sender);
}

void ParticipantUDP::ReceiveData(unsigned char bufferSize,
                                 Participant* 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 SiteMsg::id: {
      SiteMsg* msg = new SiteMsg(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;
  };

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

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

void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": process SiteMsg " << (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 ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
#if defined(DEBUG)
  std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "]\n";
#endif
}

void ParticipantUDP::Process(Participant* 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
}

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

  Thing* thing = sender->Get(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->name = thingName;

    std::cout << thing->name;
  }
  std::cout << std::endl;
}

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

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

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

  Thing* thing = sender->Get(msg->thingId);
  if (thing != nullptr)
    thing->ProcessBinary(msg->data);
  else {
    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
              << (int)msg->thingId << "]";
  }
  std::cout << std::endl;
}

// Receive
#pragma endregion

}  // namespace RoboidControl