RoboidControl-cpp/EspIdf/EspIdfParticipant.cpp

#include "EspIdfParticipant.h"

#include "esp_wifi.h"
#include "lwip/sockets.h"

namespace RoboidControl {
namespace EspIdf {

void set_socket_blocking(int sock) {
  int flags = fcntl(sock, F_GETFL, 0);
  if (flags != -1) {
    fcntl(
        sock, F_SETFL,
        flags & ~O_NONBLOCK);  // Clear O_NONBLOCK flag to set to blocking mode
  }
}

void set_socket_non_blocking(int sock) {
  int flags = fcntl(sock, F_GETFL, 0);
  if (flags != -1) {
    fcntl(sock, F_SETFL,
          flags | O_NONBLOCK);  // Set socket to non-blocking mode
  }
}

void LocalParticipant::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 socket to non-blocking mode
  // int flags = fcntl(sockfd, F_GETFL, 0);
  // if (flags < 0) {
  //   std::cout << "fcntl failed";
  //   close(sockfd);
  //   return;
  // }
  // fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);
  // set_socket_non_blocking(sockfd);

  // 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;
  }

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

void LocalParticipant::GetBroadcastAddress() {
  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;

  this->broadcastIpAddress = new char[16];  // IPv4 address can have a max of 15
                                            // characters + null  terminator
  snprintf(this->broadcastIpAddress, 16, IPSTR,
           IP2STR(&broadcast_addr.u_addr.ip4));
  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
}

void LocalParticipant::Receive() {
#if defined(IDF_VER)
return; // disable receiving for now
  set_socket_non_blocking(sockfd);

  struct sockaddr_in client_addr;
  socklen_t addr_len = sizeof(client_addr);
  int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
                            (struct sockaddr*)&client_addr, &addr_len);
  while (packetSize > 0) {
    char sender_ipAddress[16];
    inet_ntoa_r(client_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
    unsigned int sender_port = client_addr.sin_port;

    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
    // sender_port); if (remoteParticipant == nullptr) {
    //   remoteParticipant = this->AddParticipant(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);
    ReceiveData(packetSize, sender_ipAddress, sender_port);
    packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
                          (struct sockaddr*)&client_addr, &addr_len);
  }
  set_socket_blocking(sockfd);

#endif
}

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

  struct sockaddr_in dest_addr;
  memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero));
  dest_addr.sin_family = AF_INET;
  dest_addr.sin_port = htons(remoteParticipant->port);
  inet_pton(AF_INET, remoteParticipant->ipAddress, &dest_addr.sin_addr.s_addr);

  int err = 0;
  // int n = 0;
  // do {
  //   if (n > 0) {
  //     std::cout << "Retry sending\n";
  //     vTaskDelay(pdMS_TO_TICKS(10));  // Wait 10ms
  //   }
  //   n++;
  //   err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
  //                sizeof(dest_addr));

  // } while (errno == EAGAIN && n < 10);

  err = sendto(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 LocalParticipant::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