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

bool pedestrianCall[4];

Button btn4(10);
Button btn5(9);
Button btn6(8);
Button btn7(7);

struct OutTime {
    uint32_t dataOut;
    uint32_t time;
};



const byte PIN_CS   =  4;  

// Dichiarazione e formazione dei gruppi.
uint8_t id0[] =       {0, ENDL};          // Lanterna a nord lss0, id=0
uint8_t id0_102[] =   {0, 6, ENDL};       // Lanterne id=0 e id=102
uint8_t id2[] =       {2, ENDL};          // Lanterna id=2
uint8_t id2_100[] =   {2, 4, ENDL};       // Lanterne id=2 e id=100
uint8_t id1_3_100[] = {1, 3, 4, ENDL};    // Lanterne id=1, id=3, id=100
uint8_t id1_3[] =     {1, 3, ENDL};       // Lanterne id=1 e id=3
uint8_t id101[] =     {5, ENDL};          // Lanterna id=101
uint8_t id101_102_103[] = {5, 6, 7, ENDL};  // Lanterne id=101, 102 e 103
uint8_t id101_103[] =     {5, 7, ENDL};             // Lanterne id=101 e 103

#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

OutTime outTimeList[] = {
    { 0x864921, 30000UL / DIV },
    { 0x864922, 3000UL / DIV },
    { 0x864924, 1000UL / DIV },
    { 0x86130C, 25000UL / DIV },
    { 0x861514, 3000UL / DIV },
    { 0x861924, 1000UL / DIV },
    { 0x249924, 15000UL / DIV },
    { 0x491924, 3000UL / DIV },
    { 0x921924, 1000UL / DIV },
    { 0x864921, 30000UL / DIV },
    { 0x864922, 3000UL / DIV },
    { 0x864924, 1000UL / DIV },
};



// 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_102, TPH0 },       // GREEN,  PHASE 0    
    { id0, TPH1 },           // YELLOW, PHASE 1
    { id0, TPH2 },           // RED,    PHASE 2
    { id1_3_100, TPH3 },     // GREEN,  PHASE 3
    { id1_3, TPH4 },         // YELLOW, PHASE 4
    { id1_3, TPH5 },         // RED,    PHASE 5
    { id101_103, TPH6 },     // GREEN,  PHASE 6
    { id101_102_103, TPH7 },  // YELLOW,  PHASE 7
    { id101_102_103, TPH8},   // RED,     PHASE 8
    { id2, TPH9 },            // GREEN,   PHASE 9
    { id2_100, TPH10 },       // YELLOW,  PHASE 10
    { id2_100, 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"
    
*/
PhasesData roadCycle;

#define SIZE_OF(a) sizeof(a) / sizeof(a[0])
int16_t phaseNumber = 0; // 
uint8_t maxPhaseNumber = 3;

uint32_t greenStartTime;       // dove salvare millis()
uint32_t pedonaleStartTime;

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);
LSS out9(sr32, 27, 28, 29);  // non usata

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

uint32_t wait;
uint16_t minGreen;
uint16_t pedonaleGreen;
uint8_t  richiestaPedonale = 1;
uint32_t startGreenRefTime;
uint32_t pedeGreenTime;
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;
uint16_t startPedonaleCycle;

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

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


void initId() {
    // Le lanterne pedonali hanno id=100, 101, 102 e 103
    // Mentre quelle stradali hanno id=0, 1, 2, 3
    for (uint8_t i=0; i<10; i++) {
      if (i >= 4)
        out(i).setId(i+96);
      else
        out(i).setId(i);
      
    }
}


void initTimerCycle() {
    saveTime = millis();
}

uint32_t getTimerCycle() {
    return millis() - saveTime;
}

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

void initAll() {
    //Serial.println(SIZE_OF(phases));
    // initId(): assegna id progressivo da 0÷3 per lanterne stradli 
    // e 100÷103 per le pedonali
    initId();
    // setAllRed(): imposta tutte le lanterne sul rosso 
    setAllRed();
    btn4.begin();
    btn5.begin();
    btn6.begin();
    btn7.begin();
    sr32.update();
    delay(2000);
    exec = 1;
    roadCycle.exec = 1;
    roadCycle.ph = phases;
    initTimerCycle();
}


void setup() {
 
    Serial.begin(115200);
    int idx = 0;
    //nextIdx();
    //Serial.println("next");
    /*for (uint8_t i=0; i<10; i++) {
        phaseNumber = OpOnIdx(phaseNumber, 3, OpIdx::Next);
        Serial.println(phaseNumber);
    }
    Serial.println("prev");
    for (uint8_t i=0; i<10; i++) {
        phaseNumber = OpOnIdx(phaseNumber, 3, OpIdx::Prev);
        Serial.println(phaseNumber);
    }*/
    pinMode(PIN_CS, OUTPUT);

    digitalWrite(PIN_CS, HIGH);
    SPI.begin();
    
    initAll();
        
}
uint32_t waitPede;



uint8_t savePhasesIdx;
uint32_t saveTimePedReq;
uint8_t allGrpPedo[] = {4, 5, 6, 7, 255};
Phase pedoPhases[] = {
    { id0, 800 },       // YELLOW    
    { id0, 200 },      // RED
    { allGrpPedo, pedeGreenTime },     // GREEN,  
    { allGrpPedo, 800 },         // YELLOW, PHASE 4
    { allGrpPedo, 200 },   
    { nullptr, 0 }
};
void loop() { 

    //execPhases();
    execPhases(&roadCycle);
    switch (phaseNumber) {
      case 1:
          btn4.read();
          btn5.read();
          btn7.read();    
          
      case 2:
          btn5.read();
          btn7.read();
          break;
      case 4:
          btn5.read();
          btn6.read();
          btn7.read();
          break;
    }
    if (btn4.wasPressed()) {
        Serial.println("btn4");
    }    
    if (btn5.wasPressed()) {
        Serial.println("btn5");
    }       
    if (btn6.wasPressed()) {
        Serial.println("btn6");
    }       
    if (btn7.wasPressed()) {
        Serial.println("btn7");
    }       
   
    //btn4.read();
    //if (pedestrianCall[3] == false) {
    /*if (out7.color() == Color::RED && !(phasesIdx %3) && pedestrianCall[3] == false) {  // fasi 0, 3, 9
                        
        if (btn4.wasPressed() == true) {
            pedestrianCall[3]++;
            Phase &ph = getCurrentPhase(phases);
            savePhasesIdx = phasesIdx;
            saveTimePedReq = millis() - startGreenRefTime;
            Serial.println("richiesta pedonale 3");
            Serial.println(savePhasesIdx);
            Serial.println(saveTimePedReq);
            startPedonaleCycle = ph.ms * 0.26;
            pedeGreenTime = ph.ms - startPedonaleCycle * 2 - (8000 / DIV);
            Serial.print("start: ");
            Serial.println(startPedonaleCycle);
            Serial.print("pedeGreenTime: ");
            Serial.println(pedeGreenTime);
            //uint32_t end = 
        }
    }*/
    
    
}