RoboidControl-cpp/Participant.cpp

#include "Participant.h"

#include "Thing.h"

#include "Arduino/Participant.h"
#include "Posix/Participant.h"
#include "Windows/Participant.h"

#if defined(_WIN32) || defined(_WIN64)
#include <winsock2.h>
#include <ws2tcpip.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>
#endif

namespace RoboidControl {

// Participant::Participant() {}

Participant::Participant(int port) {
  this->ipAddress = "0.0.0.0";
  this->port = port;

  // this->senders.push_back(this);

  // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
  this->localPort = port;
  // SetupUDP(randomPort, ipAddress, port);
}

Participant::Participant(const char* ipAddress, int port) {
  this->ipAddress = ipAddress;
  this->port = port;

  // this->senders.push_back(this);

  // int randomPort = (rand() % (65535 - 49152 + 1)) + 49152;
  this->localPort = port;  // randomPort;
  // SetupUDP(randomPort, ipAddress, port);
}

void Participant::begin() {
  SetupUDP(this->localPort, this->ipAddress, this->port);
}

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

void Participant::Update(unsigned long currentTimeMs) {
  if (currentTimeMs == 0) {
#if defined(ARDUINO)
    currentTimeMs = millis();
#elif defined(__unix__) || defined(__APPLE__)
    auto now = std::chrono::steady_clock::now();
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
    currentTimeMs = static_cast<unsigned long>(ms.count());
#endif
  }

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

  if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
    ParticipantMsg* msg = new ParticipantMsg(this->networkId);
    this->Publish(msg);
    delete msg;
    // std::cout << this->name << " published ParticipantMsg\n";

    // for (RemoteParticipant* sender : this->senders) {
    //   for (Thing* thing : this->things)
    //     SendThingInfo(sender, thing);
    // }

    this->nextPublishMe = currentTimeMs + this->publishInterval;
  }
  this->ReceiveUDP();

  for (Thing* thing : this->things) {
    if (thing != nullptr) {
      thing->Update(currentTimeMs);
      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
      for (RemoteParticipant* sender : this->senders)
        this->Send(sender, poseMsg);
      delete poseMsg;
    }
  }
}

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

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

RemoteParticipant* Participant::AddParticipant(const char* ipAddress, int port) {
  std::cout << "New Participant " << ipAddress << ":" << port << "\n";
  RemoteParticipant* participant = new RemoteParticipant(ipAddress, port);
  participant->networkId = (unsigned char)this->senders.size();
  this->senders.push_back(participant);
  return participant;
}

#pragma region Send

void Participant::SendThingInfo(RemoteParticipant* owner, Thing* thing) {
  std::cout << "Send thing info " << thing->id << " \n";
  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
  this->Send(owner, thingMsg);
  delete thingMsg;
  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
  this->Send(owner, nameMsg);
  delete nameMsg;
  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
  this->Send(owner, modelMsg);
  delete modelMsg;
  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
  this->Send(owner, poseMsg);
  delete poseMsg;
}

void Participant::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;
  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
  this->Publish(customMsg);
  delete customMsg;
}

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

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

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

// Send
#pragma endregion

#pragma region Receive

void Participant::ReceiveData(unsigned char packetSize, char* senderIpAddress, unsigned int senderPort) {
  RemoteParticipant* remoteParticipant = this->GetParticipant(senderIpAddress, senderPort);
  if (remoteParticipant == nullptr) {
    remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
    // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
    //           << "\n";
    // std::cout << "New remote participant " << remoteParticipant->ipAddress
    //           << ":" << remoteParticipant->port << " "
    //           << (int)remoteParticipant->networkId << "\n";
  }

  ReceiveData(packetSize, remoteParticipant);
}

void Participant::ReceiveData(unsigned char bufferSize, RemoteParticipant* remoteParticipant) {
  unsigned char msgId = this->buffer[0];
  // std::cout << "receive msg " << (int)msgId << "\n";
  switch (msgId) {
    case ParticipantMsg::id: {
      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case SiteMsg::id: {
      SiteMsg* msg = new SiteMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case InvestigateMsg::id: {
      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case ThingMsg::id: {
      ThingMsg* msg = new ThingMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case NameMsg::id: {
      NameMsg* msg = new NameMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case PoseMsg::id: {
      PoseMsg* msg = new PoseMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
    case BinaryMsg::id: {
      BinaryMsg* msg = new BinaryMsg(this->buffer);
      Process(remoteParticipant, msg);
      delete msg;
    } break;
  };
}

void Participant::Process(RemoteParticipant* sender, ParticipantMsg* msg) {}

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

void Participant::Process(RemoteParticipant* sender, ThingMsg* msg) {}

void Participant::Process(RemoteParticipant* sender, NameMsg* msg) {
  Thing* thing = sender->Get(msg->networkId, msg->thingId);
  if (thing != nullptr) {
    int nameLength = msg->nameLength;
    int stringLen = nameLength + 1;
    char* thingName = new char[stringLen];
    // Use strncpy with bounds checking for other platforms (Arduino, POSIX, ESP-IDF)
    strncpy(thingName, msg->name, stringLen - 1);  // Leave space for null terminator
    thingName[nameLength] = '\0';
    thing->name = thingName;
    std::cout << "thing name = " << thing->name << " length = " << nameLength << "\n";
  }
}

void Participant::Process(RemoteParticipant* sender, PoseMsg* msg) {}

void Participant::Process(RemoteParticipant* sender, BinaryMsg* msg) {
  // std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
  //           << (int)msg->networkId << "]\n";
  Thing* thing = this->Get(msg->networkId, msg->thingId);
  if (thing != nullptr)
    thing->ProcessBinary(msg->bytes);
  else
    std::cout << "custom msg for unknown thing [" << (int)msg->networkId << "/" << (int)msg->thingId << "]\n";
}

// Receive
#pragma endregion

}  // namespace RoboidControl