#include <SPI.h>
const byte PIN_DATA    =  2;    
const byte PIN_LATCH   =  4;   
const byte PIN_CLOCK   =  3; 
const byte PIN_START   =  7;


class SR59516 {
  public:
    SR59516(uint8_t cs, uint16_t *ptr) {
        m_ptrData = ptr;
        
        m_cs = cs;
    }
    
    void update() {
        if (m_oldRegVal != *m_ptrData) {
            m_oldRegVal = *m_ptrData;
            digitalWrite(m_cs, LOW);
            SPI.transfer16(m_oldRegVal);  // spi hardware
            digitalWrite(m_cs, HIGH);
        }
    }
    
  private:
    uint16_t *m_ptrData = nullptr;
    uint32_t m_oldRegVal = -1;
    uint8_t m_cs;
 
};

union SrData {
    // MutualExOut you can't turn on al out, but only one
    // the 8 output. Es:
    // 1 0 0 0 | 0 0 0 0
    // 0 1 0 0 | 0 0 0 0
    // 0 0 0 0 | 0 0 0 1
    // Only one out is turn 1, all other are cleared at zero
    uint8_t mutualExOut;  // 6 extern input + 2 internal
    // noMutualExOut you can turn on all output.
    uint8_t out8 : 1;
    uint16_t fullData;
};

uint16_t g_data;
SR59516 sr16(4, &g_data);

uint8_t pins[] = {0, 1, 2, 3};



byte      g_state;
uint16_t  g_rndNumber; // range 1000÷3000 ms
bool      g_btn0State;

byte data_cs = 255; //tutti high

void setup_cs() {
    writeShiftReg(data_cs);
}

void cs(byte bit, bool val) {
    if (val)
      data_cs |= (1 << bit);
    else
      data_cs &= ~(1 << bit);
    
    writeShiftReg(data_cs);
}

struct timer_t {
  uint32_t oneSec;
  uint16_t oneSecInt;
  uint32_t timer0;
  uint32_t saveMillis;
  
} timer;

struct Semaforo_t {
    byte cnt = 8;
    byte data;
} semaforo;  // istanza di tipo Semaforo_t

uint32_t g_us;
byte g_testResult;
uint16_t g_595data;

void set16(uint16_t &data, uint8_t bit, bool tf) {
    if (tf)
      data |= (1 << bit);
    else
      data &= ~(1 << bit);
}

void setup() {
 
  Serial.begin(115200);
  pinMode(PIN_START, INPUT_PULLUP);
  SPI.begin();
   
  pinMode(PIN_LATCH, OUTPUT);
  pinMode(5, OUTPUT);
  //pinMode(PIN_DATA, OUTPUT);
  //pinMode(PIN_CLOCK, OUTPUT);
  digitalWrite(5, HIGH);
  digitalWrite(PIN_LATCH, HIGH);
  for (byte pin = 0; pin < 16; pin++) {
      set16(g_data, pin, true);
      sr16.update();
      delay(100);
      set16(g_data, pin, false);
      sr16.update();
      delay(100);

  }
  return;
  for (byte pin = 0; pin < 16; pin++) {
      
      wReg(PIN_LATCH, pin, true);
      delay(30);
      wReg(PIN_LATCH, pin, false);
      delay(30);

  }
  for (int8_t pin = 15; pin > -1; pin--) {
      
      wReg(PIN_LATCH, pin, true);
      delay(30);
      wReg(PIN_LATCH, pin, false);
      delay(30);

  }
  //wReg(PIN_LATCH, 0, true);
  //wReg(5, 2);
  /*digitalWrite(PIN_LATCH, LOW);
  SPI.transfer(1);
  SPI.transfer(0);
  digitalWrite(PIN_LATCH, HIGH);
  SPI.transfer(2);
  digitalWrite(PIN_LATCH, LOW);
  SPI.transfer(1);
  digitalWrite(PIN_LATCH, HIGH);*/

  //writeShiftReg(85);
  delay(1000);
  /*
  digitalWrite(PIN_LATCH, LOW);
  SPI.transfer(0);
  digitalWrite(PIN_LATCH, HIGH);
  //writeShiftReg(0);*/

}

void wReg(byte pinLatch, byte pin, bool val)
{
    if (val) {
      g_595data |= (1<<pin); 
    } else {
      g_595data &= ~(1<<pin); 
    }
    digitalWrite(pinLatch, LOW);
    SPI.transfer16(g_595data);  // spi hardware
    //shiftOut(PIN_DATA, PIN_CLOCK, MSBFIRST, data); spi software
    digitalWrite(pinLatch, HIGH);
}
// writeShiftReg usa la SPI hardware
// la chiamata writeShiftReg(byte data) consuma 148us
void writeShiftReg(byte data)
{
   digitalWrite(PIN_LATCH, LOW);
   SPI.transfer(data);  // spi hardware
   //shiftOut(PIN_DATA, PIN_CLOCK, MSBFIRST, data); spi software
   digitalWrite(PIN_LATCH, HIGH);
}

byte semaforoRun() {
    if ((millis() - timer.oneSec) >= timer.oneSecInt) {
        timer.oneSec = millis();
        timer.oneSecInt = 1000;
        
        semaforo.data |= (1 << (semaforo.cnt - 1)) 
                      | (1 << (semaforo.cnt - 2)); // end command

        g_us = micros();
        writeShiftReg(semaforo.data);
        Serial.println(micros() - g_us);
        semaforo.cnt -= 2;

        if (semaforo.cnt == 0) {
            semaforo.cnt = 8;
            g_state++;
            timer.timer0 = millis();
            
            return g_state;
        }
    }
    return g_state;
}

void initRandom() {
    static bool lstate;
    if (!lstate) {
        Serial.println("Premi per iniziare il test.");
        
        lstate = true;
    } 
    if (lstate) {
        if (digitalRead(PIN_START) == LOW) {
            // carica il timer oneSec
            timer.oneSec = millis();
            // azzera intevallo
            timer.oneSecInt = 0;
            // Inizializza il generatore rnd
            randomSeed(micros());
            // genera un numero nel range 1000÷3000
            g_rndNumber = random(1000, 3001);
        }
    }
}

enum State {
    RE_START, SEMAFORO, MONITORING, END_RESET
};

void loop() { 
    switch (g_state) {
      case RE_START:
      
        initRandom();
        if (g_rndNumber) {
            // ** CHAGE STATE **
            g_state = SEMAFORO;
        }
        break;

      case SEMAFORO:
        // quando tutte le luci sono accese
        // g_state assume il valore SEMAFORO + 1
        // che è uguale a MONITORING
        g_state = semaforoRun();
        break;

      case MONITORING:
        g_btn0State = digitalRead(PIN_START);
        if (((millis() - timer.timer0) >= g_rndNumber
              && semaforo.data != 0)) {
            // spegne tutt i led
            semaforo.data = 0;
            writeShiftReg(0);
            // salva millis()
            timer.timer0 = millis();
        }
        // Se pulsante premuto e data == 0
        if (g_btn0State == LOW && semaforo.data == 0) {
            g_testResult = 1;
            timer.saveMillis = millis();
            /*Serial.print("Tempo di reazione: ");
            Serial.print(millis() - timer.timer0);
            Serial.println("ms");*/

            // ** CHAGE STATE **
            g_state = END_RESET;
        // Se premi il pulsante con i led ancora accesi
        } else if (g_btn0State == LOW && semaforo.data != 0) {
            //Serial.println("Falsa partenza");
            g_testResult = 2;
            // ** CHAGE STATE **
            g_state = END_RESET;
        }
        break;

      case END_RESET:
        
        semaforo.data = 0;
        writeShiftReg(0);
        if (g_testResult == 1) {
            Serial.print("Tempo di reazione: ");
            Serial.print(timer.saveMillis - timer.timer0);
            Serial.println("ms");
        } else if (g_testResult == 2) {
            Serial.println("Falsa partenza");
        }
        g_testResult = 0;
        delay(200);
        while(!digitalRead(PIN_START));
        //delay(200);
        delay(g_rndNumber/8);
        
        Serial.print("rnd number: ");
        Serial.println(g_rndNumber);        
        Serial.println("Premi per ricominciare.");
  
        g_rndNumber = 0;

        // ** CHAGE STATE **
        g_state = RE_START;
        break;
    }
}
74HC595
74HC595