#include <SPI.h>
#include "Semafori.h"
#include "sr595.h"

const byte PIN_CS   =  4;   


// tempi specificati in millesimi di secondo
uint16_t lss0_2Times[] = { 5000,   750,    250 }; 
uint16_t lss1_3Times[] = { 3000,   750,    250 };
//                         Verde, Giallo, Rosso
// Nota sui tempi:
// il verde di durata meggiore viene solitamente assegnato 
// alla strada più trafficata

uint8_t lssR00 = Color::RED;   // registro per lss0 
uint8_t lssR02 = Color::RED;   // registro per lss2
uint8_t lssR1_3 = Color::RED;  // registro per lss1 e lss3 (Est e Ovest)
uint8_t lssR3_0 = Color::RED;  // registro per lss3 e lss0 (Ovest e Nord)
uint32_t greenStartTime;       // dove salvare millis()

char *forPhases[] = {
          "Grrrrrgr"  //fase 1
          "rGrGgrgr"  //fase 2
          "rrrrgGGG"  //fase 3
          "rrGrGrrr"  //fase 4

};


// sr16 è istanza di classe SR59516 per controllare due shift register 595
SR59532 sr32(PIN_CS);
// Istanze di Lanterne Semaforiche Stradali
LSS lss0(sr32, 0, 1, 2);      // Nord (ore 12)
LSS lss1(sr32, 3, 4, 5);      // Est (ore 3)
LSS lss2(sr32, 6, 7, 8);      // Sud (ore 6)     
LSS lss3(sr32, 9, 10, 11);    // Ovest (ore 9)
LSS lssp0(sr32, 12, 13, 14);  

void setup() {
 
    Serial.begin(115200);
    
    pinMode(PIN_CS, OUTPUT);

    digitalWrite(PIN_CS, HIGH);
    SPI.begin();
    // ad ogni lanterna è possibile assegnare un ID 0÷255 
    lss0.setId(0);
    lss1.setId(1);
    lss2.setId(2);
    lss3.setId(3);
    //lssp1.setId(4);
    // Connette le lanterne ai registri.
    lss0.connectToReg(lssR00);   
    lss2.connectToReg(lssR02);
    // le lanterne lss1 e 3 condividono lo stesso registro lssR1_3
    lss1.connectToReg(lssR1_3);
    lss3.connectToReg(lssR1_3);
    
}

bool f_allred = true;   
// Accende tutti rossi per 2 secondi poi lss1_3 su verde
void allRed() {
    
    static uint8_t startTime = millis();
    if (f_allred && millis() - startTime > 2000) {
        lssR1_3 = Color::GREEN;
        greenStartTime = millis();
        f_allred = false;
        Serial.println("allRed");
    }
}

Color matureToRed(uint8_t &reg, uint16_t *times) {
    Color color = Color::NOCOLOR;
    if (millis() - greenStartTime >= times[reg]) {
        greenStartTime = millis();
        reg++;
        reg = reg % 3;
        color = reg;
    }
    return color;
}

uint8_t phase = 1;  // seleziona la fase iniziale

void loop() { 
    // occorre chiamare il metodo show per ogni istanza di lanterna 
    lss0.show();
    lss1.show();
    lss2.show();
    lss3.show();
    // Mantiene accessi tutti rossi per 2 secondi, poi lss1_3 commuta sul verde 
    allRed();  
    if (f_allred) 
        return;

    switch (phase) {
        
      case 1:
          /*if (lss1.color() == Color::GREEN)
              lss3.setRegister(&lss1_3);*/
          lss3.connectOnGreen(lssR1_3, lss1);
          if (matureToRed(lssR1_3, lss1_3Times) == Color::RED) {
              phase = 2;
          } 

          break;

      case 2:
          if (matureToRed(lssR02, lss0_2Times) == Color::RED) {
              lss3.connectToReg(lssR3_0);
              lss0.connectToReg(lssR3_0);
              phase = 3;
          }
          
          break;

      case 3:
          if (lss3.color() == Color::GREEN)
              lss3.disconnect();
          if (matureToRed(lssR3_0, lss1_3Times) == Color::RED) {
              lss1.connectToReg(lssR1_3);
              phase = 1;
          } 
          
          break;
     
    }
}