#include <SPI.h>
#include "Semafori.h"
#include "sr595.h"
//#include <JC_Button.h>
//Button btn3(10);

const byte PIN_CS   =  4;  
#define ENDL      255
// Dichiarazione e formazione dei gruppi.
uint8_t id0[] =       {0, ENDL};          // Lanterna a nord lss0, id=0
uint8_t id0_6[] =     {0, 6, ENDL};       // Lanterne lss0 e lss6
uint8_t id2[] =       {2, ENDL};          // Lanterna lss2
uint8_t id2_4[] =     {2, 4, ENDL};       // Lanterne lss2 e lss4
uint8_t id1_3_4[] =   {1, 3, 4, ENDL};    // Lanterne lss1, lss3 e lss4
uint8_t id1_3[] =     {1, 3, ENDL};       // Lanterne lss1 e lss3
uint8_t id5[] =       {5, ENDL};          // Lanterna lss5
uint8_t id5_6_7[] =   {5, 6, 7, ENDL};    // Lanterne lss5, lss6 e lss7
uint8_t id5_7[] =     {5, 7, ENDL};       // Lanterne lss5 e lss7

// DIV divisore, es: 30000 / 5 = 6000ms (6 secondi) 
#define DIV 5

#define TPH0 30000UL  / DIV
#define TPH1 3000UL   / DIV
#define TPH2 1000UL   / DIV
#define TPH3 30000UL  / DIV
#define TPH4 3000UL   / DIV
#define TPH5 1000UL   / DIV
#define TPH6 30000UL  / DIV
#define TPH7 3000UL   / DIV
#define TPH8 1000UL   / DIV
#define TPH9 30000UL  / DIV
#define TPH10 3000UL  / DIV
#define TPH11 1000UL  / DIV

struct Phase {
    uint8_t *outs;  // puntatore ad array delle lanterne conivolte
    uint32_t ms;    // tempo di permanenza del coloro
};

// Pre-timed phases
Phase phases[] = {
    
    // 4 lanterne stradali, e 4 pedonali.
    // Divisione in 12 PHASES
    // Si inizia sempre da nord che commuta su verde.
    { id0_6, TPH0 },       // GREEN,  PHASE 0    
    { id0, TPH1 },         // YELLOW, PHASE 1
    { id0, TPH2 },         // RED,    PHASE 2
    { id1_3_4, TPH3 },     // GREEN,  PHASE 3
    { id1_3, TPH4 },       // YELLOW, PHASE 4
    { id1_3, TPH5 },       // RED,    PHASE 5
    { id5_7, TPH6 },       // GREEN,  PHASE 6
    { id5_6_7, TPH7 },     // YELLOW,  PHASE 7
    { id5_6_7, TPH8},      // RED,     PHASE 8
    { id2, TPH9 },         // GREEN,   PHASE 9
    { id2_4, TPH10 },      // YELLOW,  PHASE 10
    { id2_4, TPH11 },      // RED,     PHASE 11
    { nullptr, 0 }
};
/*   0  1  2  3  4  5  6  7  
    "g  r  r  r  r  r  g  r"
    "r  g  r  g  g  r  g  r"
    "r  r  r  r  g  g  g  g"
    "r  r  g  r  g  r  r  r"
*/

SR59532 sr32(PIN_CS);

LSS out0(sr32, 0, 1, 2);
LSS out1(sr32, 3, 4, 5);
LSS out2(sr32, 6, 7, 8);
LSS out3(sr32, 9, 10, 11);
LSS out4(sr32, 12, 13, 14);
LSS out5(sr32, 15, 16, 17);
LSS out6(sr32, 18, 19, 20);
LSS out7(sr32, 21, 22, 23);
LSS out8(sr32, 24, 25, 26);   // non usata
LSS out9(sr32, 27, 28, 29);   // non usata

LSS *outs[10] = { &out0, &out1, &out2, &out3, &out4, &out5, &out6, &out7, &out8, &out9 };

uint32_t wait;
uint32_t durationCycle;
uint8_t phasesIdx = 0;
uint8_t colorCounter = 0; // 0 1 2 0 1 2 ecc, GREEN, YELLOW, RED
uint8_t exec;
uint32_t saveTime;
uint32_t refTime;

LSS &out(uint8_t n) {
    return *outs[n];
}

Phase &getCurrentPhase() {
    return phases[phasesIdx];
}

void initId() {
    
    // Mentre quelle stradali hanno id=0÷9 
    for (uint8_t i=0; i<10; i++) {
      
        out(i).setId(i);
    }
}

void setAllRed() {
    for (uint8_t i=0; i<8; i++) {
        out(i).setColor(Color::RED);
    }
}

void initAll() {
    
    initId();
    // setAllRed(): imposta tutte le lanterne sul rosso 
    setAllRed();
    //btn3.begin();
    sr32.update();
    delay(2000);
    exec = 1;
}

void setup() {
 
    Serial.begin(115200);
    pinMode(PIN_CS, OUTPUT);
    digitalWrite(PIN_CS, HIGH);
    SPI.begin();
    
    initAll();
    refTime = millis();
}

void execPhases() {
    if (exec == 1) {
        
        Phase &ph = getCurrentPhase();
        wait = ph.ms;
        durationCycle += wait;
        
        uint8_t idIdx = 0;
        Serial.print("id:");
        // cicla sul gruppo
        while (ph.outs[idIdx] != ENDL) {
                       
            out(ph.outs[idIdx]).setColor(colorCounter);
            
            Serial.print(out(ph.outs[idIdx]).id());
            Serial.print(", ");
            
            idIdx++;
        }
        
        sr32.update();
        
        Serial.print("Color:");
        Serial.print(colorCounter);
        Serial.print(", ");
        Serial.print("Duration:");
        Serial.print(wait);
        Serial.println();
        saveTime = millis();
        exec = 2;
    } 

    if (exec == 2) {
        
        if (millis() - saveTime >= wait) {
            saveTime = millis();
            phasesIdx++;
            
            if (phases[phasesIdx].outs == nullptr) {
                
                phasesIdx = 0;
                Serial.print("durationCycle:");
                Serial.println(durationCycle);
                Serial.print("real time: ");
                Serial.println(millis() - refTime);
                refTime = millis();
                durationCycle = 0;
                Serial.println("\nRestart from Phase 0");
            }
            colorCounter++;
            colorCounter = colorCounter % 3;
            exec = 1;
        }
    }
    
}

void loop() { 

    execPhases();
    
}