#include "UdpArduino.h"

#if defined(ARDUINO)
#include <ESP8266WiFi.h>
#endif
namespace Passer {
namespace RoboidControl {

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

  if (WiFi.isConnected() == false) {
    std::cout << "No network available!\n";
    return;
  }
  udp.begin(this->localPort);

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

void UdpArduino::GetBroadcastAddress() {
#if ARDUINO
  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 UdpArduino::Receive() {
#if ARDUINO
  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);
    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);
    packetSize = udp.parsePacket();
  }
#endif
}

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

  udp.beginPacket(this->remoteIpAddress, this->remotePort);
  udp.write(buffer, bufferSize);
  udp.endPacket();

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

bool UdpArduino::Publish(IMessage *msg) {
#ifdef ARDUINO
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
    return true;

  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
  udp.write(buffer, bufferSize);
  udp.endPacket();

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

} // namespace Control
} // namespace Passer