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)
  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
  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  if (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(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() {
  // 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";
}

void ParticipantUDP::Receive() {
#if defined(IDF_VER)
  struct pollfd fds;
  fds.fd = sockfd;
  fds.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;
  }
  socklen_t addr_len = sizeof(this->src_addr);
  char sender_ipAddress[INET_ADDRSTRLEN];

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

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

    // ReceiveData(packetSize, sender_ipAddress, sender_port);

    int ret = poll(&fds, 1, 0);
    if (ret == -1) {
      std::cout << "poll() error\n";
      return;
    }
  }

#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";
  //   udp.beginPacket(this->broadcastIpAddress, this->remotePort);
  //   udp.write((unsigned char*)buffer, bufferSize);
  //   udp.endPacket();

  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
  //             << this->remotePort << "\n";
#endif
  return true;
};

}  // namespace EspIdf
}  // namespace RoboidControl