#include "HX711.h"
#include <ESP32Servo.h>

const int CLOCK_COMUNE = 22; 

// CORREZIONE: Aggiunte le parentesi quadre [] per definire correttamente gli array
const int PIN_HX711_DATI[] = {21, 19, 18, 5, 17}; 
const int PIN_SERVO[] = {13, 12, 14, 27, 26}; 
const String NOMI_DITA[] = {"Pollice", "Indice", "Medio", "Anulare", "Mignolo"};

// CORREZIONE: Dichiarati 5 moduli separati per sensori e motori
HX711 dita[5];
Servo motori[5];

const float FATTORE_CALIBRAZIONE = 420.0; 
const float SOGLIA_CRITICA_N = 20.0; 

void setup() {
  Serial.begin(115200);
  
  for(int i = 0; i < 5; i++) {
    dita[i].begin(PIN_HX711_DATI[i], CLOCK_COMUNE);
    dita[i].set_scale(FATTORE_CALIBRAZIONE);
    dita[i].tare(); 
    
    motori[i].attach(PIN_SERVO[i]);
    motori[i].write(0); 
  }
  Serial.println("--- Sistema Realistico ad Anello Chiuso Attivo ---");
}

void loop() {
  if (Serial.available() > 0) {
    String dataIn = Serial.readStringUntil('\n'); 
    dataIn.trim(); 
    
    // CORREZIONE: Dichiarato l'array per contenere i 5 angoli target
    int targetAngolo[5] = {0, 0, 0, 0, 0}; 
    int rimossoIndice = 0;
    
    for (int i = 0; i < 5; i++) {
      int virgolaIndice = dataIn.indexOf(',', rimossoIndice);
      if (virgolaIndice == -1) {
        targetAngolo[i] = dataIn.substring(rimossoIndice).toInt();
      } else {
        targetAngolo[i] = dataIn.substring(rimossoIndice, virgolaIndice).toInt();
        rimossoIndice = virgolaIndice + 1;
      }
    }

    for(int i = 0; i < 5; i++) {
      float forzaNewton = 0.0;
      if (dita[i].is_ready()) {
        float pesoGrammi = dita[i].get_units(1);
        if (pesoGrammi < 0) pesoGrammi = 0.0;
        forzaNewton = (pesoGrammi / 1000.0) * 9.80665;
      }

      if (forzaNewton >= SOGLIA_CRITICA_N) {
        // CORREZIONE: Gestione corretta della concatenazione delle stringhe di stampa
        Serial.print(" [COMPRESSIONE MAX ");
        Serial.print(NOMI_DITA[i]);
        Serial.print("] ");
      } else if (forzaNewton > 2.0) {
        int angoloAdattivo = map(forzaNewton, 2, SOGLIA_CRITICA_N, targetAngolo[i], motori[i].read());
        motori[i].write(angoloAdattivo);
      } else {
        motori[i].write(targetAngolo[i]); 
      }
      
      Serial.print(NOMI_DITA[i]);
      Serial.print(":");
      Serial.print(forzaNewton, 1);
      Serial.print("N->");
      Serial.print(motori[i].read());
      Serial.print("deg ");
    }
    Serial.println();
  }
  delay(30); 
}