RoboidControl-cpp/Posix/PosixParticipant.cpp

#include "PosixParticipant.h"
#if defined(__unix__) || defined(__APPLE__)

#if defined(__unix__) || defined(__APPLE__)
#include <arpa/inet.h>
#include <fcntl.h>  // For fcntl
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
#endif

namespace RoboidControl {

ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}

ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
    : ParticipantUDPGeneric(ipAddress, port, localPort) {}

void ParticipantUDP::SetupUDP(int localPort,
                              const char* remoteIpAddress,
                              int remotePort) {
#if defined(__unix__) || defined(__APPLE__)

  // Create a UDP socket

  this->sock = socket(AF_INET, SOCK_DGRAM, 0);
  if (this->sock < 0) {
    std::cerr << "Error creating socket" << std::endl;
    return;
  }

  // Set the socket to non-blocking mode
  int flags = fcntl(this->sock, F_GETFL, 0);
  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);

  if (remotePort != 0) {
    // Set up the address to send to
    memset(&remote_addr, 0, sizeof(remote_addr));
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(remotePort);
    if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
      std::cerr << "Invalid address" << std::endl;
      close(sock);
      return;
    }
  }

  // Set up the receiving address
  memset(&server_addr, 0, sizeof(server_addr));
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons(localPort);
  if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
    std::cerr << "Invalid address" << std::endl;
    close(sock);
    return;
  }

  // Bind the socket to the specified port
  if (bind(this->sock, (const struct sockaddr*)&server_addr,
           sizeof(server_addr)) < 0) {
    std::cerr << "Bind failed" << std::endl;
    close(sock);
  }

#endif
}

void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
#if defined(__unix__) || defined(__APPLE__)

  // Create a UDP socket

  this->sock = socket(AF_INET, SOCK_STREAM, 0);
  if (this->sock < 0) {
    std::cerr << "Error creating socket" << std::endl;
    return;
  }

  remote_addr.sin_family = AF_INET;
  remote_addr.sin_port = htons(remotePort);
  inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr);

  int connect_result =
      connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
  if (connect_result < 0) {  //} && errno != EINPROGRESS) {
    std::cerr << "Error connecting to server" << std::endl;
    close(this->sock);
  }

  std::cout << "TCP connection to " << remoteIpAddress << ":" << remotePort
            << "\n";

  // Set the socket to non-blocking mode
  int flags = fcntl(this->sock, F_GETFL, 0);
  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);

#endif
}

void ParticipantUDP::ReceiveUDP() {
#if defined(__unix__) || defined(__APPLE__)
  sockaddr_in client_addr;
  socklen_t len = sizeof(client_addr);
  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
                            (struct sockaddr*)&client_addr, &len);
  if (packetSize > 0) {
    char sender_ipAddress[INET_ADDRSTRLEN];
    inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress,
              INET_ADDRSTRLEN);
    unsigned int sender_port = ntohs(client_addr.sin_port);

    ReceiveData(packetSize, sender_ipAddress, sender_port);
    // RoboidControl::Participant* remoteParticipant =
    // this->Get(sender_ipAddress, sender_port); if (remoteParticipant ==
    // nullptr) {
    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
    //   // 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);
    // std::cout << "Received data\n";
  }
#endif
}

int ParticipantUDP::ReceiveTCP() {
#if defined(__unix__) || defined(__APPLE__)
  int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
  if (bytesReceived > 0) {
    buffer[bytesReceived] = '\0';  // Null-terminate the received data
    std::cout << "Received: " << buffer << std::endl;
    return bytesReceived;
  } else if (bytesReceived == 0) {
    // Connection has been gracefully closed
    std::cout << "Connection closed by the server." << std::endl;
    return 0;
  } else {
    if (errno == EAGAIN || errno == EWOULDBLOCK) {
      // No data available to read, continue the loop
      // std::cout << "No data available" << std::endl;
    } else {
      std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
    }
    return 0;
  }
#endif  // _WIN32 || _WIN64
  return 0;
}

bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
                            int bufferSize) {
#if defined(__unix__) || defined(__APPLE__)
  // std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
  // ntohs(remoteParticipant->port)
  //           << "\n";

  // Set up the destination address
  struct sockaddr_in dest_addr;
  memset(&dest_addr, 0, sizeof(dest_addr));
  dest_addr.sin_family = AF_INET;
  dest_addr.sin_port = htons(remoteParticipant->port);
  dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);

  // Send the message
  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
                          (struct sockaddr*)&dest_addr, sizeof(dest_addr));
  if (sent_bytes < 0) {
    std::cerr << "sendto failed with error: " << sent_bytes << "  "
              << strerror(errno) << std::endl;
    close(sock);
    return false;
  }
#endif
  return true;
}

// bool ParticipantUDP::SendTCP(int bufferSize) {
// #if defined(__unix__) || defined(__APPLE__)
//   // send(sock, this->buffer, bufferSize, 0);
//   ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0);
//   if (sent_bytes < 0) {
//     std::cerr << "Failed to send packet" << strerror(errno) << std::endl;
//     close(sock);
//     return -1;
//   }
// #endif
//   return false;
// }

bool ParticipantUDP::Publish(IMessage* msg) {
#if defined(__unix__) || defined(__APPLE__)
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
    return true;

  char ip_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
  std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port)
            << "\n";
  int sent_bytes =
      sendto(sock, this->buffer, bufferSize, 0,
             (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
  if (sent_bytes < 0) {
    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
    close(sock);
    return false;
  }
#endif
  return true;
}

}  // namespace RoboidControl

#endif