#include <Arduino.h>
#include <WiFi.h>
#include <lwip/sockets.h>

#define _PASSWD "PASS0000"
#define UDP_PORT 6666

#define MTU 100000

WiFiUDP udp;
SemaphoreHandle_t netw_mtx;

String SSIDs[] = { "ESP32-AP1", "ESP32-AP2" };
int scanStatus = WIFI_SCAN_FAILED;
unsigned long lastScanRequest = 0;
int otherRSSI = -9999;
String otherSSID = "";

String readSerialUntil(const String& delimiter) {
  String res = "";
  unsigned long start = millis();

  while (!res.endsWith(delimiter) && (millis() - start < 1000)) {
    if (Serial.available()) {
      char c = Serial.read();
      res += c;
      start = millis();
    } else {
      // yield the CPU so the watchdog won’t fire
      taskYIELD();
    }
  }

  return res;
}

int findSSIDIndex(const String &ssid, size_t network_count) {
  for(int i = 0; i < network_count; i++) {
    if(WiFi.SSID(i) == ssid) return i;
  }
  return -1;
}

int getSignalStrengthFromStation(const String& ssid) {
  if(ssid == "") {
    if(WiFi.isConnected()) {
      return WiFi.RSSI();
    } else {
      return -9999;
    }
  }
  size_t found_network_count = WiFi.scanNetworks(false);
  int index = -1;
  if((index = findSSIDIndex(ssid, found_network_count)) == -1) return -9999;
  return WiFi.RSSI(index);
}

String getAvailableInfo() {
  String ret = "{\"type\":\"text\",\"IP\":\"";
  ret += WiFi.localIP().toString();
  ret += "\",\"Base Station\":\"";
  ret += WiFi.SSID();
  ret += "\",\"RSSI\":\"";
  ret += WiFi.RSSI();
  ret += "\",\"Other Network\":\"";
  ret += otherSSID;
  ret += "\",\"Other RSSI\":\"";
  ret += otherRSSI;
  ret += "\"}\n";

  return ret;
}

void send_udp(void *args){
  String* msg = static_cast<String*>(args);
  udp.beginPacket(WiFi.gatewayIP(), UDP_PORT);
  udp.write((uint8_t*)msg->c_str(), msg->length());
  udp.endPacket();
}

void invoke_fn_mtx(SemaphoreHandle_t mtx, void (*fn)(void*), void* args){
  xSemaphoreTake(mtx, portMAX_DELAY);
  fn(args);
  xSemaphoreGive(mtx);
}

void changeNetwork(){
  if (otherRSSI < -1000) return;
  if(WiFi.RSSI() < otherRSSI - 3){
    String networkSwitchJson = "{\"type\": \"info\", \"data\": \"netsw\"}"; 
    invoke_fn_mtx(netw_mtx, send_udp, (void*)&networkSwitchJson);

    String prevSSID = WiFi.SSID();
    otherRSSI = WiFi.RSSI();
    udp.stop();
    WiFi.disconnect();
    WiFi.begin(otherSSID, _PASSWD);
    if (WiFi.waitForConnectResult() == WL_CONNECTED) {
      udp.begin(UDP_PORT);
      otherSSID = prevSSID;
    }
  }
}

void udp_task(void* /*args*/) {
  int start = millis();
  while(true){
    if(Serial.available()){
      String recv = readSerialUntil("\n");
      invoke_fn_mtx(netw_mtx, send_udp, (void*)&recv);
    }

    if(millis() - start >= 1000){
      if(scanStatus == WIFI_SCAN_FAILED && millis() - lastScanRequest > 10000){
        scanStatus = WiFi.scanNetworks(true);
        lastScanRequest = millis();
      }
      int n = WiFi.scanComplete();
      if(n >= 0){
        for(int i = 0; i < n; i++){
          if(WiFi.SSID(i) == otherSSID){
            otherRSSI = WiFi.RSSI(i);
            break;
          }
        }
        WiFi.scanDelete();
        scanStatus = WIFI_SCAN_FAILED;
      }

      String recv = getAvailableInfo();
      invoke_fn_mtx(netw_mtx, send_udp, (void*)&recv);
      start = millis();
    }

    vTaskDelay(pdMS_TO_TICKS(1));
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial) { delay(10); }

  netw_mtx = xSemaphoreCreateMutex();

  WiFi.mode(WIFI_STA);

  int ap1_rssi = -9999;
  int ap2_rssi = -9999;
  
  int n = WiFi.scanNetworks();
  for(int i = 0; i < n; i++){
    String ssid = WiFi.SSID(i);
    int rssi = WiFi.RSSI(i);

    if(ssid == SSIDs[0]) ap1_rssi = rssi;
    if(ssid == SSIDs[1]) ap2_rssi = rssi;
  }

  WiFi.scanDelete();

  if(ap1_rssi >= ap2_rssi){
    WiFi.begin(SSIDs[0], _PASSWD);
    otherSSID = SSIDs[1];
    otherRSSI = ap2_rssi;
  }
  else{
    WiFi.begin(SSIDs[1], _PASSWD);
    otherSSID = SSIDs[0];
    otherRSSI = ap1_rssi;
  }

  if(WiFi.waitForConnectResult() != WL_CONNECTED) 
    Serial.println("WiFi-Verbindung fehlgeschlagen!");
  else{
    Serial.print("Verbunden mit ");
    Serial.print(WiFi.SSID());
    Serial.print(" (RSSI ");
    Serial.print(WiFi.RSSI());
    Serial.println(" dBm)");
  }

  udp.begin(UDP_PORT);

  xTaskCreatePinnedToCore( udp_task, "UDPReceiver", 4096, nullptr, 5, nullptr, 1);
}


void loop() {
  changeNetwork();
  vTaskDelay(pdMS_TO_TICKS(1000));
}