#include <SPI.h>

#define TIMER4_DEBUG
#include "Semafori.h"
#include "sr595.h"
#include <JC_Button.h>

const byte PIN_CS   =  4;   

const byte PIN_START   =  7;

SR59516 sr16(PIN_CS);
Button btnStart(PIN_START);
// tempi in secondi diviso 4  60     3    1
uint16_t StradaleTempi[] = { 15000, 750, 250 };
uint8_t idxST;
uint16_t PedonaleTempi[] = { 3750, 750, 250 };
uint8_t idxPT = 2;
// creare istanza di LSS richiede come argomenti:
// * Una istanza di classe SR59516 (vedi sr16)
// * 3 argomenti bit-verde, bit-giallo e bit-rosso
// Esempio: LSS   lss2(
//               sr16,  // istanza di classe SR59516
//                 10,  // bit 10, pin sr2:Q2
//                 11,  // bit 11, pin sr2:Q3
//                 12   // bit 12, pin sr2:Q4 
//                   );

LSS lss5(sr16, 2, 3, 4);
//LSS lss35(sr16, 5, 6, 7);
LSS lsp5(sr16, 10, 11, 12);
//LSS lss24(sr16, 13, 14, 15);
//LSS ls0(sr16, 0, 1, 2);
//LSS ls1(sr16, 3, 4, 5);
//LSS ls2(sr16, 6, 7, 8);
//LSS ls3(sr16, 9, 10, 11);
//LSS ls4(sr16, 12, 13, 14);





void test0() {
  Timer4 timer4(StradaleTempi);
    // prima di potere usare una istanza di classe LSS
    // è necessario assegnargli un registro.
    // Un registro è sostanzialmente una variabile uint8_t
    // che assume un valore da 0÷2:
    // 0 verde, 1 giallo, 2 rosso.
    lss5.setRegister(timer4.getRegG());
    lsp5.setRegister(timer4.getRegR());
    lsp5.show();
    lss5.show();
    //lss24.setRegister(timer4.getRegR());
    //lss35.setRegister(timer4.getRegG());
    timer4.start();
    while(true) {
      btnStart.read();
      if (btnStart.wasPressed() && bitRead(*lss5.getRegister(), 3) ) {
          Serial.println("prenota");
      }
      // timer4.run() // restituisce true allo scadere dei 
      // seguenti tempi: uint16_t interval[] = { 3000, 1000, 800 };

      bool event = timer4.run();
      if (event) {
          // aggiorna lo stato di tutte le lanterne
          lsp5.show();
          lss5.show();
          //lss24.show();
          //lss35.show();
      }
    }
}

uint8_t regPedonale = Color::RED;
uint8_t regStradale = Color::RED;

//const char *strColor[] = { "Verde", "Giallo", "Rosso" };
void setup() {
 
    Serial.begin(115200);
    btnStart.begin();
    pinMode(PIN_START, INPUT_PULLUP);
    pinMode(PIN_CS, OUTPUT);

    digitalWrite(PIN_CS, HIGH);
    SPI.begin();
    // ad ogni lanterna è possibile assegnare un ID 0÷255 
    lsp5.setId(42);
    lss5.setId(53);
    
    lss5.setRegister(&regStradale);
    lsp5.setRegister(&regPedonale);
    //lss5.show();
    //lsp5.show();
    
    //test0();

}



void incIndex(uint8_t &idx) {
    idx++;
    idx = idx % 3;
}
uint16_t greenStartTime;
uint32_t saveMillis;
//uint16_t startTime;
bool s_allred = true;
// tutti rossi per 2 secondi poi verde per lo stradale
void allRed() {
    
    static uint8_t startTime = millis();
    if (s_allred && millis() - startTime > 2000) {
        regStradale = Color::GREEN;
        greenStartTime = millis();
        saveMillis = greenStartTime;
        s_allred = false;
    }
}
/*
// tempi in secondi diviso 4  60     3    1
uint16_t StradaleTempi[] = { 15000, 750, 250 };
uint8_t idxST;
uint16_t PedonaleTempi[] = { 3750, 750, 250 };
uint8_t idxPT = 2;
*/

// Leggi readme.md per sapere su questa versione
bool pedonalePrenotato;
uint8_t alterna = 0;
uint32_t saveMls0;
uint32_t saveMlsPed;
uint8_t idxSTP;
uint32_t salvaTempoVerdeStradale;
void loop() { 
    btnStart.read();
    
    lss5.show();
    lsp5.show();
    allRed();  // tutti rossi per 2 secondi poi verde per lo stradale
    if (s_allred == false) {
        if (alterna == 0) {
            
            if (millis() - saveMillis >= StradaleTempi[idxST]) {
                saveMillis = millis();
                incIndex(idxST);
                incIndex(regStradale);
                if (idxST == 2) {
                    if (salvaTempoVerdeStradale) {
                        StradaleTempi[0] = salvaTempoVerdeStradale;
                        salvaTempoVerdeStradale = 0;
                        Serial.println(StradaleTempi[0]);
                    }
                    alterna = 1;
                    saveMlsPed = saveMillis;
                }
            }
            if (!pedonalePrenotato && btnStart.wasPressed()) {
                  pedonalePrenotato = true;
                  sr16.set(0, true);
                  sr16.update();
                 
            }
            if (pedonalePrenotato && millis() - saveMillis >= 2500
                    && !(millis() - saveMillis >= 7750)) {
                idxSTP = 1;
                regStradale = 1;
                Serial.println(regStradale);  
                saveMls0 = millis();
                alterna = 2;
            }

        } else if (alterna == 1) {
            if (salvaTempoVerdeStradale == 0) {
                pedonalePrenotato = false;
                sr16.set(0, false);
                sr16.update();
            }
            if (millis() - saveMlsPed >= PedonaleTempi[idxPT]) {
                saveMlsPed = millis();
                incIndex(idxPT);
                incIndex(regPedonale);
                if (idxPT == 2) {
                    alterna = 0;
                    //regStradale = idxST;
                    saveMillis = millis();
                    idxST = 2;
                }
            }
        } else if (alterna == 2) {
              //Serial.println(alterna);
              
              if (millis() - saveMls0 >= StradaleTempi[idxSTP]) {
                  saveMls0 = millis();
                  regStradale = idxSTP = 2;
                  alterna = 1;
                  saveMlsPed = millis();
                  pedonalePrenotato = false;
                  salvaTempoVerdeStradale = StradaleTempi[0];
                  StradaleTempi[0] = StradaleTempi[0] - 
                          (2500+PedonaleTempi[0]+PedonaleTempi[1]+PedonaleTempi[2]);
                  Serial.println(StradaleTempi[0]);
                  sr16.set(0, false);
                  sr16.update();
              }

        }
    }
    
    
}