#define TRIGGER_PIN 9 // Pin per il Trigger
#define ECHO_PIN 8 // Pin per l'Echo
#define BUTTON_PIN 2 // Pin del pulsante
#define BUZZER_PIN 10 // Pin del pulsante

#define BACKLIGHT_PIN 3  // Pin collegato al pin A del display

//Pin del display
#define RS 12
#define E 11
#define D4 4
#define D5 5
#define D6 6
#define D7 7

//Variabile per gestire inserimento allarme
volatile bool alarmSetted = true; //True = Allarme inserito, False = Allarme disattivato
volatile bool alarmExpired = false; //Flag gestita dal timer di 5 minuti 
volatile uint32_t alarmCounter = 0; // Per il timer di 5 minuti

//Variabili per misurazione distanza
volatile bool measurementComplete = false; // Flag per indicare che la misurazione è completa
volatile bool objectDetected = false;

//Variabili per inserimento codice
volatile uint8_t userCode[5] = {-1, -1, -1, -1, -1}; // Numero a 5 cifre
const uint8_t secretCode[5] = {1, 2, 3, 4, 5}; // Combinazione segreta a 5 cifre
volatile uint8_t posizione = 0; // Indice della cifra corrente

// Variabili per la cifra a rotazione
volatile uint8_t cifra = 0; //La cifra che ruota
volatile uint16_t rotationCounter = 0; // Contatore per mezzo secondo

//Variabili per evitare accensione ripetuta display
volatile bool clearDisplay = false; 
volatile uint32_t entryCounter = 0; // Contatore per i 90s dell'inserimento

//Variabili per debouncing pulsante
volatile bool debounceFlag = true; // Flag di debounce
volatile uint8_t debounceCounter = 0; // Contatore debounce

//Variabili per gestione stato sospensione
volatile uint32_t suspendedCounter = 0; //Timer sospensione

/* Per l'implementazione di una FSM senza stati finali
//Variabili per stato di benvenuto
volatile uint16_t welcomeCounter = 0;
volatile bool welcomeExpired = false;
*/

// Definizione degli stati
enum State {
    STANDBY,
    CODE_ENTRY,
    CODE_VALIDATION,
    ALARM,
    WELCOME,
    SUSPENDED
};

//Stato iniziale
State currentState = STANDBY;

void setup() {
  sensorSetup();
  lcd_init();
  buttonSetup(); //Inizializza il pulsante e il timer di debouncing
}

void loop() {
 switch (currentState) {
        case STANDBY:
        lcd_displayOff(); //Display spento
          getDistance(); //Misura la distanza
            if (objectDetected) {
              currentState = CODE_ENTRY; //Transita nello stato CODE_ENTRY
              timer2Setup(); //Resetta il timer2 che gestisce CODE_ENTRY
              resetInserimento(); //Resetta variabili inserimento (efficace passando da ALARM e WELCOME)
              lcd_displayOn();
            }
            break;

        case CODE_ENTRY:
        updateDisplay(); //Aggiorna il display con le nuove cifre
        getDistance(); //Misura la distanza
        if(!objectDetected) {
          currentState = SUSPENDED; //Transita nello stato SUSPENDED
        }
            break;
        
        case CODE_VALIDATION:
        currentState = WELCOME; //Parte da WELCOME

        for (uint8_t i = 0; i < 5; i++) {
          if (userCode[i] != secretCode[i]) {
            currentState = ALARM; // Se trova una cifra differente transita in ALARM
            clearDisplay = true; //Bisogna pulire il display
            break;
          }
        
        if (currentState == WELCOME) {
          alarmSetted = false; //Se rimane in Welcome disattiva l'allarme
        }
        lcd_command(0x01); // Pulisce il display
        }
            break;

        case WELCOME:
        lcd_command(0x80); // Posiziona il cursore all'inizio
        printWelcome();
        /*
        if  (welcomeExpired) {  // Per l'implementazione di una FSM senza stati finali
          welcomeExpired = false;
          alarmSetted = true;
          lcd_command(0x01); // Pulisce il display
          getDistance();
          if (objectDetected) {
            currentState = CODE_ENTRY;
          } else currentState = STANDBY;
          
        } */
            break;
        
        case ALARM:
        if (clearDisplay) {
          lcd_command(0x01); // Pulisce il display
          clearDisplay = false;
        }
        lcd_command(0x80); // Posiziona il cursore all'inizio
        printAlarm();

        PINB |= (1 << PINB2); //Attiva il buzzer

        if  (alarmExpired) {
          alarmExpired = false;
          PINB &= ~(1 << PINB2);  // Riporta BUZZER_PIN su LOW
          lcd_command(0x01); // Pulisce il display
          resetInserimento();
          getDistance();
          if (!objectDetected) {
            currentState = STANDBY;
          }
        }
            break;
        
        case SUSPENDED:
        getDistance();
        if (objectDetected) {
          currentState = CODE_ENTRY;
          suspendedCounter = 0;
        }
            break;
  } 
}

void sensorSetup() {
  // Impostazione dei pin 9 (TRIGGER_PIN) e 8 (ECHO_PIN) come output e input usando i registri
  DDRB |= (1 << DDB1);  // TRIGGER_PIN come output (pin 9)
  DDRB &= ~(1 << DDB0); // ECHO_PIN come input (pin 8)

  // Configuriamo il timer 0 in modalità normale con prescaler 8
  TCCR1A = 0;           // Modalità normale
  TCCR1B = (1 << CS11); // Prescaler 8, ogni tick = 0.5 µs
  TCNT1 = 0;            // Reset del contatore

  // Abilitazione dell'interrupt PCINT su pin 8
  PCICR |= (1 << PCIE0);    // Abilita interrupt per il gruppo PCINT0
  PCMSK0 |= (1 << PCINT0);  // Abilita interrupt sul pin 8 (PCINT0)
}

void getDistance() {
  if (!measurementComplete) {
    // Genera il segnale di Trigger
    PORTB &= ~(1 << PORTB1);  // Imposta TRIGGER_PIN su LOW
    PORTB |= (1 << PORTB1);   // Imposta TRIGGER_PIN su HIGH
    PORTB &= ~(1 << PORTB1);  // Riporta TRIGGER_PIN su LOW

    // Aspetta il completamento della misurazione
    while (!measurementComplete) {
      if (TCNT1 >= 47000) { 
        objectDetected = false;
      }
    }
  }
  // Resetta il flag per la prossima misurazione
  measurementComplete = false;
}

void lcd_init() {
  DDRD |= (1 << PD3) | (1 << PD4) | (1 << PD5) | (1 << PD6) | (1 << PD7); // Imposta D3-D7 come OUTPUT
  DDRB |= (1 << PB4) | (1 << PB3); // Imposta RS e E come OUTPUT
}

// Funzione per inviare un comando al display
void lcd_command(byte cmd) {
  PORTB &= ~(1 << PB4); // Imposta RS (pin 12) a LOW (porta B, pin 4)
  lcd_sendNibble(cmd >> 4); // Invia i primi 4 bit
  lcd_sendNibble(cmd & 0x0F); // Invia i secondi 4 bit
}

// Funzione per scrivere un carattere sul display
void lcd_write(byte data) {
  PORTB |= (1 << PB4);  // Imposta RS (pin 12) a HIGH (porta B, pin 4)
  lcd_sendNibble(data >> 4); // Invia i primi 4 bit
  lcd_sendNibble(data & 0x0F); // Invia i secondi 4 bit
}

// Funzione per inviare i dati al display
void lcd_sendNibble(byte data) {
  // Maschera e invia i 4 bit
  //digitalWrite(D4, (data >> 0) & 1); // D4
  PORTD = (PORTD & ~(1 << PD4)) | (((data >> 0) & 1) << PD4);
  //digitalWrite(D5, (data >> 1) & 1); // D5
  PORTD = (PORTD & ~(1 << PD5)) | (((data >> 1) & 1) << PD5);
  //digitalWrite(D6, (data >> 2) & 1); // D6
  PORTD = (PORTD & ~(1 << PD6)) | (((data >> 2) & 1) << PD6);
  //digitalWrite(D7, (data >> 3) & 1); // D7
  PORTD = (PORTD & ~(1 << PD7)) | (((data >> 3) & 1) << PD7);
  
  // Attiva l'abilitazione (E) per 1 ms per inviare il dato
  PORTB |= (1 << PB3);  // Imposta E (pin 11) a HIGH
  PORTB &= ~(1 << PB3);  // Imposta E (pin 11) a LOW
}

void lcd_displayOn() {
  PORTD |= (1 << PORTD3);  // Imposta il bit 3 di PORTD su HIGH, accendi la retroilluminazione
  // Inizializza il display in modalità 4 bit
  lcd_command(0x02); // Imposta modalità 4 bit
  lcd_command(0x28); // 2 linee, 5x8 font
  lcd_command(0x0C); // Display acceso, cursore spento
  lcd_command(0x06); // Incremento del cursore
  lcd_command(0x01); // Pulisce il display
}

void lcd_displayOff() {
  lcd_command(0x08); // Display off, cursore spento
  PORTD &= ~(1 << PORTD3); // Resetta il bit 3 di PORTD su LOW, spegni la retroilluminazione
}

// Configurazione del timer 2
void timer2Setup() {
  TCCR2A = 0;              // Modalità normale
  TCCR2B = (1 << CS22);    // Prescaler 64
  TCNT2 = 0;               // Reset del timer
  OCR2A = 250;             // Con prescaler 64, 1 tick = 4 µs; 250 * 4 µs = 1 ms
  TIMSK2 = (1 << OCIE2A);  // Abilita l'interrupt su confronto
}

void updateDisplay() {
  lcd_command(0x80); // Posiziona il cursore all'inizio della prima riga
  lcd_write('0' + cifra);  // Scrive la cifra sul display
  lcd_command(0xC0); // Posiziona il cursore all'inizio della seconda riga
  for (uint8_t i = 0; i < 5; i++) {
    lcd_write('0' + userCode[i]); //Stampa il codice inserito carattere per carattere.
  } /* '0' = 48, che sta per 0 nella tabella ASCII. Se per caso userCode[i] è ancora -1, stampa
    48-1=47, che nella tabella ASCII è '/' */
}

void buttonSetup() {
  // Configura il pulsante come input con pull-up
  DDRD &= ~(1 << DDD2); // BUTTON_PIN come input
  PORTD |= (1 << PORTD2); // Pull-up abilitato sul BUTTON_PIN

  // Abilita interrupt esterno sul pulsante
  EICRA |= (1 << ISC01); // Falling edge su INT0
  EIMSK |= (1 << INT0);  // Abilita INT0

  // Configura il timer0 per generare interrupt ogni 200ms
  TCCR0A = 0; // Modalità normale
  TCCR0B = (1 << CS01) | (1 << CS00); // Prescaler 64, 1 tick = 4 microsecondi
  OCR0A = 250; // Imposta il valore di confronto per 1 ms (calcolato)
  TIMSK0 |= (1 << OCIE0A); // Abilita l'interrupt per il timer0
}

void printWelcome() {
    lcd_write('W'); // Scrive "Benvenuto"
    lcd_write('e');
    lcd_write('l');
    lcd_write('c');
    lcd_write('o');
    lcd_write('m');
    lcd_write('e');
    lcd_write('!');
}

void printAlarm() {
  lcd_write('A'); // Scrive "Allarme!"
  lcd_write('l');
  lcd_write('a');
  lcd_write('r');
  lcd_write('m');
  lcd_write('!');
}

//Per resettare la cifra in rotazione e il codice inserito dall'utente
void resetInserimento() {
  cifra = 0;
  for (uint8_t i = 0; i < 5; i++) {
    userCode[i] = -1;
  }
}
//---------------------------------------ISR---------------------------------------------------

//Gestione cambiamento di stato del pin ECHO_PIN
ISR(PCINT0_vect) {
  if (PINB & (1 << PINB0)) {
    // Rising edge: ECHO_PIN è in ascolto
    TCNT1 = 0; // Resetta il contatore del timer
  } else {
    // Falling edge: ECHO_PIN ha rilevato il segnale
    measurementComplete = true;  // La misurazione è completa
    if (TCNT1 < 47000) { //simuliamo un timeout a circa 340cm, per emulare un comportamento reale
      objectDetected = true;
    }
  }
}

ISR(TIMER2_COMPA_vect) {
    if (currentState == CODE_ENTRY) {
      rotationCounter++; //Incrementa il timer di rotazione
      entryCounter++; //Incrementa il timer di inserimento

      if (rotationCounter == 500) { // 500ms -> 0,5s
        rotationCounter = 0; //Azzera il timer
        cifra = (cifra + 1) % 10; // Incrementa la cifra (da 0 a 9)
      }
      if (entryCounter == 90000) { // 90s
        entryCounter = 0; //Azzera i timer di stato
        rotationCounter = 0;
        currentState = ALARM; //Transita in ALARM
        clearDisplay = true; //Notifica la necessità di pulire il display
      }
    }

    if (currentState == ALARM) {
    alarmCounter++; //Incrementa il timer di allarme
      if (alarmCounter >= 300000) { //300s = 5 minuti
        alarmExpired = true; //Notifica la scadenza dell'allarme
        alarmCounter = 0; // Resetta il contatore
      }
    }
    
    if (currentState == SUSPENDED) {
    suspendedCounter++; //Incrementa il timer di sospensione
      if (suspendedCounter >= 60000) { //60s
        suspendedCounter = 0; // Resetta il contatore
        currentState = STANDBY; //Transita in STANDBY
      }
    }
/*  //Per l'implementazione di una FSM senza stati finali
    if (currentState == WELCOME) {
      welcomeCounter++;
      if (welcomeCounter == 10000) { //10s, arbitrario
        welcomeExpired = true;
        welcomeCounter = 0;
        rotationCounter = 0;
        entryCounter = 0;
        cifra = 0;
        for (uint8_t i = 0; i < 5; i++) {
          userCode[i] = 0;
        }
      }

    }
*/
    //timer2 va fermato dopo welcome o alarm? variabili già si fermano quando esco da
    //CODE_ENTRY, è solo il timer che continua a funzionare per altri stati in modo diverso,
    //quindi penso che se per esempio andassi in SUSPENDED devo solo aggiornare un
    //altro counter del SUSPENDED che poi si azzera ogni volta che esce da SUSPENDED o
    //ci riporta in STANDBY dopo 60s o ci riporta a CODE_ENTRY con le variabili già ferme
    //che dovrebbero riprendere da dov'erano.
}

// Funzione ISR per il timer0 (debounce)
ISR(TIMER0_COMPA_vect) {
  if (!debounceFlag) { //Se possiamo già premere il tasto, evitiamo di ripetere questo codice
    debounceCounter++; 
    if (debounceCounter == 200) { //200ms tempo medio debounce
      debounceCounter = 0;
      debounceFlag = true;  // Segna che è passato il tempo di debounce
    }
  }
}


// ISR per INT0 (interrupt del pulsante, acquisizione cifra e richiesta di validazione)
ISR(INT0_vect) {
  if (debounceFlag && (currentState == CODE_ENTRY)) {  // Verifica se è passato il tempo di debounce e se può acquisire input
    userCode[posizione] = cifra; // Acquisisce la cifra
      debounceFlag = false;  // Resetta il flag di debounce
      debounceCounter = 0; // Azzera il contatore
    if (posizione == 4) { //Se la posizione è la 4, abbiamo riempito l'inserimento del codice
      currentState = CODE_VALIDATION;
    }
  posizione = (posizione + 1) % 5;  // Passa alla cifra successiva
  }
}