#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>


#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);
    digitalWrite(ledPins[i], HIGH);
    delay(100);
    digitalWrite(ledPins[i], LOW);
  }
  pinMode(PIN_JOY_1_V, INPUT);
  pinMode(PIN_JOY_1_H, INPUT);
  pinMode(PIN_JOY_1_B, INPUT);
  pinMode(PIN_JOY_2_V, INPUT);
  pinMode(PIN_JOY_2_H, INPUT);
  pinMode(PIN_JOY_2_B, INPUT);

  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);
  pinMode(PIN_ENC_DT, INPUT);
  pinMode(PIN_ENC_CL, INPUT);

  pinMode(PIN_LEDRING, OUTPUT);

  pinMode(PIN_BUZZ, OUTPUT);

  pinMode(SERVO_1, OUTPUT);
  pinMode(SERVO_2, 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);
}

void setup() {
  InitPINs();
}

void loop() {
}