RoboidControl-cpp/EspIdf/EspIdfParticipant.cpp

#include "EspIdfParticipant.h"

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

#include <arpa/inet.h>
namespace RoboidControl {

void ParticipantUDP::SetupUDP(int localPort,
                           const char* remoteIpAddress,
                           int remotePort) {
  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->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (this->sock < 0) {
    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
    vTaskDelete(NULL);
    return;
  }

  /*
  // Set up the receiving(local) address
  struct sockaddr_in local_addr;
  memset(&local_addr, 0, sizeof(local_addr));
  local_addr.sin_family = AF_INET;
  local_addr.sin_port = htons(localPort);
  local_addr.sin_addr.s_addr =
      htonl(INADDR_ANY);  // Listen on all available network interfaces

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

  // Initialize the destination(remote) address
  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);
  this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
  // inet_pton(AF_INET, this->remoteSite->ipAddress,
  //           &this->dest_addr.sin_addr.s_addr);

    this->connected = true;

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

  // std::cout << "socket: " << (int)this->sock << std::endl;
  // ParticipantMsg* msg = new ParticipantMsg(this->networkId);
  // int bufferSize = msg->Serialize(this->buffer);
  // int err = sendto(this->sock, buffer, bufferSize, 0,
  //                  (struct sockaddr*)&dest_addr, sizeof(dest_addr));
  // if (errno != 0)
  //   std::cout << "AASend error " << err << " or " << errno << "\n";
  // else
  //   std::cout << "AASend SUCCESS\n";

  //SendTest();
}

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::ReceiveUDP() {
#if defined(IDF_VER)
/*
  struct pollfd fds[1];
  fds[0].fd = sock;
  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->sock, 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
}

ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}

ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
    : ParticipantUDPGeneric(ipAddress, port, localPort) {}

// bool ParticipantUDP::SendTest() {
// #if defined(IDF_VER)
//   // std::cout << "socket: " << (int)this->sock << std::endl;
//   // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
//   // size_t free_heap = xPortGetFreeHeapSize();
//   // std::cout <<  "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;

//   ParticipantMsg* msg = new ParticipantMsg(this->networkId);
//   int bSize = msg->Serialize(this->buffer);
//   // std::cout << "buffer size " << bSize << std::endl;
//   int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
//                    sizeof(dest_addr));
//   if (errno != 0)
//     std::cout << "BBSend error " << err << " or " << errno << "\n";
//   else
//     std::cout << "BBSend SUCCESS\n";

// #endif
//   return true;
// }

bool ParticipantUDP::Send(IMessage* msg) {
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
    return true;

  std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
            << " to " << this->remoteSite->ipAddress << std::endl;

  return this->SendTo(this->remoteSite, bufferSize);
}

bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
                            int bufferSize) {
#if defined(IDF_VER)
  uint16_t port = ntohs(dest_addr.sin_port);

  char ip_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
  std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";

  // Print the IP address and port
  // printf("IP Address: %s\n", ip_str);
  // printf("Port: %d\n", port);

  this->dest_addr.sin_port = htons(remoteParticipant->port);
  this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);

  int err = sendto(this->sock, buffer, bufferSize, 0,
                   (struct sockaddr*)&this->dest_addr, sizeof(this->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(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                   sizeof(dest_addr));
  if (err != 0)
    std::cout << "Publish error\n";
#endif
  return true;
};

}  // namespace RoboidControl