#include <math.h>
#include <SPI.h>
#include <mcp2515_can.h>
#include "Ktech_motor.h"
#include <Dynamixel2Arduino.h>
#include <SoftwareSerial.h>
#include <Servo.h>

// ============================================================
//  CONFIGURATION MATERIELLE
// ============================================================
const int SPI_CS_PIN      = 9;
const int DXL_DIR_PIN     = 2;
const int GRIPPER_PIN     = 6;
const int GRIPPER_POS_PIN = A15;

SoftwareSerial PC_SERIAL(7, 8);

const uint8_t DXL_ID               = 1;
const float   DXL_PROTOCOL_VERSION = 1.0;

#define NUMBER_OF_MOTORS 2
Motor KTMOTORS[NUMBER_OF_MOTORS] = { Motor(1), Motor(3) };

// US[0] pins 22-23 : capteur LATERAL DROIT
// US[1] pins 42-43 : capteur AVANT
#define NUMBER_OF_US_SENSORS 2
const int trigPinUS[NUMBER_OF_US_SENSORS] = { 22, 42 };
const int echoPinUS[NUMBER_OF_US_SENSORS] = { 23, 43 };

#define R     0.023f
#define E     0.20f
#define MY_PI 3.14159265359f

// ============================================================
//  PARAMETRES
// ============================================================

#define DXL_PROFILE_VEL     20
#define DXL_POS_HAUT        750
#define DXL_POS_BAS         540

#define PINCE_OUVERTE_US    500
#define PINCE_FERMEE_US     950

// Vitesses
#define VITESSE_AVANCE         0.4f
#define VITESSE_AVANCE_LENTE   0.3f

// Correcteur P suivi mur
#define DIST_MUR_CIBLE         22.4f
#define KP_MUR                 0.03f
#define V_ROT_MAX              0.3f
#define SEUIL_ERREUR_LENTE     12.0f

// Seuils capteur avant
#define DIST_DETECTION_TOTEM   20.0f
#define DIST_MUR_ATTEINT       30.0f

// Distances precises
#define DIST_AVANCE_TOTEM      0.11f
#define DIST_DEGAGEMENT        0.10f
#define DIST_TRAVERSEE_ARENE   1.10f
#define DIST_RECUL_DEPOT       0.10f

// ============================================================
//  OBJETS
// ============================================================
mcp2515_can       CAN(SPI_CS_PIN);
Dynamixel2Arduino dxl(Serial, DXL_DIR_PIN);
Servo             pince;

using namespace ControlTableItem;

// ============================================================
//  VARIABLES GLOBALES
// ============================================================
float _refPos0    = 0.0f;
float _refPos1    = 0.0f;
float _a1_mesure  = 0.0f;  // distance parcourue jusqu'au totem
float _dist_depot = 0.0f;  // distance capteur avant au moment de la saisie

// ============================================================
//  CONVERSIONS
// ============================================================
float deg_to_m(float deg) {
  return (deg * MY_PI / 180.0f) * R;
}

// ============================================================
//  CINEMATIQUE
// ============================================================
void MotorSpeed(float v_translation, float v_rotation) {
  v_translation = -v_translation;
  float w1 =  (v_translation / R) + (E / (2.0f * R)) * v_rotation;
  float w2 =  (v_translation / R) - (E / (2.0f * R)) * v_rotation;
  long int rpm1 = (long int)(w1 * 60.0f / (2.0f * MY_PI));
  long int rpm2 = (long int)(w2 * 60.0f / (2.0f * MY_PI));
  KTMOTORS[0].sendVelocityCommand(rpm1, CAN);
  KTMOTORS[1].sendVelocityCommand(-rpm2, CAN);
}

void stopMotors() {
  KTMOTORS[0].sendVelocityCommand(0, CAN);
  KTMOTORS[1].sendVelocityCommand(0, CAN);
}

// ============================================================
//  CAPTEURS ULTRASON
// ============================================================
float lireDistance(int pinTrig, int pinEcho) {
  digitalWrite(pinTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(pinTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrig, LOW);
  long duration = pulseIn(pinEcho, HIGH, 30000UL);
  if (duration == 0) return 999.0f;
  return (duration * 0.0343f) / 2.0f;
}

bool distanceValide(float d) {
  return (d > 2.0f && d < 900.0f);
}

// ============================================================
//  ACTIONNEURS
// ============================================================
void controlePince(bool ouvrir) {
  if (ouvrir) {
    pince.writeMicroseconds(PINCE_OUVERTE_US);
    PC_SERIAL.println("Pince : ouverture");
  } else {
    pince.writeMicroseconds(PINCE_FERMEE_US);
    PC_SERIAL.println("Pince : fermeture");
  }
  delay(800);
}

void baisserBras() {
  PC_SERIAL.println("Bras : descente (200)...");
  dxl.setGoalPosition(DXL_ID, DXL_POS_BAS);
  delay(1500);
  PC_SERIAL.println("Bras : bas OK.");
}

void leverBras() {
  PC_SERIAL.println("Bras : montee (700)...");
  dxl.setGoalPosition(DXL_ID, DXL_POS_HAUT);
  delay(1500);
  PC_SERIAL.println("Bras : haut OK.");
}



// ============================================================
//  ODOMETRIE
// ============================================================
void reset_encodeurs() {
  KTMOTORS[0].readState(CAN);
  KTMOTORS[1].readState(CAN);
  delay(50);
  _refPos0 = KTMOTORS[0].posInDeg;
  _refPos1 = KTMOTORS[1].posInDeg;
}

void lire_d0_d1(float &d0, float &d1) {
  KTMOTORS[0].readState(CAN);
  KTMOTORS[1].readState(CAN);
  d0 =  deg_to_m(KTMOTORS[0].posInDeg - _refPos0);
  d1 = -deg_to_m(KTMOTORS[1].posInDeg - _refPos1);
}

float mesurer_distance_parcourue() {
  float d0, d1;
  lire_d0_d1(d0, d1);
  PC_SERIAL.print("d0: "); PC_SERIAL.print(d0, 4);
  PC_SERIAL.print(" | d1: "); PC_SERIAL.println(d1, 4);
  return (d0 + d1) / 2.0f;
}

// ============================================================
//  CORRECTEUR P SUIVI MUR (droite uniquement)
// ============================================================
void avancerSuiviMur() {
  float distMur = lireDistance(trigPinUS[0], echoPinUS[0]);
  float v_rot   = 0.0f;

  if (distMur < 70.0f && distMur > 2.0f) {
    float erreur = distMur - DIST_MUR_CIBLE;
    v_rot = KP_MUR * erreur;
    if (v_rot >  V_ROT_MAX) v_rot =  V_ROT_MAX;
    if (v_rot < -V_ROT_MAX) v_rot = -V_ROT_MAX;
  }

  float erreur_abs = fabs(distMur - DIST_MUR_CIBLE);
  float v_avance = (erreur_abs > SEUIL_ERREUR_LENTE) ? VITESSE_AVANCE_LENTE : VITESSE_AVANCE;

  MotorSpeed(v_avance, v_rot);

  PC_SERIAL.print("Lat: "); PC_SERIAL.print(distMur, 1);
  PC_SERIAL.print(" | v_rot: "); PC_SERIAL.println(v_rot, 2);
}

// ============================================================
//  AVANCEMENTS PAR ODOMETRIE
// ============================================================
void avancerDistancePrecise(float distCible_m) {
  if (distCible_m <= 0.0f) return;
  PC_SERIAL.print("Avancer "); PC_SERIAL.print(distCible_m, 3); PC_SERIAL.println(" m...");
  reset_encodeurs();
  delay(50);
  while (true) {
    float d = mesurer_distance_parcourue();
    if (fabs(d) >= distCible_m) { stopMotors(); break; }
    MotorSpeed(VITESSE_AVANCE, 0.0f);
    delay(30);
  }
  PC_SERIAL.println("Distance atteinte.");
}

void reculerDistancePrecise(float distCible_m) {
  if (distCible_m <= 0.0f) return;
  PC_SERIAL.print("Reculer "); PC_SERIAL.print(distCible_m, 3); PC_SERIAL.println(" m...");
  reset_encodeurs();
  delay(50);
  while (true) {
    float d = mesurer_distance_parcourue();
    if (fabs(d) >= distCible_m) { stopMotors(); break; }
    MotorSpeed(-VITESSE_AVANCE, 0.0f);
    delay(30);
  }
  PC_SERIAL.println("Recul atteint.");
}

void avancerDistanceSuiviMur(float distCible_m) {
  if (distCible_m <= 0.0f) return;
  PC_SERIAL.print("Avancer suivi mur "); PC_SERIAL.print(distCible_m, 3); PC_SERIAL.println(" m...");
  reset_encodeurs();
  delay(50);
  while (true) {
    float d = mesurer_distance_parcourue();
    if (fabs(d) >= distCible_m) { stopMotors(); break; }
    avancerSuiviMur();
    delay(30);
  }
  PC_SERIAL.println("Distance atteinte.");
}

// ============================================================
//  ROTATIONS PAR ODOMETRIE
// ============================================================
void tournerDroite90() {
  PC_SERIAL.println("Rotation 90 droite...");
  reset_encodeurs();
  delay(50);
  float cible_deg = ((E * (MY_PI / 2.0f)) / 2.0f) / (MY_PI * R) * 180.0f * 0.83f;
  while (true) {
    KTMOTORS[0].readState(CAN);
    KTMOTORS[1].readState(CAN);
    float p0  = KTMOTORS[0].posInDeg - _refPos0;
    float p1  = KTMOTORS[1].posInDeg - _refPos1;
    float avg = (fabs(p0) + fabs(p1)) / 2.0f;
    if (avg >= cible_deg) { stopMotors(); break; }
    MotorSpeed(0.0f, 0.6f);
    delay(30);
  }
  delay(200);
  PC_SERIAL.println("Rotation droite OK.");
}

void tournerGauche90() {
  PC_SERIAL.println("Rotation 90 gauche...");
  reset_encodeurs();
  delay(50);
  float cible_deg = ((E * (MY_PI / 2.0f)) / 2.0f) / (MY_PI * R) * 180.0f * 0.83f;
  while (true) {
    KTMOTORS[0].readState(CAN);
    KTMOTORS[1].readState(CAN);
    float p0  = KTMOTORS[0].posInDeg - _refPos0;
    float p1  = KTMOTORS[1].posInDeg - _refPos1;
    float avg = (fabs(p0) + fabs(p1)) / 2.0f;
    if (avg >= cible_deg) { stopMotors(); break; }
    MotorSpeed(0.0f, -0.6f);
    delay(30);
  }
  delay(200);
  PC_SERIAL.println("Rotation gauche OK.");
}

// ============================================================
//  VERIFICATION INITIALE
// ============================================================
void verifierEtatInitial() {
  PC_SERIAL.println("--- Verification initiale ---");
  leverBras();
  controlePince(true);
  int posPince = analogRead(GRIPPER_POS_PIN);
  PC_SERIAL.print("Retour position pince (A15): "); PC_SERIAL.println(posPince);
  PC_SERIAL.println("Etat initial OK.");
}

// ============================================================
//  SETUP
// ============================================================
void setup() {
  PC_SERIAL.begin(115200);
  delay(500);

  for (int i = 0; i < NUMBER_OF_US_SENSORS; i++) {
    pinMode(trigPinUS[i], OUTPUT);
    pinMode(echoPinUS[i], INPUT);
  }

  pince.attach(GRIPPER_PIN, PINCE_OUVERTE_US, PINCE_FERMEE_US);
  pinMode(GRIPPER_POS_PIN, INPUT);

  while (CAN_OK != CAN.begin(CAN_500KBPS)) {
    PC_SERIAL.println("CAN init echouee, retry...");
    delay(500);
  }
  PC_SERIAL.println("CAN : OK");

  dxl.begin(1000000);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  dxl.ping(DXL_ID);
  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
  dxl.torqueOn(DXL_ID);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, DXL_PROFILE_VEL);
  PC_SERIAL.println("Dynamixel : OK");

  for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
    KTMOTORS[i].prevPosInDeg  = 0.0f;
    KTMOTORS[i].revolNumber   = 0;
    KTMOTORS[i].offsetEncoder = 0;
  }
  PC_SERIAL.println("Moteurs KTech : OK");

  KTMOTORS[0].readState(CAN);
  KTMOTORS[1].readState(CAN);
  KTMOTORS[0].prevPosInDeg = KTMOTORS[0].posInDeg;
  KTMOTORS[1].prevPosInDeg = KTMOTORS[1].posInDeg;
  delay(200);
  reset_encodeurs();

  PC_SERIAL.println();
  PC_SERIAL.println("************************************************************");
  PC_SERIAL.println("  SYSTEME PRET                                              ");
  PC_SERIAL.println("************************************************************");
}

// ============================================================
//  MACHINE A ETATS
// ============================================================
enum Etats {
  INITIALISATION,
  DEGAGEMENT,
  ROTATION_DROITE_1,
  ALLER_VERS_DC,
  ROTATION_GAUCHE_1,
  CHERCHER_TOTEM,
  BAISSER_ET_AVANCER,
  SAISIR_TOTEM,
  REHAUSSER_BRAS,
  DISTANCE_RESTANTE,
  ROTATION_GAUCHE_2,
  ALLER_MUR_FACE,
  ROTATION_GAUCHE_3,
  ALLER_POSITION_DEPOT,
  DEPOSER_TOTEM,
  FIN_PROGRAMME
};

Etats etape = INITIALISATION;

void loop() {
  switch (etape) {

    case INITIALISATION:
      verifierEtatInitial();
      PC_SERIAL.println("Depart dans 2 secondes...");
      delay(2000);
      etape = DEGAGEMENT;
      break;

    case DEGAGEMENT:
      PC_SERIAL.println("=== Etape 1 : degagement mur AD ===");
      avancerDistancePrecise(DIST_DEGAGEMENT);
      etape = ROTATION_DROITE_1;
      break;

    case ROTATION_DROITE_1:
      PC_SERIAL.println("=== Etape 2 : rotation 90 droite ===");
      tournerDroite90();
      reset_encodeurs();
      etape = ALLER_VERS_DC;
      break;

    case ALLER_VERS_DC: {
      float distAvant = lireDistance(trigPinUS[1], echoPinUS[1]);
      static int compteur_av = 0;
      if (distAvant < DIST_MUR_ATTEINT && distanceValide(distAvant)) {
        compteur_av++;
      } else {
        compteur_av = 0;
      }
      if (compteur_av >= 3) {
        compteur_av = 0;
        stopMotors();
        PC_SERIAL.println("Mur DC atteint.");
        etape = ROTATION_GAUCHE_1;
      } else {
        avancerSuiviMur();
      }
      delay(40);
      break;
    }

    case ROTATION_GAUCHE_1:
      PC_SERIAL.println("=== Etape 4 : rotation 90 gauche ===");
      tournerGauche90();
      reset_encodeurs();
      _a1_mesure = 0.0f;
      etape = CHERCHER_TOTEM;
      break;

    case CHERCHER_TOTEM: {
      float distAvant = lireDistance(trigPinUS[1], echoPinUS[1]);
      static int compteur_totem = 0;
      if (distAvant < DIST_DETECTION_TOTEM && distanceValide(distAvant)) {
        compteur_totem++;
      } else {
        compteur_totem = 0;
      }
      if (compteur_totem >= 3) {
        compteur_totem = 0;
        _a1_mesure = mesurer_distance_parcourue();
        PC_SERIAL.print("Totem detecte ! a1 = ");
        PC_SERIAL.print(_a1_mesure, 3); PC_SERIAL.println(" m");
        stopMotors();
        etape = BAISSER_ET_AVANCER;
      } else {
        avancerSuiviMur();
      }
      delay(40);
      break;
    }

    case BAISSER_ET_AVANCER:
      PC_SERIAL.println("=== Etape 6 : baisser bras et avancer vers totem ===");
      baisserBras();
      avancerDistancePrecise(DIST_AVANCE_TOTEM);
      etape = SAISIR_TOTEM;
      break;

    case SAISIR_TOTEM: {
      PC_SERIAL.println("=== Etape 7 : saisie totem ===");
      controlePince(false);
      delay(500);
      etape = REHAUSSER_BRAS;
      break;
    }

    case REHAUSSER_BRAS:
      PC_SERIAL.println("=== Etape 8 : lever bras ===");
      leverBras();
      etape = DISTANCE_RESTANTE;
      break;
    case DISTANCE_RESTANTE:
      // Capture de la distance restante jusqu'au mur en face
      // On essaie plusieurs fois pour avoir une valeur valide
      _dist_depot = 999.0f;
      delay(2000);
      for (int i = 0; i < 5; i++) {
        float d = lireDistance(trigPinUS[1], echoPinUS[1]);
        if (distanceValide(d)) {
          _dist_depot = d;
          break;
        }
        delay(100);
      }
      PC_SERIAL.print("Distance depot sauvegardee : ");
      PC_SERIAL.print(_dist_depot, 1); PC_SERIAL.println(" cm");
      etape = ROTATION_GAUCHE_2;
      break;
    case ROTATION_GAUCHE_2:
      PC_SERIAL.println("=== Etape 9 : rotation 90 gauche ===");
      tournerGauche90();
      etape = ALLER_MUR_FACE;
      break;

    case ALLER_MUR_FACE: {
      // Avance jusqu'au mur DC (en face) puis tourne gauche
      float distAvant = lireDistance(trigPinUS[1], echoPinUS[1]);
      static int compteur_mur = 0;
      if (distAvant < DIST_MUR_ATTEINT && distanceValide(distAvant)) {
        compteur_mur++;
      } else {
        compteur_mur = 0;
      }
      if (compteur_mur >= 3) {
        compteur_mur = 0;
        stopMotors();
        PC_SERIAL.println("Mur face atteint, rotation gauche.");
        tournerGauche90();
        reset_encodeurs();
        etape = ALLER_POSITION_DEPOT;
      } else {
        MotorSpeed(VITESSE_AVANCE, 0.0f);
      }
      delay(40);
      break;
    }

    case ALLER_POSITION_DEPOT: {
      // Avance jusqu'a ce que le capteur avant lise _dist_depot
      float distAvant = lireDistance(trigPinUS[1], echoPinUS[1]);
      static int compteur_depot = 0;
      PC_SERIAL.print("Av: "); PC_SERIAL.print(distAvant, 1);
      PC_SERIAL.print(" | Cible: "); PC_SERIAL.println(_dist_depot, 1);
      if (distanceValide(distAvant) && distAvant <= _dist_depot) {
        compteur_depot++;
      } else {
        compteur_depot = 0;
      }
      if (compteur_depot >= 3) {
        compteur_depot = 0;
        stopMotors();
        PC_SERIAL.println("Position depot atteinte.");
        etape = DEPOSER_TOTEM;
      } else {
        avancerSuiviMur();
      }
      delay(40);
      break;
    }

    case DEPOSER_TOTEM:
      PC_SERIAL.println("=== Etape 13 : depot totem ===");
      baisserBras();
      controlePince(true);
      reculerDistancePrecise(DIST_RECUL_DEPOT);
      leverBras();
      PC_SERIAL.println("Mission accomplie !");
      etape = FIN_PROGRAMME;
      break;

    case FIN_PROGRAMME:
      PC_SERIAL.println("=== FIN ===");
      while (true) {
        stopMotors();
        delay(1000);
      }
      break;
  }
}