#define TIMER4_DEBUG

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

const byte PIN_CS   =  4;   

const byte PIN_START   =  7;

uint32_t _counter_seconds_;
uint32_t _one_sec_millis_;
uint32_t seconds() {
    return _counter_seconds_;
}

bool secondCounter() {
    uint32_t _millis_ = millis();
  
    if (_millis_ - _one_sec_millis_ ==  1000) {
        _one_sec_millis_ = _millis_;
        _counter_seconds_++;
        return true;
        //Serial.println(seconds());
    }
    return false;
}

SR59516 sr16(PIN_CS);

uint32_t millisNow;
uint16_t interval[] = { 3000, 1000, 800};
uint8_t quattroTempi[] = { 6, 1, 8 };
//                         G  Y  R
//                         R  R  R
uint8_t intervalIdx = 0;
LSS lss5(sr16, 2, 3, 4);
LSS lss2(sr16, 10, 11, 12);

LSS *cps[] = { &lss2, &lss5 };
uint8_t cpsIdx = 0;

int16_t incrementValue = 1000;

void lshift(uint16_t &d, uint8_t ovfBit = 4) {
  d = d << 1;
  if (d & (1 << ovfBit)) 
      d = 1;
}
const char *strColor[] = { "Verde", "Giallo", "Rosso" };
void setup() {
 
    Serial.begin(115200);
    
    
    pinMode(PIN_START, INPUT_PULLUP);
    SPI.begin();


    
    pinMode(PIN_CS, OUTPUT);
    digitalWrite(PIN_CS, HIGH);
    // sequenza RGV     RGV
    //          001     100  0
    //          010     100  1
    //          100     100  2
    //          100     001  3     
    //          100     010  4
    //          100     100  5
    lss5.setId(35);
    lss2.setId(24);
    lss5.setStartColor(RED);
   
    
    //lss5.next();
    //lss2.next();
    millisNow = millis();
    Timer4 timer4(interval);
    lss5.setRegister(timer4.getRegG());
    lss2.setRegister(timer4.getRegR());
    //lss5.printReg();
    //lss2.printReg();
    timer4.start();
    while(true) {
      timer4.run();
      lss2.switchColor();
      lss5.switchColor();
    }

}
// uint16_t interval[] = { 5000,   1000,    800   }; // dichiara all'inizio
//     S5+S3               Rosso   Rosso    Rosso 
//     S2+S4               Verde   giallo   Rosso
//     S5+S3               Verde   giallo   Rosso
//     S2+S4               Rosso   Rosso    Rosso
void cicla() {
    if (millis() - millisNow >= interval[intervalIdx]) {
        uint8_t currentColor = cps[cpsIdx]->color();
        Serial.print(cps[cpsIdx]->id());
        Serial.print(".");
        Serial.print(interval[intervalIdx]);
        Serial.print(".");
        Serial.print(strColor[currentColor]);
        Serial.print("\t");
        uint8_t cpsIdxNegatoColor = cps[!cpsIdx]->color();
        Serial.print(cps[!cpsIdx]->id());
        Serial.print(".");
        Serial.print(interval[intervalIdx]);
        Serial.print(".");
        Serial.println(strColor[cpsIdxNegatoColor]);
        
        if (currentColor != Color::RED) {
            
            cps[cpsIdx]->next();
            
        } else {
            /*if (interval[0] == 3000) {
                incrementValue = 1000;
            } else if (interval[0] == 6000) {
                incrementValue = -1000;
            }
             
            interval[0] += incrementValue;*/
           
            cpsIdx = !cpsIdx;
            cps[cpsIdx]->next();
              
        }
        intervalIdx++;
        intervalIdx = intervalIdx % 3;
        millisNow = millis();
    }
}

uint8_t timerA;
uint8_t timerB;
uint8_t idxA;
uint8_t idxB = 2;
uint8_t iA;
uint8_t iB;
uint8_t times[] = { 1, 1, 1, 2 };
//                         G  Y  R
//                         R  R  R
/*
{ 1, 1, 1, 3 };
      0, 1, 2, 3, 4, 5, 6, 7, 8, 9. 10, 11, 12
      G  Y  R  R  R  R  R  G  Y  R  R   R   R   R
      R  R  R  R  R  R  G  Y  R  R  R   R   R         
*/
void loop() { 
  /*bool signal = secondCounter();
  if (seconds() - timerA >= iA) {
          timerA = seconds();
          iA = times[idxA];
          lss5.setColor(idxA);
          Serial.print("idxA: ");
          Serial.println(idxA);
          idxA++;
          idxA = idxA % 4;
      }
      if (seconds() - timerB >= iB) {
          timerB = seconds();
          iB = times[idxB];
          lss2.setColor(idxB);
          Serial.print("idxB: ");
          Serial.println(idxB);
          idxB++;
          idxB = idxB % 4;
      }*/
  /*bool signal = secondCounter();
  if (signal == true) {
      //Serial.println(seconds());
      if (seconds() - timerA >= iA) {
          timerA = seconds();
          iA = times[idxA];
          lss5.setColor(idxA);
          Serial.print("idxA: ");
          Serial.println(idxA);
          idxA++;
          idxA = idxA % 4;
      }
      if (seconds() - timerB >= iB) {
          timerB = seconds();
          iB = times[idxB];
          lss2.setColor(idxB);
          Serial.print("idxB: ");
          Serial.println(idxB);
          idxB++;
          idxB = idxB % 4;
      }
  }*/
  
  cicla();
  
}