#include <Wire.h>
#include <SPI.h>
#include <LiquidCrystal_I2C.h>
#include <MD_Parola.h>
#include <MD_MAX72xx.h>
#include <Adafruit_NeoPixel.h>
#include "TM1637.h"
#include <Servo.h>
#include <GyverOLED.h>
#include "font.h"

// Definisci PIN
#define PIN_JOY_1_V A0
#define PIN_JOY_1_H A1
#define PIN_JOY_1_B 52
#define PIN_JOY_2_V A3
#define PIN_JOY_2_H A4
#define PIN_JOY_2_B 53
#define PIN_POT_1 A2
#define PIN_POT_2 A5
#define PIN_POT_3 A6
#define PIN_POT_4 A7
#define PIN_ENC_SW 50
#define PIN_ENC_DT 51
#define PIN_ENC_CL 49
#define PIN_LEDRING 48
#define PIN_BUZZ 47
#define SERVO_1 46
#define SERVO_2 44
#define PIN_MATRIX_DIN 43
#define PIN_MATRIX_CS 42
#define PIN_MATRIX_CLK 45
#define PIN_4DIG_1_DIO 41
#define PIN_4DIG_1_CLK 40
#define PIN_4DIG_2_DIO 38
#define PIN_4DIG_2_CLK 39
#define PIN_ULTRA_1_TRIG 36
#define PIN_ULTRA_1_ECHO 37
#define PIN_ULTRA_2_TRIG 34
#define PIN_ULTRA_2_ECHO 35
#define PIN_LED_JOY_GREEN 32
#define PIN_LED_JOY_RED 33
#define PIN_LED_JOY_F1 30
#define PIN_LED_JOY_F2 31
#define PIN_LED_JOY_F3 28
#define PIN_LED_JOY_F4 29
#define PIN_LED_DIST_GREEN 12
#define PIN_LED_DIST_RED 13
#define PIN_LED_DIST_1_H 26
#define PIN_LED_DIST_1_O 27
#define PIN_LED_DIST_1_L 25
#define PIN_LED_DIST_2_H 24
#define PIN_LED_DIST_2_O 23
#define PIN_LED_DIST_2_L 22
#define PIN_LED_DIST_SERVO 11
#define PIN_LED_ROLL_F1 10
#define PIN_LED_ROLL_F2 9
#define PIN_LED_ROLL_F3 8
#define PIN_LED_ROLL_F4 7
#define PIN_LED_ROLL_F5 6
#define PIN_LED_ROLL_SERVO 5
#define PIN_LED_ULTRA_1 4
#define PIN_LED_ULTRA_2 3


// Inizializza PIN
void InitPINs() {
  int ledPins[] = {
    PIN_LED_JOY_GREEN, PIN_LED_JOY_RED, PIN_LED_JOY_F1, PIN_LED_JOY_F2, PIN_LED_JOY_F3, PIN_LED_JOY_F4,
    PIN_LED_DIST_GREEN, PIN_LED_DIST_RED, PIN_LED_DIST_1_H, PIN_LED_DIST_1_O, PIN_LED_DIST_1_L, PIN_LED_DIST_2_H, PIN_LED_DIST_2_O, PIN_LED_DIST_2_L, PIN_LED_DIST_SERVO,
    PIN_LED_ROLL_F1, PIN_LED_ROLL_F2, PIN_LED_ROLL_F3, PIN_LED_ROLL_F4, PIN_LED_ROLL_F5, PIN_LED_ROLL_SERVO,
    PIN_LED_ULTRA_1, PIN_LED_ULTRA_2
  };
  int numLeds = sizeof(ledPins) / sizeof(ledPins[0]);
  for (int i = 0; i < numLeds; i++) {
    pinMode(ledPins[i], OUTPUT);
  }
  pinMode(PIN_JOY_1_V, INPUT);
  pinMode(PIN_JOY_1_H, INPUT);
  pinMode(PIN_JOY_1_B, INPUT_PULLUP);
  pinMode(PIN_JOY_2_V, INPUT);
  pinMode(PIN_JOY_2_H, INPUT);
  pinMode(PIN_JOY_2_B, INPUT_PULLUP);
  pinMode(PIN_POT_1, INPUT);
  pinMode(PIN_POT_2, INPUT);
  pinMode(PIN_POT_3, INPUT);
  pinMode(PIN_POT_4, INPUT);
  pinMode(PIN_ENC_SW, INPUT_PULLUP);
  pinMode(PIN_ENC_DT, INPUT);
  pinMode(PIN_ENC_CL, INPUT);
  pinMode(PIN_LEDRING, OUTPUT);
  pinMode(PIN_BUZZ, OUTPUT);
  pinMode(PIN_MATRIX_DIN, OUTPUT);
  pinMode(PIN_MATRIX_CS, OUTPUT);
  pinMode(PIN_MATRIX_CLK, OUTPUT);
  pinMode(PIN_4DIG_1_DIO, OUTPUT);
  pinMode(PIN_4DIG_1_CLK, OUTPUT);
  pinMode(PIN_4DIG_2_DIO, OUTPUT);
  pinMode(PIN_4DIG_2_CLK, OUTPUT);
  pinMode(PIN_ULTRA_1_TRIG, OUTPUT);
  pinMode(PIN_ULTRA_1_ECHO, INPUT);
  pinMode(PIN_ULTRA_2_TRIG, OUTPUT);
  pinMode(PIN_ULTRA_2_ECHO, INPUT);
}

// Inizializza componenti
Servo MOTA;
Servo MOTB;
LiquidCrystal_I2C lcd(0x27, 16, 2);
TM1637 d1(PIN_4DIG_1_CLK, PIN_4DIG_1_DIO);
TM1637 d2(PIN_4DIG_2_CLK, PIN_4DIG_2_DIO);
MD_Parola P = MD_Parola('MD_MAX72XX::FC16_HW', PIN_MATRIX_DIN, PIN_MATRIX_CLK, PIN_MATRIX_CS, 1);
Adafruit_NeoPixel strip(24, PIN_LEDRING, NEO_GRB + NEO_KHZ800);
GyverOLED<SSD1306_128x64, OLED_NO_BUFFER> oled;

// Genera Codici
int Cifre[16]; 
int Codice[4];
void GeneraCodici() {
//Codice 16 cifre  
for (int i = 0; i < 16; i++) {
    int nuovoNumero;
    do {
      nuovoNumero = random(0, 10);
    } while (i > 0 && nuovoNumero == Cifre[i - 1]);
    Cifre[i] = nuovoNumero;
  }  
//Codice 4 cifre
  for (int i = 0; i < 4; i++) {
    bool numeroTrovato = false;
    do {
      Codice[i] = random(0, 10);
      numeroTrovato = true;
      for (int j = 0; j < i; j++) {
        if (Codice[i] == Codice[j]) {
          numeroTrovato = false;
          break;
        }
      }
    } while (!numeroTrovato);
  }

}

//Variabili per Fase 2
bool Ch1 = false, Ch2 = false, Ch3 = false, Ch4 = false;
int PosDisplay[5] = { 99, 0, 4, 8, 12 };
int VAL_M = 0;
int VAL_C = 0;
int VAL_D = 0;
int VAL_U = 0;     
int Passo = 1;     
bool FInit = false;

//Variabili per Fase 4
unsigned long startTime;
unsigned long lastBeep;
unsigned long currentTime;
//Distanze sensori ultra
int MIN_S1 = 20;
int MAX_S1 = 30;
int MIN_S2 = 30;
int MAX_S2 = 40;

//Variabili per Fase 5
bool RollReady = false;
//Caratteri per il roller
char Roller[24][1] = {"A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V", "W", "X"};
int RollCount= 0;
bool RollCounter= false;
//Codice per il roller
char RollerCode[5][1] = {"A", "B", "C", "D", "E"};
int RollerCodeCount = 0;
int lastClk = HIGH;
int Temp;


//STATO INIZIALE
int state = 2;


// ******* SETUP
void setup() {
  InitPINs();  //Inizializza PIN

  randomSeed(analogRead(8)*analogRead(9));
  GeneraCodici(); //Codici Casuali

  lcd.init();
  lcd.backlight(); //LCD1602 i2c

  d1.init();
  d1.set(BRIGHT_TYPICAL); //4-digit Fasi 1-2
  d2.init();
  d2.set(BRIGHT_TYPICAL); //4-digit Fasi 3-4

  P.begin();
  P.setFont(numeric7Seg); //DotMatrix Fase 5

  strip.begin(); //Led ring Fae 5

  MOTA.attach(SERVO_1); //Servo Fase 4
  MOTB.attach(SERVO_2); //Servo Fase 5

  oled.init();
//TotalTest();

oled.clear();
oled.setScale(3);
oled.setCursor(1, 1);
oled.print("Hello!");
oled.update();

}
// ******* /SETUP


// ******* LOOP
void loop() {
  switch (state) {
    case 1:
      funzione1(); //FASE 1 - Barre
      break;
    case 2:
      funzione2(); //FASE 2 - Joy4x4
      break;
    case 3:
      funzione3(); //FASE 3 - Codice 4
      break;
    case 4:
      funzione4(); //FASE 4 - Distance
      break;
    case 5:
      funzione5(); //FASE 5 - Roller
      break;
    case 6:
      funzione6();
      break;
    case 7:
      funzione7();
      break;
    case 8:
      funzione8();
      break;
    case 9:
      funzione9();
      break;
    case 10:
      funzione10();
      break;
    default:
      break;
  }
}// ******* /LOOP

// ######## FASE 1 #########
void funzione1() {
  char S_A[17] = "                ";
  char S_B[17] = "                ";
  int P1 = analogRead(PIN_POT_1) / 63;
  int P2 = analogRead(PIN_POT_2) / 63;

  P1 = constrain(P1, 1, 16);
  P2 = constrain(P2, 1, 16);

  for (int i = 1; i <= 16; i++) {
    S_A[i - 1] = (i == P1) ? '|' : ' ';
    S_B[i - 1] = (i == P2) ? '|' : ' ';
  }

  lcd.setCursor(0, 0);
  lcd.print(S_A);
  lcd.setCursor(0, 1);
  lcd.print(S_B);

  if (checkConditions(P1, P2)) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("  GENERAZIONE   ");
    lcd.setCursor(0, 1);
    lcd.print(" CODICE ACCESSO ");

    for (int i = 28; i < 32; i++) {
      playTone(1000, 200);
      digitalWrite(i, LOW);
      delay(1000);
    }

    state = 2;
  }
}

bool checkConditions(int P1, int P2) {
  const int Sequence[][2] = {
    {13, 3},
    {4, 15},
    {10, 5},
    {6, 11}
  };
  bool* ledArray[] = {&Ch1, &Ch2, &Ch3, &Ch4};
  int ledPins[] = {PIN_LED_JOY_F1, PIN_LED_JOY_F2, PIN_LED_JOY_F3, PIN_LED_JOY_F4};
  for (int i = 0; i < 4; ++i) {
    if (P1 == Sequence[i][0] && P2 == Sequence[i][1] && !(*ledArray[i])) {
      digitalWrite(ledPins[i], HIGH);
      *ledArray[i] = true;
    }
  }
  return (Ch1 && Ch2 && Ch3 && Ch4);
}

// ######## FASE 2 #########
void funzione2() {

  if (FInit == false)
  {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("DECODIFICA DATI ");
    for (int i = 0; i <= 16; i++) {
      lcd.setCursor(i, 1);
      lcd.print("*");
      delay(200);
    }
    lcd.clear();
    playTone(1000, 200);
    for (int i = 0; i < 16; ++i) {
      lcd.setCursor(i, 0);
      lcd.print(Cifre[i]);
    }
    lcd.setCursor(0, 1);
    lcd.print("0000");
    digitalWrite(PIN_LED_JOY_GREEN, LOW);  //LED VERDE
    digitalWrite(PIN_LED_JOY_RED, HIGH);   //LED ROSSO
    FInit = true;
  }
  int H1 = analogRead(PIN_JOY_1_H);
  int V1 = analogRead(PIN_JOY_1_V);
  int H2 = analogRead(PIN_JOY_2_H);
  int V2 = analogRead(PIN_JOY_2_V);

  switch (Passo) {

      case 1:
        if (V1 < 300) { VAL_U++; }
        if (V1 > 700) { VAL_C--; }
        if (H1 > 700) { VAL_C++; }
        if (H1 < 300) { VAL_D--; }
        if (V2 < 300) { VAL_M++; }
        if (V2 > 700) { VAL_U--; }
        if (H2 > 700) { VAL_D++; }
        if (H2 < 300) { VAL_M--; }
        break;

      case 2:
        if (V1 < 300) { VAL_D--; }
        if (V1 > 700) { VAL_C--; }
        if (H1 > 700) { VAL_U++; }
        if (H1 < 300) { VAL_M--; }
        if (V2 < 300) { VAL_D++; }
        if (V2 > 700) { VAL_C++; }
        if (H2 > 700) { VAL_M++; }
        if (H2 < 300) { VAL_U--; }
        break;

      case 3:
        if (V1 < 300) { VAL_U--; }
        if (V1 > 700) { VAL_D--; }
        if (H1 > 700) { VAL_M++; }
        if (H1 < 300) { VAL_C--; }
        if (V2 < 300) { VAL_D++; }
        if (V2 > 700) { VAL_M--; }
        if (H2 > 700) { VAL_C++; }
        if (H2 < 300) { VAL_U++; }
        break;
      case 4:
        if (V1 < 300) { VAL_D++; }
        if (V1 > 700) { VAL_U--; }
        if (H1 > 700) { VAL_C++; }
        if (H1 < 300) { VAL_M--; }
        if (V2 < 300) { VAL_U++; }
        if (V2 > 700) { VAL_D--; }
        if (H2 > 700) { VAL_M++; }
        if (H2 < 300) { VAL_C--; }
        break;
    }

    if (VAL_M < 0) { VAL_M = 0; }  
    if (VAL_M > 9) { VAL_M = 9; }
    if (VAL_C < 0) { VAL_C = 0; }
    if (VAL_C > 9) { VAL_C = 9; }
    if (VAL_D < 0) { VAL_D = 0; }
    if (VAL_D > 9) { VAL_D = 9; }
    if (VAL_U < 0) { VAL_U = 0; }
    if (VAL_U > 9) { VAL_U = 9; }


  d1.display(0, VAL_M);
  d1.display(1, VAL_C);
  d1.display(2, VAL_D);
  d1.display(3, VAL_U);

  delay(100);


  if (digitalRead(PIN_JOY_1_B) == LOW)  { //&& digitalRead(S_PIN2) == LOW)

    bool a = checkOK(Passo, VAL_M, VAL_C, VAL_D, VAL_U);

    if (a == true) {
      Passo++;
      playTone(2000, 200);

      switch (Passo) {
        case 2:
          digitalWrite(PIN_LED_JOY_F1, HIGH);
          break;
        case 3:
          digitalWrite(PIN_LED_JOY_F2, HIGH);
          break;
        case 4:
          digitalWrite(PIN_LED_JOY_F3, HIGH);
          break;
        case 5:
          digitalWrite(PIN_LED_JOY_F4, HIGH);
          digitalWrite(PIN_LED_JOY_GREEN, HIGH);  //LED VERDE
          digitalWrite(PIN_LED_JOY_RED, LOW);   //LED ROSSO

          VAL_M = Codice[0];
          VAL_C = Codice[1];
          VAL_D = Codice[2];
          VAL_U = Codice[3];

          lcd.setCursor(0, 0);
          lcd.print(" AUTORIZZATO AL ");
          lcd.setCursor(0, 1);
          lcd.print(" CODICE SBLOCCO ");

          for (int i = 28; i < 32; i++) {
          playTone(3000, 200);
          digitalWrite(i, LOW);
          delay(500);
          }
            d1.set(1);
            d1.display(0, VAL_M);
            d1.display(1, VAL_C);
            d1.display(2, VAL_D);
            d1.display(3, VAL_U);
            delay(300);
          digitalWrite(PIN_LED_JOY_GREEN, LOW);  //LED VERDE
          digitalWrite(PIN_LED_JOY_RED, LOW);   //LED ROSSO

          digitalWrite(PIN_LED_DIST_GREEN, LOW);  //LED VERDE
          digitalWrite(PIN_LED_DIST_RED, HIGH);   //LED ROSSO
        lcd.noBacklight();
          state = 3;
          break;
      }
    } else {
      lcd.setCursor(PosDisplay[Passo], 1);
      lcd.print("0000");
      VAL_M = 0;
      VAL_C = 0;
      VAL_D = 0;
      VAL_U = 0;
      d1.display(0, VAL_M);
      d1.display(1, VAL_C);
      d1.display(2, VAL_D);
      d1.display(3, VAL_U);
      }
      lcd.setCursor(PosDisplay[Passo], 1);
      lcd.print("0000");
      VAL_M = 0;
      VAL_C = 0;
      VAL_D = 0;
      VAL_U = 0;
      d1.display(0, VAL_M);
      d1.display(1, VAL_C);
      d1.display(2, VAL_D);
      d1.display(3, VAL_U);

  }

}

bool checkOK(int B, int VAL_M, int VAL_C, int VAL_D, int VAL_U) {
  int Quale = PosDisplay[B];
  if (Cifre[Quale] == VAL_M && Cifre[Quale + 1] == VAL_C && Cifre[Quale + 2] == VAL_D && Cifre[Quale + 3] == VAL_U) {
    lcd.setCursor(PosDisplay[B], 1);
    lcd.print(VAL_M);
    lcd.print(VAL_C);
    lcd.print(VAL_D);
    lcd.print(VAL_U);
    return true;
  } else {
    playTone(100, 300);
    return false;
  }
}

// ######## FASE 3 #########

void funzione3() {

    int val1 = analogRead(PIN_POT_3);
    val1 = map(val1, 0, 1023, 99, 0);
    int val2 = analogRead(PIN_POT_4);
    val2 = map(val2, 0, 1023, 99, 0);

    int c1 = val1 / 10;
    int c2 = val1 - (c1 * 10);
    int c3 = val2 / 10;
    int c4 = val2 - (c3 * 10);
      d2.display(0, c1);
      d2.display(1, c4);
      d2.display(2, c3);
      d2.display(3, c2);

    if (c1 == Codice[0] && c2 == Codice[3] && c3 == Codice[2] && c4 == Codice[1]) {
      playTone(2000, 200);
      delay(1000);
      playTone(2000, 200);
      delay(1000);
      state = 4;
      startTime = millis();
      lastBeep = millis();
      d1.clearDisplay();
      d2.clearDisplay();
      return;
    }

}

// ######## FASE 4 #########

void funzione4() {

    digitalWrite(PIN_LED_ULTRA_1, HIGH);
    digitalWrite(PIN_ULTRA_1_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PIN_ULTRA_1_TRIG, LOW);
    int MISURA_S1 = pulseIn(PIN_ULTRA_1_ECHO, HIGH);

    digitalWrite(PIN_LED_ULTRA_2, HIGH);
    digitalWrite(PIN_ULTRA_2_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PIN_ULTRA_2_TRIG, LOW);
    int MISURA_S2 = pulseIn(PIN_ULTRA_2_ECHO, HIGH);

    const int DIST_MAX = 99;
    const int DIST_DIVISOR = 58;
    int DIST_S1 = min(MISURA_S1 / DIST_DIVISOR, DIST_MAX);
    int distS1Units = DIST_S1 % 10;
    int distS1Tens = (DIST_S1 / 10) % 10;
    d2.display(0, distS1Tens);
    d2.display(1, distS1Units);
    int DIST_S2 = min(MISURA_S2 / DIST_DIVISOR, DIST_MAX);
    int distS2Units = DIST_S2 % 10;
    int distS2Tens = (DIST_S2 / 10) % 10;
    d2.display(2, distS2Tens);
    d2.display(3, distS2Units);

    if (DIST_S1 <= MIN_S1) {
      digitalWrite(PIN_LED_DIST_1_L, HIGH);
    }
    if (DIST_S1 >= MAX_S1) {
      digitalWrite(PIN_LED_DIST_1_H, HIGH);
    }
    if (DIST_S1 > MIN_S1 && DIST_S1 < MAX_S1) {
      digitalWrite(PIN_LED_DIST_1_O, HIGH);
    }

    if (DIST_S2 <= MIN_S2) {
      digitalWrite(PIN_LED_DIST_2_L, HIGH);
    }
    if (DIST_S2 >= MAX_S2) {
      digitalWrite(PIN_LED_DIST_2_H, HIGH);
    }
    if (DIST_S2 > MIN_S2 && DIST_S2 < MAX_S2) {
      digitalWrite(PIN_LED_DIST_2_O, HIGH);
    }

    if ((DIST_S1 > MIN_S1 && DIST_S1 < MAX_S1) && (DIST_S2 > MIN_S2 && DIST_S2 < MAX_S2)) {
      currentTime = millis();
      if (currentTime - lastBeep >= 500) {
      playTone(1500, 5);
        lastBeep = millis();
      }

      if (currentTime - startTime >= 5000) {
        digitalWrite(PIN_LED_ULTRA_1, LOW);
        digitalWrite(PIN_LED_ULTRA_2, LOW);
          digitalWrite(PIN_LED_DIST_GREEN, HIGH);  //LED VERDE
          digitalWrite(PIN_LED_DIST_RED, LOW);   //LED ROSSO
        d2.clearDisplay();
      for (int p = 22; p < 28; p++) {
        digitalWrite(p, HIGH);
      }
        for (int pos = 0; pos <= 180; pos += 1) {
          digitalWrite(PIN_LED_DIST_SERVO, HIGH);
          delay(10);
          MOTA.write(pos);
          digitalWrite(PIN_LED_DIST_SERVO, LOW);
          delay(20);
        }
      for (int p = 22; p < 28; p++) {
        digitalWrite(p, LOW);
      }
        digitalWrite(PIN_LED_DIST_SERVO, HIGH);
          digitalWrite(PIN_LED_DIST_GREEN, LOW);  //LED VERDE
        state=5;
      }

    } else {

      for (int p = 22; p < 28; p++) {
        digitalWrite(p, LOW);
      }
      digitalWrite(PIN_LED_ULTRA_1, LOW);
      digitalWrite(PIN_LED_ULTRA_2, LOW);
      delay(100);
      startTime = millis();
    }
}


// ######## FASE 5 #########
void funzione5() {

          if (RollReady==false) {
                            for (int c = 0; c < 4; c++) {
                    for (int i = 0; i < 24; i++) {
                      strip.setPixelColor(i - 4, strip.Color(0, 0, 0));
                      strip.setPixelColor(i - 3, strip.Color(50, 0, 0));
                      strip.setPixelColor(i - 2, strip.Color(100, 0, 0));
                      strip.setPixelColor(i - 1, strip.Color(180, 0, 0));
                      strip.setPixelColor(i, strip.Color(255, 0, 0));
                      strip.show();
                      delay(50);
                    }
                    SpegniPixel();
                  }
                  strip.setPixelColor(0, strip.Color(255, 0, 0));
                  strip.setPixelColor(12, strip.Color(255, 0, 0));
                  strip.show();
                  delay(50);
                  P.print(Roller[RollCount][0]);
                  RollReady=true;
          }

 int newClk = digitalRead(PIN_ENC_CL);
  if (newClk != lastClk) {
    lastClk = newClk;
    int dtValue = digitalRead(PIN_ENC_DT);

      if (RollCount> 11) {Temp=-12;} else {Temp=12;}

    if (newClk == LOW && dtValue == HIGH) {
      strip.setPixelColor(RollCount, strip.Color(0, 0, 0));
      strip.setPixelColor(RollCount+ Temp, strip.Color(0, 0, 0));
      strip.show();
      RollCount++;
      RollCounter= true;
    }
    if (newClk == LOW && dtValue == LOW) {
      strip.setPixelColor(RollCount, strip.Color(0, 0, 0));
      strip.setPixelColor(RollCount+ Temp, strip.Color(0, 0, 0));
      RollCount--;
      RollCounter= true;
    }
    if (RollCount< 0) {
      RollCount= 23;
    }
    if (RollCount> 23) {
      RollCount= 0;
    }
    P.print(Roller[RollCount][0]);
    strip.setPixelColor(RollCount, strip.Color(255, 0, 0));
    strip.setPixelColor(RollCount+ Temp, strip.Color(255, 0, 0));

    strip.show();
  }

  if (digitalRead(PIN_ENC_SW) == LOW) {

    if (RollCounter) {

      if (Roller[RollCount][0] == RollerCode[RollerCodeCount][0]) {
        FlashVerde();
        SpegniPixel();
        strip.setPixelColor(RollCount, strip.Color(0, 255, 0));
       strip.setPixelColor(RollCount+ Temp, strip.Color(0, 255, 0));
       strip.show();
        RollerCodeCount++;
        digitalWrite(RollerCodeCount+5, HIGH);
      } else {
        FlashRosso();
        SpegniPixel();
        strip.setPixelColor(RollCount, strip.Color(255, 0, 0));
             strip.setPixelColor(RollCount+ Temp, strip.Color(255, 0, 0));
        strip.show();
    for (int i = 10; i >= 6; i--) {
        digitalWrite(i, LOW);
      }

        RollerCodeCount = 0;
        delay(1000);
      }
      RollCounter= false;
    }
  }

  if (RollerCodeCount == 5) {
    P.print(" ");
    for (int i = 24; i >= 0; i--) {
      strip.setPixelColor(i, strip.Color(0, 255, 0));
      strip.show();
    }
    SpegniPixel();
    for (int i = 10; i >= 6; i--) {
        digitalWrite(i, LOW);
      }
          digitalWrite(PIN_LED_DIST_SERVO, LOW);
          for (int pos = 0; pos <= 180; pos += 1) {
          digitalWrite(PIN_LED_ROLL_SERVO, HIGH);
          delay(10);
          MOTB.write(pos);
          digitalWrite(PIN_LED_ROLL_SERVO, LOW);
          delay(20);
        }
          digitalWrite(PIN_LED_ROLL_SERVO, HIGH);

          state=6;
  }

}
void SpegniPixel() {
  for (int i = 24; i >= 0; i--) {
    strip.setPixelColor(i, strip.Color(0, 0, 0));
    strip.show();
  }
}

void FlashRosso() {
  for (int i = 24; i >= 0; i--) {
    strip.setPixelColor(i, strip.Color(200, 0, 0));
    strip.show();
  }
      playTone(200, 300);

  delay((200));
}

void FlashVerde() {
  for (int i = 24; i >= 0; i--) {
    strip.setPixelColor(i, strip.Color(0, 200, 0));
    strip.show();
  }
      playTone(2000, 200);

  delay((200));
}



// ######## FASE 6 #########


void funzione6() {
}

void funzione7() {
}

void funzione8() {
}

void funzione9() {
}

void funzione10() {
}


// PLAYTONE
void playTone(int frequency, int duration) {
  tone(PIN_BUZZ, frequency);
  delay(duration);
  noTone(PIN_BUZZ);
}


// TOTAL TEST
void TotalTest() {

  playTone(1000, 300);

  int ledPins[] = {
    PIN_LED_JOY_GREEN, PIN_LED_JOY_RED, PIN_LED_JOY_F1, PIN_LED_JOY_F2, PIN_LED_JOY_F3, PIN_LED_JOY_F4,
    PIN_LED_DIST_GREEN, PIN_LED_DIST_RED, PIN_LED_DIST_1_H, PIN_LED_DIST_1_O, PIN_LED_DIST_1_L, PIN_LED_DIST_2_H, PIN_LED_DIST_2_O, PIN_LED_DIST_2_L, PIN_LED_DIST_SERVO,
    PIN_LED_ROLL_F1, PIN_LED_ROLL_F2, PIN_LED_ROLL_F3, PIN_LED_ROLL_F4, PIN_LED_ROLL_F5, PIN_LED_ROLL_SERVO,
    PIN_LED_ULTRA_1, PIN_LED_ULTRA_2
  };
  int numLeds = sizeof(ledPins) / sizeof(ledPins[0]);
  for (int i = 0; i < numLeds; i++) {
    digitalWrite(ledPins[i], HIGH);
    delay(200);
    digitalWrite(ledPins[i], LOW);
  }
  MOTA.write(180);
  MOTB.write(180);

  for (long firstPixelHue = 0; firstPixelHue < 3 * 65536; firstPixelHue += 256) {
    for (int i = 0; i < 24; i++) {
      int pixelHue = firstPixelHue + (i * 65536L / 24);
      strip.setPixelColor(i, strip.gamma32(strip.ColorHSV(pixelHue)));
    }
    strip.show();
  }
  lcd.setCursor(2, 0);
  lcd.print("Inizializzo");
  for (int i = 0; i < 4; i++) {
    d1.display(i, 8);
    d2.display(i, 8);
  }
  P.print("A");
  MOTA.write(0);
  MOTB.write(0);

  playTone(2000, 300);

}