#include "ArduinoParticipant.h"

#if defined(ARDUINO)
#if defined(ARDUINO_ARCH_ESP8266)
#include <ESP8266WiFi.h>

#elif defined(ESP32)
#include <WiFi.h>

#elif defined(UNO_R4)
#include <WiFi.h>

#elif defined(ARDUINO_ARCH_RP2040)  // not functional, for future use
#include <WifiNINA.h>

#endif
#endif

namespace RoboidControl {
namespace Arduino {

void LocalParticipant::Setup(int localPort,
                             const char* remoteIpAddress,
                             int remotePort) {
#if defined(ARDUINO) && defined(HAS_WIFI)
  this->remoteIpAddress = remoteIpAddress;
  this->remotePort = remotePort;
  GetBroadcastAddress();

#if defined(UNO_R4)
  if (WiFi.status() == WL_NO_MODULE) {
    std::cout << "No network available!\n";
    return;
  }
#else
  if (WiFi.isConnected() == false) {
    std::cout << "No network available!\n";
    return;
  }
#endif
  udp.begin(localPort);

  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
#endif
}

void LocalParticipant::GetBroadcastAddress() {
#if defined(ARDUINO) && defined(HAS_WIFI)
  IPAddress broadcastAddress = WiFi.localIP();
  broadcastAddress[3] = 255;
  String broadcastIpString = broadcastAddress.toString();
  this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
  broadcastIpString.toCharArray(this->broadcastIpAddress,
                                broadcastIpString.length() + 1);
  std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
#endif
}

void LocalParticipant::Receive() {
#if defined(ARDUINO) && defined(HAS_WIFI)
  int packetSize = udp.parsePacket();
  while (packetSize > 0) {
    udp.read(buffer, packetSize);

    String senderAddress = udp.remoteIP().toString();
    char sender_ipAddress[16];
    senderAddress.toCharArray(sender_ipAddress, 16);
    unsigned int sender_port = udp.remotePort();

    // 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 = udp.parsePacket();
  }
#endif
}

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

  int n = 0;
  do {
    if (n > 0) {
      std::cout << "Retry sending\n";
      delay(10);
    }
    n++;
    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
    udp.write((unsigned char*)buffer, bufferSize);
  } while (udp.endPacket() == 0 && n < 10);

#endif
  return true;
}

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

  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 Arduino
}  // namespace RoboidControl