RoboidControl-cpp/Windows/WindowsParticipant.cpp

#include "WindowsParticipant.h"

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

namespace RoboidControl {
namespace Windows {

ParticipantUDP::ParticipantUDP() {}

void ParticipantUDP::Setup(int localPort,
                           const char* remoteIpAddress,
                           int remotePort) {
#if defined(_WIN32) || defined(_WIN64)

  // Create a UDP socket
  // Windows-specific Winsock initialization
  WSADATA wsaData;
  if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
    std::cerr << "WSAStartup failed" << std::endl;
    return;
  }

  // Create an UDP socket
  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (this->sock == INVALID_SOCKET) {
    std::cerr << "UDP Socket creation failed: " << WSAGetLastError()
              << std::endl;
    WSACleanup();
    return;
  }

  // Set the socket to non-blocking mode
  u_long mode = 1;  // 1 to enable non-blocking socket
  if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
    std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
              << std::endl;
    closesocket(sock);
    WSACleanup();
    return;
  }

  if (remotePort != 0) {
    // Define the server address
    memset(&remote_addr, 0, sizeof(remote_addr));
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons((u_short)remotePort);
    if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
      std::cerr << "Invalid address" << std::endl;
      closesocket(sock);
      WSACleanup();
      return;
    }
  }

  // Set up the receiving address
  memset(&server_addr, 0, sizeof(server_addr));
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons((u_short)localPort);
  if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
    std::cerr << "Invalid address" << std::endl;
    closesocket(sock);
    WSACleanup();
    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;
    closesocket(sock);
    WSACleanup();
  }

#endif  // _WIN32 || _WIN64
}

void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
#if defined(_WIN32) || defined(_WIN64)
  // Initialize Winsock
  WSADATA wsaData;
  if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
    std::cerr << "WSAStartup failed: " << std::endl;
    return;
  }

  // Create a TCP socket
  this->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
  if (sock == INVALID_SOCKET) {
    std::cerr << "TCP Socket creation failed: " << WSAGetLastError()
              << std::endl;
    WSACleanup();
    return;
  }

  // Set the socket to non-blocking mode
  u_long mode = 1;  // 1 to enable non-blocking socket
  if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
    std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
              << std::endl;
    closesocket(sock);
    WSACleanup();
    return;
  }

  // Define the server address
  memset(&this->remote_addr, 0, sizeof(server_addr));
  this->remote_addr.sin_family = AF_INET;
  this->remote_addr.sin_port = htons((u_short)remotePort);
  if (inet_pton(AF_INET, remoteIpAddress, &this->remote_addr.sin_addr) <= 0) {
    std::cerr << "Invalid address" << std::endl;
    closesocket(sock);
    WSACleanup();
    return;
  }

  // Connect to the server
  if (connect(sock, (sockaddr*)&this->remote_addr, sizeof(this->remote_addr)) ==
      SOCKET_ERROR) {
    std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
    closesocket(sock);
    WSACleanup();
    return;
  }
#endif  // _WIN32 || _WIN64
}

void ParticipantUDP::Receive() {
#if defined(_WIN32) || defined(_WIN64)
  // char ip_str[INET_ADDRSTRLEN];
  // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
  // std::cout << this->name << " Receive on " << ip_str << ":"
  //           << ntohs(server_addr.sin_port) << "\n";

  sockaddr_in client_addr;
  int len = sizeof(client_addr);
  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
                            (struct sockaddr*)&client_addr, &len);
  // std::cout << "received data " << packetSize << "\n";
  if (packetSize < 0) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
    if (error_code != WSAEWOULDBLOCK)
      std::cerr << "recvfrom failed with error: " << error_code << std::endl;
  } else 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::ParticipantUDP* 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);
  }
#endif  // _WIN32 || _WIN64
}

int ParticipantUDP::ReceiveTCP() {
#if defined(_WIN32) || defined(_WIN64)
  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 {
    int error = WSAGetLastError();
    if (error == WSAEWOULDBLOCK) {
      // No data available, continue with other tasks
      std::cout << "No data available, continuing..." << std::endl;
      // You can add a sleep or other logic here to avoid busy waiting
    } else {
      std::cerr << "Receive failed: " << error << std::endl;
    }
    return 0;
  }
#endif  // _WIN32 || _WIN64
  return 0;
}

bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
  char ip_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
  std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port)
            << "\n";
  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
                          (struct sockaddr*)&remote_addr, sizeof(remote_addr));

  if (sent_bytes <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
    std::cerr << "sendto failed with error: " << error_code << std::endl;
    closesocket(sock);
    WSACleanup();
    return false;
  }
#endif  // _WIN32 || _WIN64
  return true;
}

bool ParticipantUDP::SendTCP(int bufferSize) {
#if defined(_WIN32) || defined(_WIN64)
  send(sock, this->buffer, bufferSize, 0);
#endif
  return false;
}

bool ParticipantUDP::Publish(IMessage* msg) {
#if defined(_WIN32) || defined(_WIN64)
  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 <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
    std::cerr << "sendto failed with error: " << error_code << std::endl;
    closesocket(sock);
    WSACleanup();
    return false;
  }
#endif  // _WIN32 || _WIN64
  return true;
}

}  // namespace Windows
}  // namespace RoboidControl