RoboidControl-cpp/EspIdf/EspIdfParticipant.cpp

#include "EspIdfParticipant.h"

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

namespace RoboidControl {
namespace EspIdf {

void LocalParticipant::Setup(int localPort,
                             const char* remoteIpAddress,
                             int remotePort) {
#if defined(IDF_VER)
  this->remoteIpAddress = remoteIpAddress;
  this->remotePort = remotePort;
  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;
  }

  struct sockaddr_in server_addr; //, client_addr;
  // socklen_t addr_len = sizeof(client_addr);
  // char recv_buffer[1024];
  //    int sockfd;

  // 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 up the server address structure
  memset(&server_addr, 0, sizeof(server_addr));
  server_addr.sin_family = AF_INET;
  server_addr.sin_port = htons(localPort);
  server_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*)&server_addr, sizeof(server_addr)) < 0) {
    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
    close(sockfd);
    vTaskDelete(NULL);
    return;
  }

  std::cout << "Wifi sync started to port " << this->remotePort << "\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;

  std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
}

void LocalParticipant::Receive() {
#if defined(IDF_VER)
  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);
  }
#endif
}

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

  struct sockaddr_in dest_addr;
  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 = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                   sizeof(dest_addr));
  if (err != 0)
    std::cout << "Send error\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->remotePort);
  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