#include "HX711.h"
#include <Wire.h>
#include <vl53lx_class.h>

// --- MOTEUR AVANT-GAUCHE (Front-Left / FL) ---
const int pinM1A_FL = 5;
const int pinM1B_FL = 7;
const int pinEncA_FL = 2;
const int pinEncB_FL = 22;

// --- MOTEUR AVANT-DROIT (Front-Right / FR) ---
const int pinM1A_FR = 6;
const int pinM1B_FR = 8;
const int pinEncA_FR = 3;
const int pinEncB_FR = 23;

// --- MOTEUR ARRIÈRE-GAUCHE (Back-Left / BL) ---
const int pinM1A_BL = 9;
const int pinM1B_BL = 11;
const int pinEncA_BL = 18;
const int pinEncB_BL = 24;

// --- MOTEUR ARRIÈRE-DROIT (Back-Right / BR) ---
const int pinM1A_BR = 10;
const int pinM1B_BR = 12;
const int pinEncA_BR = 19;
const int pinEncB_BR = 25;

// --- ULTRASONS ---
const int trigAvant = 30;
const int echoAvant = 31;

const int trigArriere = 32;
const int echoArriere = 33;

const int trigGauche = 34;
const int echoGauche = 35;

const int trigDroite = 36;
const int echoDroite = 37;

// --- ToF VL53L3CX avec bibliothèque STM32duino ---
const int pinXSHUT_Gauche = 38;
const int pinXSHUT_Droite = 40;

#define DEV_I2C Wire

VL53LX tofGauche(&DEV_I2C, pinXSHUT_Gauche);
VL53LX tofDroite(&DEV_I2C, pinXSHUT_Droite);

const float toleranceMm = 10.0;
const int vitesseAlignement = 70;
const int delaiControle = 50;

// --- CAPTEUR DE FORCE ---
HX711 scale;
const int pinData = 46;
const int pinSCK = 48;

// À calibrer correctement ensuite
const float INSERT_CALIBRATION_FACTOR = 1.0;

// --- SEUILS ---
const float seuilLongueur = 80.0;
const float seuilLargeur = 60.0;
float seuilTirage = 200.0;

// --- PARAMÈTRES DE MOUVEMENT ---
volatile long compteurFL = 0;
volatile long compteurFR = 0;
volatile long compteurBL = 0;
volatile long compteurBR = 0;

const long cibleTics = 834;
const long cibleTicsLateral = 417;

const int vitesseMoteur = 100;

void setup() {
  Serial.begin(9600);

  // --- MOTEURS ---
  int brochesMoteurs[] = {
    pinM1A_FL, pinM1B_FL,
    pinM1A_FR, pinM1B_FR,
    pinM1A_BL, pinM1B_BL,
    pinM1A_BR, pinM1B_BR
  };

  for (int i = 0; i < 8; i++) {
    pinMode(brochesMoteurs[i], OUTPUT);
  }

  // --- ENCODEURS ---
  int brochesEncodeurs[] = {
    pinEncA_FL, pinEncB_FL,
    pinEncA_FR, pinEncB_FR,
    pinEncA_BL, pinEncB_BL,
    pinEncA_BR, pinEncB_BR
  };

  for (int i = 0; i < 8; i++) {
    pinMode(brochesEncodeurs[i], INPUT_PULLUP);
  }

  attachInterrupt(digitalPinToInterrupt(pinEncA_FL), isrFL, RISING);
  attachInterrupt(digitalPinToInterrupt(pinEncA_FR), isrFR, RISING);
  attachInterrupt(digitalPinToInterrupt(pinEncA_BL), isrBL, RISING);
  attachInterrupt(digitalPinToInterrupt(pinEncA_BR), isrBR, RISING);

  // --- ULTRASONS ---
  pinMode(trigAvant, OUTPUT);
  pinMode(echoAvant, INPUT);

  pinMode(trigArriere, OUTPUT);
  pinMode(echoArriere, INPUT);

  pinMode(trigGauche, OUTPUT);
  pinMode(echoGauche, INPUT);

  pinMode(trigDroite, OUTPUT);
  pinMode(echoDroite, INPUT);

  // --- CAPTEUR DE FORCE ---
  scale.begin(pinData, pinSCK);
  scale.set_scale(INSERT_CALIBRATION_FACTOR);
  scale.tare();

  // --- ToF ---
  Wire.begin();

  pinMode(pinXSHUT_Gauche, OUTPUT);
  pinMode(pinXSHUT_Droite, OUTPUT);

  digitalWrite(pinXSHUT_Gauche, LOW);
  digitalWrite(pinXSHUT_Droite, LOW);
  delay(100);

  // Initialisation ToF gauche
  digitalWrite(pinXSHUT_Gauche, HIGH);
  delay(100);

  tofGauche.begin();
  tofGauche.InitSensor(0x30);
  tofGauche.VL53LX_StartMeasurement();

  Serial.println("ToF gauche OK");

  // Initialisation ToF droit
  digitalWrite(pinXSHUT_Droite, HIGH);
  delay(100);

  tofDroite.begin();
  tofDroite.InitSensor(0x31);
  tofDroite.VL53LX_StartMeasurement();

  Serial.println("ToF droit OK");

  Serial.println("Base prete ! Depart dans 3 secondes...");
  delay(3000);

  ajusterOrientation();
  delay(2000);

  avancer();
  delay(2000);

  ajusterOrientation();
  delay(2000);

  commandeCapteurForces();
  delay(2000);

  ajusterOrientation();
  delay(2000);

  reculer();
}

void loop() {
  // Le test s'exécute une seule fois dans setup()
}

void resetCompteurs() {
  compteurFL = 0;
  compteurFR = 0;
  compteurBL = 0;
  compteurBR = 0;
}

void stopMoteurs() {
  digitalWrite(pinM1A_FL, LOW);
  digitalWrite(pinM1B_FL, LOW);

  digitalWrite(pinM1A_FR, LOW);
  digitalWrite(pinM1B_FR, LOW);

  digitalWrite(pinM1A_BL, LOW);
  digitalWrite(pinM1B_BL, LOW);

  digitalWrite(pinM1A_BR, LOW);
  digitalWrite(pinM1B_BR, LOW);
}

long calculerMoyenneAbsolue() {
  return (abs(compteurFL) + abs(compteurFR) + abs(compteurBL) + abs(compteurBR)) / 4;
}

// --- INTERRUPTIONS ENCODEURS ---
void isrFL() {
  digitalRead(pinEncB_FL) == HIGH ? compteurFL++ : compteurFL--;
}

void isrFR() {
  digitalRead(pinEncB_FR) == HIGH ? compteurFR++ : compteurFR--;
}

void isrBL() {
  digitalRead(pinEncB_BL) == HIGH ? compteurBL++ : compteurBL--;
}

void isrBR() {
  digitalRead(pinEncB_BR) == HIGH ? compteurBR++ : compteurBR--;
}

// --- AVANCER ---
void avancer() {
  resetCompteurs();
  Serial.println("--- MARCHE AVANT 50 cm ---");

  analogWrite(pinM1A_FL, vitesseMoteur);
  digitalWrite(pinM1B_FL, LOW);

  analogWrite(pinM1A_FR, vitesseMoteur);
  digitalWrite(pinM1B_FR, LOW);

  analogWrite(pinM1A_BL, vitesseMoteur);
  digitalWrite(pinM1B_BL, LOW);

  analogWrite(pinM1A_BR, vitesseMoteur);
  digitalWrite(pinM1B_BR, LOW);

  while (calculerMoyenneAbsolue() < cibleTics) {
    if (obstacleDetecte()) {
      stopMoteurs();
      return;
    }

    Serial.print("Moyenne tics : ");
    Serial.println(calculerMoyenneAbsolue());

    delay(50);
  }

  stopMoteurs();
  Serial.println("50 cm avant atteints !");
}

// --- RECULER ---
void reculer() {
  resetCompteurs();
  Serial.println("--- MARCHE ARRIERE 50 cm ---");

  digitalWrite(pinM1A_FL, LOW);
  analogWrite(pinM1B_FL, vitesseMoteur);

  digitalWrite(pinM1A_FR, LOW);
  analogWrite(pinM1B_FR, vitesseMoteur);

  digitalWrite(pinM1A_BL, LOW);
  analogWrite(pinM1B_BL, vitesseMoteur);

  digitalWrite(pinM1A_BR, LOW);
  analogWrite(pinM1B_BR, vitesseMoteur);

  while (calculerMoyenneAbsolue() < cibleTics) {
    if (obstacleDetecte()) {
      stopMoteurs();
      return;
    }

    Serial.print("Moyenne tics : ");
    Serial.println(calculerMoyenneAbsolue());

    delay(50);
  }

  stopMoteurs();
  Serial.println("50 cm arriere atteints !");
}

// --- CAPTEUR DE FORCE ---
void commandeCapteurForces() {
  Serial.println("--- LECTURE CAPTEUR DE FORCE ---");

  float force = scale.get_units(5);

  Serial.print("Force mesuree : ");
  Serial.println(force);

  if (force > seuilTirage) {
    mouvementDroite();
  } else {
    stopMoteurs();
  }

  if (obstacleDetecte()) {
    stopMoteurs();
    return;
  }
}

// --- MOUVEMENT LATERAL DROITE ---
void mouvementDroite() {
  resetCompteurs();

  Serial.println("--- MOUVEMENT LATERAL DROITE ---");

  analogWrite(pinM1A_FL, vitesseMoteur);
  digitalWrite(pinM1B_FL, LOW);

  analogWrite(pinM1B_FR, vitesseMoteur);
  digitalWrite(pinM1A_FR, LOW);

  analogWrite(pinM1B_BL, vitesseMoteur);
  digitalWrite(pinM1A_BL, LOW);

  analogWrite(pinM1A_BR, vitesseMoteur);
  digitalWrite(pinM1B_BR, LOW);

  while (calculerMoyenneAbsolue() < cibleTicsLateral) {
    if (obstacleDetecte()) {
      stopMoteurs();
      return;
    }

    Serial.print("Tics lateral : ");
    Serial.println(calculerMoyenneAbsolue());

    delay(50);
  }

  stopMoteurs();
  Serial.println("Mouvement lateral termine !");
}

// --- ULTRASON ---
float lireUltrason(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duree = pulseIn(echoPin, HIGH, 30000);

  if (duree == 0) {
    return -1;
  }

  return duree * 0.343 / 2.0;
}

// --- DETECTION OBSTACLE ---
bool obstacleDetecte() {
  float dAvant = lireUltrason(trigAvant, echoAvant);
  float dArriere = lireUltrason(trigArriere, echoArriere);
  float dGauche = lireUltrason(trigGauche, echoGauche);
  float dDroite = lireUltrason(trigDroite, echoDroite);

  if (dAvant > 0 && dAvant < seuilLongueur) {
    Serial.println("STOP obstacle avant");
    return true;
  }

  if (dArriere > 0 && dArriere < seuilLongueur) {
    Serial.println("STOP obstacle arriere");
    return true;
  }

  if (dGauche > 0 && dGauche < seuilLargeur) {
    Serial.println("STOP obstacle gauche");
    return true;
  }

  if (dDroite > 0 && dDroite < seuilLargeur) {
    Serial.println("STOP obstacle droite");
    return true;
  }

  return false;
}

// --- ALIGNEMENT AVEC LES ToF ---
void ajusterOrientation() {
  Serial.println("--- ALIGNEMENT PARALLELE AU LIT ---");

  while (true) {
    if (obstacleDetecte()) {
      stopMoteurs();
      return;
    }

    float distanceGauche = lireToFGauche();
    float distanceDroite = lireToFDroite();

    if (distanceGauche < 0 || distanceDroite < 0) {
      Serial.println("Erreur lecture ToF");
      stopMoteurs();
      return;
    }

    float erreur = distanceGauche - distanceDroite;

    Serial.print("ToF gauche = ");
    Serial.print(distanceGauche);
    Serial.print(" mm | ToF droite = ");
    Serial.print(distanceDroite);
    Serial.print(" mm | erreur = ");
    Serial.println(erreur);

    if (abs(erreur) <= toleranceMm) {
      stopMoteurs();
      Serial.println("Base alignee parallele au lit !");
      return;
    }

    if (erreur > 0) {
      rotationDroite(vitesseAlignement);
    } else {
      rotationGauche(vitesseAlignement);
    }

    delay(delaiControle);
  }
}

// --- ROTATION GAUCHE ---
void rotationGauche(int vitesse) {
  analogWrite(pinM1A_FR, vitesse);
  digitalWrite(pinM1B_FR, LOW);

  analogWrite(pinM1A_BR, vitesse);
  digitalWrite(pinM1B_BR, LOW);

  analogWrite(pinM1B_FL, vitesse);
  digitalWrite(pinM1A_FL, LOW);

  analogWrite(pinM1B_BL, vitesse);
  digitalWrite(pinM1A_BL, LOW);
}

// --- ROTATION DROITE ---
void rotationDroite(int vitesse) {
  analogWrite(pinM1B_FR, vitesse);
  digitalWrite(pinM1A_FR, LOW);

  analogWrite(pinM1B_BR, vitesse);
  digitalWrite(pinM1A_BR, LOW);

  analogWrite(pinM1A_FL, vitesse);
  digitalWrite(pinM1B_FL, LOW);

  analogWrite(pinM1A_BL, vitesse);
  digitalWrite(pinM1B_BL, LOW);
}

// --- LECTURE ToF GAUCHE ---
float lireToFGauche() {
  VL53LX_MultiRangingData_t data;
  uint8_t ready = 0;

  tofGauche.VL53LX_GetMeasurementDataReady(&ready);

  if (ready) {
    tofGauche.VL53LX_GetMultiRangingData(&data);

    int distance = data.RangeData[0].RangeMilliMeter;

    tofGauche.VL53LX_ClearInterruptAndStartMeasurement();

    if (distance > 0) {
      return (float)distance;
    }
  }

  return -1;
}

// --- LECTURE ToF DROITE ---
float lireToFDroite() {
  VL53LX_MultiRangingData_t data;
  uint8_t ready = 0;

  tofDroite.VL53LX_GetMeasurementDataReady(&ready);

  if (ready) {
    tofDroite.VL53LX_GetMultiRangingData(&data);

    int distance = data.RangeData[0].RangeMilliMeter;

    tofDroite.VL53LX_ClearInterruptAndStartMeasurement();

    if (distance > 0) {
      return (float)distance;
    }
  }

  return -1;
}