RoboidControl-cpp/EspIdf/EspIdfParticipant.cpp

#include "EspIdfParticipant.h"

#if defined(IDF_VER)
#include "esp_wifi.h"
#endif

namespace RoboidControl {
namespace EspIdf {

void ParticipantUDP::Setup() {//int localPort,
                          //  const char* remoteIpAddress,
                          //  int remotePort) {
#if defined(IDF_VER)
  std::cout << "Set up UDP\n";
  GetBroadcastAddress();

  wifi_ap_record_t ap_info;
  esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info);
  if (result != ESP_OK) {
    std::cout << "No network available!\n";
    return;
  }

  // Create a UDP socket
  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (this->sockfd < 0) {
    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
    vTaskDelete(NULL);
    return;
  }

  // Set up the server address structure
  struct sockaddr_in local_addr;
  memset(&local_addr, 0, sizeof(local_addr));
  local_addr.sin_family = AF_INET;
  local_addr.sin_port = htons(this->port);
  local_addr.sin_addr.s_addr =
      htonl(INADDR_ANY);  // Listen on all available network interfaces

  // Bind the socket to the address and port
  if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
      0) {
    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
    close(sockfd);
    vTaskDelete(NULL);
    return;
  }

  // Initialize the dest_addr structure
  memset(&this->dest_addr, 0,
         sizeof(this->dest_addr));  // Clear the entire structure
  this->dest_addr.sin_family = AF_INET;
  this->dest_addr.sin_port = htons(this->remoteSite->port);
  inet_pton(AF_INET, this->remoteSite->ipAddress,
            &this->dest_addr.sin_addr.s_addr);

  std::cout << "Wifi sync started local " << this->port << ", remote "
            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
            << "\n";
#endif  // IDF_VER
}

void ParticipantUDP::GetBroadcastAddress() {
#if defined(IDF_VER)
  // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...

  esp_netif_ip_info_t ip_info;
  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");

  // Get IP information (IP address, netmask, gateway)
  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
    std::cout << "Failed to get IP info\n";
    return;
  }

  ip_addr_t broadcast_addr = {};
  broadcast_addr.u_addr.ip4.addr =
      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;

  snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
           IP2STR(&broadcast_addr.u_addr.ip4));
  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
#endif  // IDF_VER
}

void ParticipantUDP::Receive() {
#if defined(IDF_VER)
  struct pollfd fds[1];
  fds[0].fd = sockfd;
  fds[0].events = POLLIN;  // We're looking for data available to read

  // Use poll() with a timeout of 0 to return immediately
  int ret = poll(fds, 1, 0);
  if (ret == -1) {
    std::cout << "poll() error\n";
    return;
  }

  // char buffer[1024];
  struct sockaddr_in source_addr;

  socklen_t addr_len = sizeof(source_addr);
  char sender_ipAddress[INET_ADDRSTRLEN];

  while (ret > 0 && fds[0].revents & POLLIN) {
    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
                              (struct sockaddr*)&source_addr, &addr_len);
    if (packetSize < 0) {
      std::cout << "recvfrom() error\n";
      return;
    } else if (packetSize == 0) {
      break;
    }

    // std::cout << "receiving " << packetSize << " bytes, msgId " <<
    // (int)this->buffer[0] << "\n";
    inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
    unsigned int sender_port = ntohs(source_addr.sin_port);

    ReceiveData(packetSize, sender_ipAddress, sender_port);

    ret = poll(fds, 1, 0);
    if (ret == -1) {
      std::cout << "poll() error\n";
      return;
    }
  }
  // std::cout << "no more messages\n";

#endif  // IDF_VER
}

bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
#if defined(IDF_VER)
  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
  //           << remoteParticipant->port << "\n";

  int err = sendto(this->sockfd, buffer, bufferSize, 0,
                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
  if (errno != 0)
    std::cout << "Send error " << err << " or " << errno << "\n";

#endif
  return true;
}

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

  struct sockaddr_in dest_addr;
  dest_addr.sin_family = AF_INET;
  dest_addr.sin_port = htons(this->port);
  inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                   sizeof(dest_addr));
  if (err != 0)
    std::cout << "Publish error\n";
#endif
  return true;
};

}  // namespace EspIdf
}  // namespace RoboidControl