// ============================================================
// SISTEMA DI RILEVAMENTO SENSORI HALL SS49E - Arduino Nano
// Controllo OUTPUT basato su logica sequenziale con lockout
// ============================================================

// ===== CONFIGURAZIONE DEBUG =====
#define DEBUG_MODE 1        // 1 = debug attivo, 0 = zero overhead
#define BAUD_RATE 115200    // 115200 = veloce, 9600 = lento

#if DEBUG_MODE
  #define DEBUG_PRINT(x) Serial.println(x)
  #define DEBUG_SETUP() Serial.begin(BAUD_RATE); delay(100);
#else
  #define DEBUG_PRINT(x)
  #define DEBUG_SETUP()
#endif

// ===== DEFINIZIONI PIN =====
#define HallPin_0 A7   // SENSORE 0: Magnete 1 (attiva OUTPUT_1)
#define HallPin_1 A1   // SENSORE 1: Magnete 2 (interrompe OUTPUT_1)
#define HallPin_2 A0   // SENSORE 2: Magnete 3 (attiva OUTPUT_2)

#define LED_Pin 13     // LED stato
#define OUT_LED_Pin 9  // LED output
#define OUTPUT_1 3     // Gestito da Sensore 0
#define OUTPUT_2 6     // Gestito da Sensore 2

// ===== SOGLIE DI RILEVAMENTO =====
#define THRESHOLD_0 0.03   // Sensore 0 (Volt)
#define THRESHOLD_1 0.04   // Sensore 1 (Volt)
#define THRESHOLD_2 0.04  // Sensore 2 (Volt)


// ===== VARIABI CALCOLO VELOCITA =====
unsigned long time_sensor_0 = 0;
unsigned long time_sensor_2 = 0;
unsigned long time_sensor_1 = 0;
bool sensor_0_detected = false;
bool sensor_1_detected = false;
bool sensor_2_detected = false;

bool has_velocity_0_1 = false;
bool has_velocity_1_2 = false;

// ===== TIMING =====
#define NUM_READINGS 3              // Numero letture per media mobile
#define OUTPUT_DURATION 100         // Durata impulso output (ms)
#define LOCKOUT_DURATION 4000       // Tempo di lockout dopo output (ms)

// ============================================================
// VARIABILI SENSORE 0 (OUTPUT_1)
// ============================================================
float baselineVoltage_0 = 0;
int readings_0[NUM_READINGS] = {0};
long total_0 = 0;
int average_0 = 0;

bool outputActive_0 = false;
bool lockoutActive_0 = false;
unsigned long outputStartTime_0 = 0;
unsigned long lockoutStartTime_0 = 0;

// ============================================================
// VARIABILI SENSORE 1 (Trigger di interruzione)
// ============================================================
float baselineVoltage_1 = 0;
int readings_1[NUM_READINGS] = {0};
long total_1 = 0;
int average_1 = 0;

// ============================================================
// VARIABILI SENSORE 2 (OUTPUT_2)
// ============================================================
float baselineVoltage_2 = 0;
int readings_2[NUM_READINGS] = {0};
long total_2 = 0;
int average_2 = 0;

bool outputActive_2 = false;
bool lockoutActive_2 = false;
unsigned long outputStartTime_2 = 0;
unsigned long lockoutStartTime_2 = 0;

// ===== INDICE MEDIA MOBILE =====
int readIndex = 0;

// ============================================================
// CALCOLO VELOCITÀ DELLA PALLINA
// ============================================================
void calculateAndPrintVelocity() {
  // Distanze in metri
  const float distance_0_1_m = 0.027;  // 27 mm
  const float distance_1_2_m = 0.058;  // 58 mm
  float velocity_0_1 = 0.0;
  float velocity_1_2 = 0.0;

  
  // Calcola velocità 1: tra sensore 0 e sensore 1
  if (sensor_0_detected && sensor_1_detected && time_sensor_0 > 0 && time_sensor_1 > 0) {
    unsigned long delta_time_0_1 = time_sensor_1 - time_sensor_0;
    velocity_0_1 = (distance_0_1_m / (float)delta_time_0_1) * 1000.0;  // m/s
    has_velocity_0_1 = true;

   // DEBUG_PRINT(String("[VELOCITÀ] S0→S1: ") + String(velocity_0_1, 2) + " m/s (Δt: " + delta_time_0_1 + "ms)");
   // Serial.print("V01=");
   // Serial.println(velocity_0_1, 2);
  }
  
  // Calcola velocità 2: tra sensore 1 e sensore 2
  if (sensor_1_detected && sensor_2_detected && time_sensor_1 > 0 && time_sensor_2 > 0) {
    unsigned long delta_time_1_2 = time_sensor_2 - time_sensor_1;
    velocity_1_2 = (distance_1_2_m / (float)delta_time_1_2) * 1000.0;  // m/s
    has_velocity_1_2 = true;

    //DEBUG_PRINT(String("[VELOCITÀ] S1→S2: ") + String(velocity_1_2, 2) + " m/s (Δt: " + delta_time_1_2 + "ms)");
   // Serial.print("V12=");
    //Serial.println(velocity_1_2, 2);
  }
  // Calcola e stampa velocità media
  if (has_velocity_0_1 && has_velocity_1_2) {
    float average_velocity = (velocity_0_1 + velocity_1_2) / 2.0;
    DEBUG_PRINT(String("[VELOCITÀ MEDIA] ") + String(average_velocity, 2) + " m/s");
    Serial.print("VAVG=");
    Serial.println(average_velocity, 2);
  }
}


// ============================================================
// SETUP
// ============================================================
void setup() {
  // Configurazione pin ingresso
  pinMode(HallPin_0, INPUT);
  pinMode(HallPin_1, INPUT);
  pinMode(HallPin_2, INPUT);
  
  // Configurazione pin uscita
  pinMode(LED_Pin, OUTPUT);
  pinMode(OUT_LED_Pin, OUTPUT);
  pinMode(OUTPUT_1, OUTPUT);
  pinMode(OUTPUT_2, OUTPUT);
  
  // Inizializza uscite a LOW
  digitalWrite(OUTPUT_1, LOW);
  digitalWrite(OUTPUT_2, LOW);
  digitalWrite(LED_Pin, HIGH);      // Acceso durante setup
  digitalWrite(OUT_LED_Pin, HIGH);  // Acceso durante setup
  
  // Inizializza seriale (solo se DEBUG_MODE = 1)
  DEBUG_SETUP();
  
  // Stampa intestazione (solo se DEBUG_MODE = 1)
  DEBUG_PRINT("\n========================================");
  DEBUG_PRINT("SISTEMA SENSORI HALL - INIZIO SETUP");
  DEBUG_PRINT("========================================");
  DEBUG_PRINT("Calibrazione in corso...\n");
  
  // Calibrazione baseline per tutti i sensori
  calibrateSensors();
  
  DEBUG_PRINT("\n========================================");
  DEBUG_PRINT("SETUP COMPLETATO - SISTEMA PRONTO");
  DEBUG_PRINT("========================================\n");
  
  delay(1000);
  
  // Spegni LED setup
  digitalWrite(LED_Pin, LOW);
  digitalWrite(OUT_LED_Pin, LOW);
}

// ============================================================
// CALIBRAZIONE SENSORI
// ============================================================
void calibrateSensors() {
  long sum_0 = 0;
  long sum_1 = 0;
  long sum_2 = 0;
  
  // Leggi 50 campioni da ogni sensore
  for (int i = 0; i < 50; i++) {
    sum_0 += analogRead(HallPin_0);
    delay(10);
    sum_1 += analogRead(HallPin_1);
    delay(10);
    sum_2 += analogRead(HallPin_2);
    delay(10);
  }
  
  // Calcola medie
  int baselineADC_0 = sum_0 / 50;
  int baselineADC_1 = sum_1 / 50;
  int baselineADC_2 = sum_2 / 50;
  
  // Converti a voltaggio
  baselineVoltage_0 = (baselineADC_0 * 5.0) / 1023.0;
  baselineVoltage_1 = (baselineADC_1 * 5.0) / 1023.0;
  baselineVoltage_2 = (baselineADC_2 * 5.0) / 1023.0;
  
  // Stampa baseline (solo se DEBUG_MODE = 1)
  DEBUG_PRINT("Calibrazione Sensore 0:");
  DEBUG_PRINT(String("  ADC: ") + baselineADC_0 + ", Voltage: " + baselineVoltage_0 + " V");
  
  DEBUG_PRINT("Calibrazione Sensore 1:");
  DEBUG_PRINT(String("  ADC: ") + baselineADC_1 + ", Voltage: " + baselineVoltage_1 + " V");
  
  DEBUG_PRINT("Calibrazione Sensore 2:");
  DEBUG_PRINT(String("  ADC: ") + baselineADC_2 + ", Voltage: " + baselineVoltage_2 + " V");
}

// ============================================================
// LOOP PRINCIPALE
// ============================================================
void loop() {
  // ===== LETTURA SENSORI CON MEDIA MOBILE =====
  
  // Sottrai il valore più vecchio dal totale
  total_0 -= readings_0[readIndex];
  total_1 -= readings_1[readIndex];
  total_2 -= readings_2[readIndex];
  
  // Leggi nuovi valori
  readings_0[readIndex] = analogRead(HallPin_0);
  readings_1[readIndex] = analogRead(HallPin_1);
  readings_2[readIndex] = analogRead(HallPin_2);
  
  // Aggiungi i nuovi valori al totale
  total_0 += readings_0[readIndex];
  total_1 += readings_1[readIndex];
  total_2 += readings_2[readIndex];
  
  // Sposta indice (circolare)
  readIndex = (readIndex + 1) % NUM_READINGS;
  
  // Calcola medie
  average_0 = total_0 / NUM_READINGS;
  average_1 = total_1 / NUM_READINGS;
  average_2 = total_2 / NUM_READINGS;
  
  // Converti a voltaggio
  float currentVoltage_0 = (average_0 * 5.0) / 1023.0;
  float currentVoltage_1 = (average_1 * 5.0) / 1023.0;
  float currentVoltage_2 = (average_2 * 5.0) / 1023.0;
  
  // Calcola delta rispetto baseline
  float voltageDelta_0 = currentVoltage_0 - baselineVoltage_0;
  float voltageDelta_1 = currentVoltage_1 - baselineVoltage_1;
  float voltageDelta_2 = currentVoltage_2 - baselineVoltage_2;

  //DEBUG_PRINT(String("  S1: ") + currentVoltage_0 );
   DEBUG_PRINT(String("  S2: ") + currentVoltage_1 );
  // DEBUG_PRINT(String("  S3: ") + currentVoltage_2 );
  delay(1000);
  
  //DEBUG_PRINT(String(" Sens0: ") + voltageDelta_0);
  // ===== LOGICA SENSORE 0 → OUTPUT_1 =====
  // ATTIVAZIONE: Se S0 detecta movimento E output non attivo E non in lockout
  if (voltageDelta_0 > THRESHOLD_0 && !outputActive_0 && !lockoutActive_0) {
    digitalWrite(LED_Pin, HIGH);
    digitalWrite(OUTPUT_1, HIGH);
    outputActive_0 = true;
    outputStartTime_0 = millis();

    // NUOVO: Registra timestamp rilevamento
    time_sensor_0 = millis();
    sensor_0_detected = true;
    DEBUG_PRINT("[S0] OUTPUT_1 ATTIVATO");
  }
  
  // DISATTIVAZIONE: Output_1 si spegne se (timeout OR sensore 1 detectato)
  if (outputActive_0) {
    bool timeoutExpired = (millis() - outputStartTime_0 >= OUTPUT_DURATION);
    bool sensor1Detected = (voltageDelta_1 > THRESHOLD_1);
    
    if (timeoutExpired || sensor1Detected) {
      digitalWrite(LED_Pin, LOW);
      digitalWrite(OUTPUT_1, LOW);
      outputActive_0 = false;
      lockoutActive_0 = true;
      lockoutStartTime_0 = millis();
      
      if (sensor1Detected) {
        time_sensor_1 = millis();
        sensor_1_detected = true;
        DEBUG_PRINT("[S1] RILEVATO");
        calculateAndPrintVelocity();  // Calcola V01
        sensor_0_detected = false;
        DEBUG_PRINT("[S0/S1] OUTPUT_1 DISATTIVATO (trigger Sensore 1)");
      } else {
        DEBUG_PRINT("[S0] OUTPUT_1 DISATTIVATO (timeout)");
      }
    }
  }
  
  // FINE LOCKOUT: Consenti riattivazione
  if (lockoutActive_0 && (millis() - lockoutStartTime_0 >= LOCKOUT_DURATION)) {
    lockoutActive_0 = false;
    sensor_0_detected = false;  // NUOVO
    sensor_1_detected = false;  // NUOVO
    sensor_2_detected = false;  // NUOVO
    DEBUG_PRINT("[S0] LOCKOUT terminato");
  }
  
  // ===== LOGICA SENSORE 2 → OUTPUT_2 =====
  // ATTIVAZIONE: Se S2 detecta movimento E output non attivo E non in lockout
  if (voltageDelta_2 > THRESHOLD_2 && !outputActive_2 && !lockoutActive_2) {
    digitalWrite(OUT_LED_Pin, HIGH);
    digitalWrite(OUTPUT_2, HIGH);
    outputActive_2 = true;
    outputStartTime_2 = millis();

    // NUOVO: Registra timestamp rilevamento
    time_sensor_2 = millis();
    sensor_2_detected = true;
    DEBUG_PRINT("[S2] OUTPUT_2 ATTIVATO");
    calculateAndPrintVelocity();  // Calcola V12
    has_velocity_0_1 = false;
    has_velocity_1_2 = false;
  }
  
  // DISATTIVAZIONE: Output_2 si spegne dopo timeout
  if (outputActive_2 && (millis() - outputStartTime_2 >= OUTPUT_DURATION)) {
    digitalWrite(OUT_LED_Pin, LOW);
    digitalWrite(OUTPUT_2, LOW);
    outputActive_2 = false;
    lockoutActive_2 = true;
    lockoutStartTime_2 = millis();
    DEBUG_PRINT("[S2] OUTPUT_2 DISATTIVATO (timeout)");
  }
  
  // FINE LOCKOUT: Consenti riattivazione
  if (lockoutActive_2 && (millis() - lockoutStartTime_2 >= LOCKOUT_DURATION)) {
    lockoutActive_2 = false;
    DEBUG_PRINT("[S2] LOCKOUT terminato");
  }
}

// ============================================================
// FINE CODICE
// ============================================================