#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <set>

// Durata minima dell'impulso di uscita
#define MIN_PULSE_TIME 5000

// Pulsante per abilitare l'associazione del device
const byte Button = 0;

// Uscita attiva quando il device è in prossimità
const byte LED = 2;

// Per associare un dispositivo, questo deve essere molto vicino all'ESP32
int linkRSSI = -85;

// Quando il dispositivo è abbastanza vicino, esegui azione
int nearRSSI = -90;

// Variabile che diventa true quando il dispositivo è abbastanza vicino
bool isNearDevice = false;

// Viene usato un set (lista di elementi univoci) per memorizzare i device autorizzati
std::set<std::string> allowedDevices;


BLEScan* pBLEScan;
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice advertisedDevice) {

      if (advertisedDevice.haveServiceUUID()) {

        // Salva in una variabile std:struing una chiave univoca ce identifica il device
        std::string uuid = advertisedDevice.getServiceUUID().toString();
        //std::string mac_adr = advertisedDevice.getAddress().toString();
        //Serial.printf("MAC %S - UUID %s (%ddB)\n", mac_adr.c_str(), uuid.c_str(), advertisedDevice.getRSSI());

        // Controlla se il device è nella lista di quelli autorizzati
        bool authorized = ( allowedDevices.count(uuid) > 0);

        // Aggiungo alla lista dei device ammessi se molto vicino e pulsante premuto
        if (digitalRead(Button) == LOW) {
          Serial.println("Pulsante di abbinamento premuto");

          if (advertisedDevice.getRSSI() >= linkRSSI ) {
            // Se è già presente in lista -> ret.second == false
            auto ret = allowedDevices.emplace(uuid);
            if (ret.second) {
              Serial.printf("Dispositivo aggiunto alla lista: %s (%ddB)\n", uuid.c_str(), advertisedDevice.getRSSI());
            }
          }
        }

        // Se il dispositivo è autorizzato ed è abbastanza vicino, isNearDevice -> true
        if (advertisedDevice.getRSSI() > nearRSSI && authorized) {
          isNearDevice = true;
          Serial.printf("Dispositivo %s autorizzato (%ddB)\n", uuid.c_str(), advertisedDevice.getRSSI());
        }
      }
    }
};

// Task che esegue continuamente lo scan dei device BLE raggiungibili
void bleScanTask(void * taskPar) {
  Serial.print("Task \"BLE Scanning\" avviato sul core: ");
  Serial.println(xPortGetCoreID());

  while (true) {
    vTaskDelay(100 / portTICK_PERIOD_MS);
    BLEScanResults foundDevices = pBLEScan->start(1, true); // duration( seconds), is_continue
    pBLEScan->clearResults();                               // delete results from BLEScan buffer
  }

  Serial.println("Chiusura del task \"BLE scanning\"");
  vTaskDelete(NULL);
}


void setup() {
  Serial.begin(115200);
  Serial.println();
  pinMode(Button, INPUT);
  pinMode(LED, OUTPUT);
  digitalWrite(LED , LOW);

  Serial.println("Scanning...");
  BLEDevice::init("");
  pBLEScan = BLEDevice::getScan();  // Create new scan object
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setActiveScan(true);    // Active scan uses more power, but get results faster
  pBLEScan->setInterval(100);
  pBLEScan->setWindow(99);          // Less or equal setInterval value

  // Siccome l'esecuzione dello scanning è bloccante, facciamola in un task dedicato
  xTaskCreate(
    bleScanTask,     // Function to be called
    "BLE Scan task", // Name of the task (for debugging)
    2048,            // Stack size (bytes)
    NULL,            // Parameter to pass
    1,               // Task priority
    NULL             // Task handle
  );
}

void loop() {

  // Un dispositivo è passato abbastanza vicino, attivo un segnale
  static uint32_t pulseTime;
  if (isNearDevice) {
    pulseTime = millis();
    digitalWrite(LED , HIGH);
    isNearDevice = false;
  }

  // Il segnale è durato abbastanza -> reset
  if (millis() - pulseTime > MIN_PULSE_TIME) {
    digitalWrite(LED , LOW);
  }

}