#include <Arduino_APDS9960.h>
#include <Arduino_HS300x.h>
#include <Arduino_LPS22HB.h>
#include <ArduinoBLE.h>

#define BLE_UUID_TEST_SERVICE               "4321"
#define BLE_UUID_COMANDO                    "1234"
BLEService BLE_servizio( BLE_UUID_TEST_SERVICE );
BLECharacteristic BLE_comando(BLE_UUID_COMANDO, BLEWrite | BLERead | BLENotify, "00000000000000000000000000000000000000000000000000");

void setup() {
  Serial.begin(9600); // velocita' della porta seriale
  if (APDS.begin()==1) {
    Serial.print("APDS inizializzato.");
    APDS.setGestureSensitivity(80); // sensibilita' nel riconoscimento dei gesti (verso alto, basso, destra e sinistra)
  }
  else
    Serial.print("Inizializzazione APDS fallita.");

  // inizializza la libreria per leggere temperatura ed umidita'
  HS300x.begin();

  if (BARO.begin() == 0) {
    Serial.println("Inizializzazione del sensore di pressione fallita.");
    while(1) ;
  }

  if (BLE.begin() == 0) {
    Serial.println("Inizializzazione del bluetooth fallita!");
    while(1) ;
  }
  BLE.setDeviceName("Nano 33 BLE Sense Rev 2");
  BLE.setLocalName("Nano 33 BLE Sense Rev 2");
  BLE.setAdvertisedService(BLE_servizio);
  // BLE add characteristics
  BLE_servizio.addCharacteristic(BLE_comando);
  // add service
  BLE.addService(BLE_servizio);
  // set the initial value for the characeristic:
  BLE_comando.writeValue("10000000000000000000000000000000000000000000000001", true);
  // start advertising
  BLE.advertise();
}

void leggi_prossimita() {
  while (APDS.proximityAvailable() == 0) ;
  // significato del numero letto:
  // -   0  =>  Vicino
  // - 255  =>  Lontano
  // -  -1  =>  Errore
  int prossimita = APDS.readProximity();
  Serial.print("Prossimita = ");
  Serial.println(prossimita);
  BLE_comando.writeValue("Prossimita = ", false);
  BLE_comando.writeValue(String(prossimita).c_str(), false);
  BLE_comando.writeValue("\n", false);
}
void allarme_prossimita(int num) {
  int allarme_scattato = 0;
  while (allarme_scattato == 0) {
    while(APDS.proximityAvailable() == 0) ;
    // significato del numero letto:
    // -   0  =>  Vicino
    // - 255  =>  Lontano
    // -  -1  =>  Errore
    int prossimita = APDS.readProximity();
    if (prossimita > num) {
      allarme_scattato = 1;
      Serial.print("Prossimita maggiore di ");
      Serial.println(num);
      BLE_comando.writeValue("Prossimita maggiore di ", false);
      BLE_comando.writeValue(String(num).c_str(), false);
      BLE_comando.writeValue("\n", false);
    }
  }
}

void leggi_colore() {
  int r, g, b, a;
  while (APDS.colorAvailable() == 0) ;
  APDS.readColor(r, g, b, a);
  Serial.print("Rosso = ");
  Serial.println(r);
  Serial.print("Verde = ");
  Serial.println(g);
  Serial.print("Blu   = ");
  Serial.println(b);
  BLE_comando.writeValue("Rosso = ", false);
  BLE_comando.writeValue(String(r).c_str(), false);
  BLE_comando.writeValue("\n", false);
  BLE_comando.writeValue("Verde = ", false);
  BLE_comando.writeValue(String(g).c_str(), false);
  BLE_comando.writeValue("\n", false);
  BLE_comando.writeValue("Blu  = ", false);
  BLE_comando.writeValue(String(b).c_str(), false);
  BLE_comando.writeValue("\n", false);
}

void leggi_temperatura() {
  float temperatura_attuale = HS300x.readTemperature(CELSIUS);
  Serial.print("Temperatura = ");
  Serial.print(temperatura_attuale);
  Serial.println(" C");
  BLE_comando.writeValue("Temperatura = ", false);
  BLE_comando.writeValue(String(temperatura_attuale).c_str(), false);
  BLE_comando.writeValue(" C\n", false);
}
void allarme_temperatura(float num) {
  int allarme_scattato = 0;
  while (allarme_scattato == 0) {
    float temperatura = HS300x.readTemperature(CELSIUS);
    if (temperatura > num) {
      allarme_scattato = 1;
      Serial.print("Temperatura maggiore di ");
      Serial.print(num);
      Serial.println(" C");
      BLE_comando.writeValue("Temperatura maggiore di ", false);
      BLE_comando.writeValue(String(num).c_str(), false);
      BLE_comando.writeValue(" C\n", false);
    }
  }
}

void leggi_umidita() {
  float umidita_attuale = HS300x.readHumidity();
  Serial.print("Umidita = ");
  Serial.print(umidita_attuale);
  Serial.println(" %");
  BLE_comando.writeValue("Umidita = ", false);
  BLE_comando.writeValue(String(umidita_attuale).c_str(), false);
  BLE_comando.writeValue(" %\n", false);
}
void allarme_umidita(float num) {
  int allarme_scattato = 0;
  while (allarme_scattato == 0) {
    float umidita = HS300x.readHumidity();
    if (umidita > num) {
      allarme_scattato = 1;
      Serial.print("Umidita maggiore di ");
      Serial.print(num);
      Serial.println(" %");
      BLE_comando.writeValue("Umidita maggiore di ", false);
      BLE_comando.writeValue(String(num).c_str(), false);
      BLE_comando.writeValue(" %\n", false);
    }
  }
}

void leggi_pressione() {
  float pressione_attuale = BARO.readPressure();
  Serial.print("Pressione = ");
  Serial.print(pressione_attuale);
  Serial.println(" kPa");

  BLE_comando.writeValue("Pressione = ", false);
  BLE_comando.writeValue(String(pressione_attuale).c_str(), false);
  BLE_comando.writeValue(" kPa\n", false);
}
void allarme_pressione(float num) {
  int allarme_scattato = 0;
  while (allarme_scattato == 0) {
    float pressione = BARO.readPressure();
    if (pressione > num) {
      allarme_scattato = 1;
      Serial.print("Pressione maggiore di ");
      Serial.print(num);
      Serial.println(" kPa");
      BLE_comando.writeValue("Pressione maggiore di ", false);
      BLE_comando.writeValue(String(num).c_str(), false);
      BLE_comando.writeValue(" kPa\n", false);
    }
  }
}

void gestisci_comando(String comando) {
  comando.trim();
  Serial.println("\n\n---------------------------");
  Serial.println(comando);
  // formato dei comandi:
  // leggi sensore
  // allarme sensore valore_allarme
  int fine_prima_parola = comando.indexOf(" ", 0);
  int sensore_sconosciuto = 1;
  if (fine_prima_parola >= 0) {
    if (comando.substring(0, fine_prima_parola) == "leggi") { // legge il valore di un sensore
      if (comando.substring(fine_prima_parola+1) == "prossimita") { // legge la prossimita' di un oggetto
        leggi_prossimita();
        sensore_sconosciuto = 0;
      }
      if (comando.substring(fine_prima_parola+1) == "colore") { // legge un colore di un oggetto e lo replica sul led integrato
        leggi_colore();
        sensore_sconosciuto = 0;
      }
      if (comando.substring(fine_prima_parola+1) == "temperatura") { // legge la temperatura
        leggi_temperatura();
        sensore_sconosciuto = 0;
      }
      if (comando.substring(fine_prima_parola+1) == "umidita") { // legge l'umidita'
        leggi_umidita();
        sensore_sconosciuto = 0;
      }
      if (comando.substring(fine_prima_parola+1) == "pressione") { // legge la pressione
        leggi_pressione();
        sensore_sconosciuto = 0;
      }
      if (sensore_sconosciuto == 1) {
        Serial.print("Sensore sconosciuto: ");
        Serial.println(comando.substring(fine_prima_parola+1));
        BLE_comando.writeValue("Sensore sconosciuto: ", false);
        BLE_comando.writeValue(comando.substring(fine_prima_parola+1).c_str(), false);
        BLE_comando.writeValue("\n", false);
      }
    }
    else
      if (comando.substring(0, fine_prima_parola) == "allarme") { // imposta un allarme per un sensore
        int fine_seconda_parola = comando.indexOf(" ", fine_prima_parola+1);
        if (comando.substring(fine_prima_parola+1, fine_seconda_parola) == "prossimita") { // legge la prossimita' di un oggetto
          allarme_prossimita(comando.substring(fine_seconda_parola+1).toInt());
          sensore_sconosciuto = 0;
        }
        if (comando.substring(fine_prima_parola+1, fine_seconda_parola) == "temperatura") {// legge la temperatura
          allarme_temperatura(comando.substring(fine_seconda_parola+1).toFloat());
          sensore_sconosciuto = 0;
        }
        if (comando.substring(fine_prima_parola+1, fine_seconda_parola) == "umidita") { // legge l'umidita'
          allarme_umidita(comando.substring(fine_seconda_parola+1).toFloat());
          sensore_sconosciuto = 0;
        }
        if (comando.substring(fine_prima_parola+1, fine_seconda_parola) == "pressione") { // legge la pressione
          allarme_pressione(comando.substring(fine_seconda_parola+1).toFloat());
          sensore_sconosciuto = 0;
        }
        if (sensore_sconosciuto == 1) {
          Serial.print("Sensore sconosciuto: ");
          Serial.println(comando.substring(fine_prima_parola+1, fine_seconda_parola));
          BLE_comando.writeValue("Sensore sconosciuto: ", false);
          BLE_comando.writeValue(comando.substring(fine_prima_parola+1, fine_seconda_parola).c_str(), false);
          BLE_comando.writeValue("\n", false);
        }
      }
      else {
        Serial.print("Comando sconosciuto\n");
        BLE_comando.writeValue("Comando sconosciuto\n", false);
      }
  }
  else {
    Serial.print("Comando sconosciuto\n");
    BLE_comando.writeValue("Comando sconosciuto\n", false);
  }
}

long previousMillis=0;
#define MASSIMA_LUNGHEZZA_COMANDO 50
#define DELAY_LETTURA_COMANDI_MS 1000
char comando_ricevuto[MASSIMA_LUNGHEZZA_COMANDO];

void pulisci_comando() {
  for (int i=0; i<MASSIMA_LUNGHEZZA_COMANDO; i++)
    comando_ricevuto[i] = 0;
}

void loop() {
  BLEDevice central = BLE.central();
  if (central)
  {
    Serial.print("Connesso a: ");
    Serial.println(central.address());
    while (central.connected()) {
      long currentMillis = millis();
      if (currentMillis - previousMillis >= DELAY_LETTURA_COMANDI_MS) {
        if( BLE_comando.written() ) {
          pulisci_comando();
          int bytes_letti = BLE_comando.readValue(comando_ricevuto, MASSIMA_LUNGHEZZA_COMANDO);
          gestisci_comando(comando_ricevuto);
        }
      }
    }
  }
}