RoboidControl-cpp/Participant.cpp

#include "Participant.h"

#include "Thing.h"

#include "UdpArduino.h"
#include "UdpPosix.h"
#include "UdpWindows.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 <chrono>
#include <fcntl.h> // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif

namespace Passer {
namespace RoboidControl {

Participant::Participant() {}

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

  this->participants.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->participants.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)
  UdpWindows *thisWindows = static_cast<UdpWindows *>(this);
  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(__unix__) || defined(__APPLE__)
  UdpPosix *thisPosix = static_cast<UdpPosix *>(this);
  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
#elif defined(ARDUINO)
  UdpArduino *thisArduino = static_cast<UdpArduino *>(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) {
    ClientMsg *msg = new ClientMsg(this->networkId);
    this->Publish(msg);
    delete msg;
    std::cout << this->name << " published ClientMsg\n";
    this->nextPublishMe = currentTimeMs + this->publishInterval;
  }
  this->ReceiveUDP();

  this->UpdateAll(currentTimeMs);
  this->UpdateAll(currentTimeMs);
}

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

Participant *Participant::GetParticipant(const char *ipAddress, int port) {
  for (Participant *participant : this->participants) {
    if (participant->ipAddress == ipAddress && participant->port == port)
      return participant;
  }
  return nullptr;
}

Participant *Participant::AddParticipant(const char *ipAddress, int port) {
  Participant *participant = new Participant(ipAddress, port);
  participant->networkId = (unsigned char)this->participants.size();
  this->participants.push_back(participant);
  return participant;
}

#pragma region Send

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

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

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

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

// Send
#pragma endregion

#pragma region Receive

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

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

void Participant::Process(Participant *sender, NetworkIdMsg *msg) {
  std::cout << this->name << " receive network id " << (int)this->networkId
            << " " << (int)msg->networkId << "\n";
  if (this->networkId != msg->networkId) {
    this->networkId = msg->networkId;
    // Thing **allThings = Thing::GetAllThings();
    // for (uint16_t ix = 0; ix < THING_STORE_SIZE; ix++) {
    //   Thing *thing = allThings[ix];
    //   if (thing == nullptr)
    //     continue;

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

void Participant::Process(InvestigateMsg *msg) {}

void Participant::Process(ThingMsg *msg) {}

void Participant::Process(NameMsg *msg) {
  Thing *thing = this->Get(msg->networkId, msg->thingId);
  if (thing != nullptr) {
    int nameLength = msg->nameLength;
    char *thingName = new char[nameLength + 1];
    strcpy(thingName, msg->name);
    thingName[nameLength] = '\0';
    thing->name = thingName;
    std::cout << "thing name = " << thing->name << " length = " << nameLength
              << "\n";
  }
}

void Participant::Process(PoseMsg *msg) {}

void Participant::Process(CustomMsg *msg) {
  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";
  // std::cout << "Processed custom msg\n";
}

// Receive
#pragma endregion

#pragma region Things

Thing *Participant::Get(unsigned char networkId, unsigned char thingId) {
  // std::cout << "Get " << (int)networkId << "/" << (int)thingId << " from "
  //           << this->things.size() << " things\n";
  for (auto &thing : this->things) {
    // std::cout << "  ? " << (int)thing->networkId << "/" << (int)thing->id
    //           << "\n";
    if (thing->networkId == networkId && thing->id == thingId) {
      return thing;
    }
  }
  return nullptr;
}

int Participant::Add(Thing *newThing) {
  for (Thing *thing : this->things) {
    if (thing == newThing) {
      std::cout << "Thing already exists, not adding\n";
      return thing->id;
    }
  }
  std::cout << "Adding " << (int)newThing->networkId << "/" << (int)newThing->id
            << "\n";
  this->things.push_back(newThing);
  return this->things.size();
}

void Participant::Remove(Thing *thing) {
  this->things.remove_if([thing](Thing *obj) { return obj == thing; });
  std::cout << "Removing " << thing->networkId << "/" << thing->id
            << " list size = " << this->things.size() << "\n";
}

void Participant::UpdateAll(unsigned long currentTimeMs) {
  // Not very efficient, but it works for now.

  for (Thing *thing : this->things) {
    if (thing != nullptr &&
        thing->GetParent() == nullptr) { // update all root things
      // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
      //           << "\n";
      thing->Update(currentTimeMs);
    }
  }
}

#pragma endregion

} // namespace Control
} // namespace Passer