/*******************************************************************************
* Uses code lines copied from example "velocity_mode" of the library Dynamixel2Arduino
* Copyright 2016 ROBOTIS CO., LTD.
* Licensed under the Apache License, Version 2.0 (the "License");
* Adapted in Feb 2026 By G. Morel for the POLYTECH ROB3 PROJECT
* In this example, you have code examples to control a KTECH motor, a DYNAMIXEL motor
* (model compatible with protocol 1.0 such as MX-28), and a servo motot controlling a gripper
*******************************************************************************/
#include <math.h>




// Libraries for KT motors
// Arduino library for communacting with SPI devices
#include <SPI.h>
// CAN communications
#include <mcp2515_can.h>
const int SPI_CS_PIN = 9;
mcp2515_can CAN(SPI_CS_PIN);  // Creates the mcp2515_can class object for CAN communication, while setting CS pin
// Homemade KTmotor interface
#include "Ktech_motor.h"
// Declaration of the two motors with their ID
#define NUMBER_OF_MOTORS 2
Motor KTMOTORS[NUMBER_OF_MOTORS] = { Motor(1), Motor(3) };  // 1 and 2 are the motor IDs for CAN protocol 



// Libraries for DXL motor
#include <Dynamixel2Arduino.h>
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8);  // PIN NUMBER FOR RX/TX UART COMMUNICATION WITH YOUR PC
#define PC_SERIAL soft_serial
#define DXL_SERIAL Serial
const int DXL_DIR_PIN = 2;  // DYNAMIXEL Shield DIR PIN
const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 1.0;
// creates dxl as the dynamixel object
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
//This namespace is required to use Control table item names
using namespace ControlTableItem;




// Gripper PIN
#define GRIPPER_PIN 6
#define GRIPPER_LIFT_PIN 10

#include <Servo.h>
Servo gripper;      // servo fermeture/ouverture de la pince
Servo gripperLift;


// Constant definitions
#if !defined(FALSE)
#define FALSE 0
#define TRUE 1
#endif
#define MY_PI 3.14159265359




#define SEND_A_POSITION_TO_DXL TRUE
#define SEND_A_VELOCITY_TO_KTM TRUE
#define MEASURE_THE_US_SENSORS TRUE
#define MOVE_THE_GRIPPER TRUE
#define PRINTING_ON TRUE




// Global variables
// Variables for US sensors
#define NUMBER_OF_US_SENSORS 2
const int trigPinUS[NUMBER_OF_US_SENSORS] = { 22, 42 };
const int echoPinUS[NUMBER_OF_US_SENSORS] = { 23, 43 };
float durationUS[NUMBER_OF_US_SENSORS] = { 0.0, 0.0 };
float distanceUS[NUMBER_OF_US_SENSORS] = { 0.0, 0.0 };
int etat = 0;
float vit_mot = 80;
float vit_mot_tourne = 50;
float pos_deg_D_init = -1.0;
float pos_deg_G_init = -1.0;
int comp = 0;
float motorVelocityCommandInDegPerSec = 0.0;
float long_pincecapt=6;
float dist_a1 = 0;
float a0 = 0.0;
float corr_max = 10.0;

// ============================================================
// SECTION 6 — PARAMÈTRES PHYSIQUES DU ROBOT
// ============================================================

float WHEEL_BASE = 0.265;

float BASE_VEL = 50.0;
float SLOW_VEL = 25.0;

// ============================================================
// SECTION 7 — SEUILS DES CAPTEURS ULTRASON
// ============================================================

float US_COLLISION_STOP  = 10.0;
float US_COLLISION_SLOW  = 25.0;
float US_OBJECT_DETECT   = 18.0;
float US_OBJECT_APPROACH = 15.0;
float US_OBJECT_FINAL    = 15.0;
float K = 0.14;

//Bouton Anass
const int boutonPin = 2;
int ebouton = 0;
//fin Anass











// Clock
unsigned long currentTimeInMicros = 0;
float currentTimeInS = 0.0;




float measureUS(int id) {
  digitalWrite(trigPinUS[id], LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinUS[id], HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinUS[id], LOW);
  float d = pulseIn(echoPinUS[id], HIGH);
  return (d * 0.0343) / 2.0;
}

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

// === AJOUT : durée rotation 90° ===
unsigned long ROTATE_90_TIME_MS = 9250;  
// = 600 ms si ROTATE_180_TIME_MS = 1200


// === AJOUT : rotation 90° droite ===
void rotate90Right() {
  if (!SEND_A_VELOCITY_TO_KTM) return;
  long int v = (long int)(BASE_VEL);
  KTMOTORS[0].sendVelocityCommand( v, CAN);
  KTMOTORS[1].sendVelocityCommand(v, CAN);
  delay(ROTATE_90_TIME_MS);
  stopMotors();
}

// === AJOUT : rotation 90° gauche ===
void rotate90Left() {
  if (!SEND_A_VELOCITY_TO_KTM) return;
  long int v = (long int)(BASE_VEL);
  KTMOTORS[0].sendVelocityCommand(-v, CAN);
  KTMOTORS[1].sendVelocityCommand( -v, CAN);
  delay(ROTATE_90_TIME_MS);
  stopMotors();
}


void moveForward(float vel) {
  if (SEND_A_VELOCITY_TO_KTM) {
    long int safeVel =  (long int)(vel); //collisionSafeVelocity(vel);
    KTMOTORS[0].sendVelocityCommand((long int)(safeVel), CAN);
    KTMOTORS[1].sendVelocityCommand((long int)(-safeVel), CAN);
  }
}


void suivi(float dist_mur){
  float dc = measureUS(0);
  float erreur= dc - dist_mur;
  float tau=0.5;

  float motG = BASE_VEL;
  float motD= BASE_VEL;

  if (fabs(erreur)> tau){
    float C= erreur * K;
    if (C > corr_max) C= corr_max;
    if (C < -corr_max) C= -corr_max;

    motG = BASE_VEL + C;
    motD = BASE_VEL - C;
  }

  if (dc > 40 || dc < 20) {
    motG = BASE_VEL;
    motD = BASE_VEL;
}

  KTMOTORS[0].sendVelocityCommand((long int)(motG), CAN);
 delay(2);
 KTMOTORS[1].sendVelocityCommand(-(long int)motD, CAN);
  delay(10);
}




void lowerArm() {
  if (MOVE_THE_GRIPPER) dxl.setGoalPosition(DXL_ID, 145, UNIT_DEGREE);    // ⚠️ AJUSTER SI BESOIN : angle bras abaissé
}

void raiseArm() {
  if (MOVE_THE_GRIPPER) dxl.setGoalPosition(DXL_ID, 200, UNIT_DEGREE);   // ⚠️ AJUSTER SI BESOIN : angle bras relevé
}

void openGripper() {
  if (MOVE_THE_GRIPPER) gripper.write(2);        // ⚠️ AJUSTER SI BESOIN
}

void closeGripper() {
  if (MOVE_THE_GRIPPER) gripper.write(90);      // ⚠️ AJUSTER SI BESOIN
}








void setup() {
 int i;
 /*
 //bouton
 pinMode(boutonPin, INPUT_PULLUP);
 */
 // Setup digital I/O for US sensor
 for (i = 0; i < NUMBER_OF_US_SENSORS; i++) {
   pinMode(trigPinUS[i], OUTPUT);
   pinMode(echoPinUS[i], INPUT);
 }

  gripper.attach(GRIPPER_PIN);
  gripperLift.attach(GRIPPER_LIFT_PIN);
  openGripper();
  lowerArm();


 // Use UART port of DYNAMIXEL Shield to debug.
 PC_SERIAL.begin(9600);




 // Initialization of the CAN communication. THis will wait until the motor is powered on
 while (CAN_OK != CAN.begin(CAN_500KBPS)) {
   PC_SERIAL.println("CAN init fail, retry ...");
   delay(500);
 }




 // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
 dxl.begin(1000000);
 // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
 dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
 // Get DYNAMIXEL information
 dxl.ping(DXL_ID);
 // Turn off torque when configuring items in EEPROM area
 dxl.torqueOff(DXL_ID);
 dxl.setOperatingMode(DXL_ID, OP_POSITION);
 dxl.torqueOn(DXL_ID);
 // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
 dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 5);




 // Initialization of the motor command and the position measurement variables
 KTMOTORS[0].prevPosInDeg = 0.0;
 KTMOTORS[0].revolNumber = 0;
 KTMOTORS[0].offsetEncoder = 0;
 // Send motor off then motor on command to reset
 KTMOTORS[0].OFF(CAN);

 delay(10);



 KTMOTORS[0].ON(CAN);
 delay(10);
 KTMOTORS[0].sendVelocityCommand((long int)(0), CAN);
  delay(10);
 KTMOTORS[0].revolNumber = 0;
 KTMOTORS[0].prevPosInDeg = 0.0;




 // Initialization of the motor command and the position measurement variables
 KTMOTORS[1].prevPosInDeg = 0.0;
 KTMOTORS[1].revolNumber = 0;
 KTMOTORS[1].offsetEncoder = 0;
 // Send motor off then motor on command to reset
 KTMOTORS[1].OFF(CAN);
  delay(10);

 KTMOTORS[1].ON(CAN);
  delay(10);

 KTMOTORS[1].sendVelocityCommand((long int)(0), CAN);
  delay(10);

 KTMOTORS[1].revolNumber = 0;
 KTMOTORS[1].prevPosInDeg = 0.0;




 // Initialize the gripper to output
 //pinMode(GRIPPER_PIN, OUTPUT);
 //analogWrite(GRIPPER_PIN, 50);








 PC_SERIAL.println("");
 PC_SERIAL.println("");
 PC_SERIAL.println("************************************************************");
 PC_SERIAL.println(" LE SYSTEME EST PRET A FONCTIONNER. SIGNAL DE COMMANDE = 0 *");
 PC_SERIAL.println("************************************************************");
 PC_SERIAL.println("");

 delay(1000);
}

typedef enum {
  TEST_MODE,
  WAIT_HAND,
  ATT,
  AVANCE,
  TURN_RIGHT_90,            // AJOUT
  ADVANCE_TO_WALL,          // AJOUT
  TURN_LEFT_90_AFTER_WALL,  // AJOUT
  SEARCH_OBJECT,
  APPROACH_OBJECT,
  GRAB_OBJECT,
  TURN_LEFT_90_AFTER_GRAB,  // AJOUT
  GO_TO_SYM_LINE,
  TURN_LEFT_90_FINAL,       // AJOUT
  AVANCE_FINAL,
  RELEASE_OBJECT,
  MISSION_END,
  AVANCE_MUR20
} RobotState;



RobotState state = AVANCE;
unsigned long stateStartTime = 0;
unsigned long lastSearchObjectTime = 0;

int a=0;


void loop() {
  /*//Bouton Anass
  ebouton=digitalRead(boutonPin);
  while (a==0){
    if(ebouton==LOW ){a=1;}
    //attend le bouton;
  }
  //fin Anass
  */
  int i;

  currentTimeInMicros = micros();
  currentTimeInS = 0.000001 * ((float)currentTimeInMicros);
  /*
  if (MEASURE_THE_US_SENSORS && state != WAIT_HAND) {
    float dFront = measureUS(0);
    float dBack  = measureUS(1);
    if (dFront < US_COLLISION_STOP || dBack < US_COLLISION_STOP) {
      stopMotors();
      if (PRINTING_ON) PC_SERIAL.println("!!! COLLISION EVITEE (STOP GLOBAL) !!!");
    }
  }*/

  switch (state) {
    /*case ATT:
      if (digitalRead(boutonPin)==LOW){
        state = AVANCE;
      }
      break;*/

    case TEST_MODE:
      moveForward(BASE_VEL);
      delay(3000);
      rotate90Right();
      rotate90Left();
      break;
      


    
    /*case WAIT_HAND: {
      static int confirmCount = 0;
      float dF = measureUS(1);
      if (PRINTING_ON) { PC_SERIAL.print("WAIT_HAND - US avant : "); PC_SERIAL.println(dF); }
      if (dF < 8.0 && dF > 0.5) {
        confirmCount++;
        if (confirmCount >= 3) {
          confirmCount = 0;
          PC_SERIAL.println("MAIN DETECTEE — MISSION LANCEE !");
          state = AVANCE;
          stateStartTime = millis();
        }
      } else {
        confirmCount = 0;
      }
      break;
    }*/


    case AVANCE:
      delay(3000);
      raiseArm();
      openGripper();
      delay(2000);
      moveForward(BASE_VEL);
      delay(2000);
      stopMotors();
      state = TURN_RIGHT_90;
      stateStartTime = millis();
      break;

    case TURN_RIGHT_90:
      rotate90Right();
      state = ADVANCE_TO_WALL;
      stateStartTime = millis();
      break;

    case ADVANCE_TO_WALL:
      moveForward(BASE_VEL);
      if (MEASURE_THE_US_SENSORS) {
        float dF = measureUS(1);
        if (PRINTING_ON) { PC_SERIAL.print("ADVANCE_TO_WALL - US avant : "); PC_SERIAL.println(dF); }
        if (dF < 30.0) {
          stopMotors();
          state = TURN_LEFT_90_AFTER_WALL;
          stateStartTime = millis();
        }
      }
      break;

    case TURN_LEFT_90_AFTER_WALL:
      rotate90Left();
      state = SEARCH_OBJECT;
      stateStartTime = millis();
      break;


    case SEARCH_OBJECT: {
    // Timer : temps écoulé depuis l'entrée dans l'état
    unsigned long elapsedTime = millis() - stateStartTime;

    suivi(22);
    //lowerArm();

    if (MEASURE_THE_US_SENSORS) {
        float dF = measureUS(1);
        if (PRINTING_ON) { 
            PC_SERIAL.print("SEARCH_OBJECT - US avant : "); 
            PC_SERIAL.println(dF); 
        }

        if (dF < US_OBJECT_DETECT) {
            stopMotors();

            // <<< STOCKAGE DU TEMPS PASSÉ DANS L'ÉTAT
            lastSearchObjectTime = elapsedTime;

            state = GRAB_OBJECT;
            stateStartTime = millis();
        }
    }

    break;
    }


    

    case GRAB_OBJECT: {
      delay(500);
      lowerArm();
      delay(500);
      moveForward(BASE_VEL);
      delay(3000);
      stopMotors();
      delay(500);
      //delay(400);              // ⚠️ AJUSTER SI BESOIN : attendre que le bras soit en position
      closeGripper();          // 1ère fermeture
      delay(1000);              // ⚠️ AJUSTER SI BESOIN
      //moveForward(-10);        // micro-recul pour caler l'objet
      //delay(150);              // ⚠️ AJUSTER SI BESOIN
      //stopMotors();
      //closeGripper();          // 2ème fermeture (plus ferme)
      //delay(300);              // ⚠️ AJUSTER SI BESOIN
      raiseArm();              // relever le bras avec l'objet saisi
      delay(1000);              // ⚠️ AJUSTER SI BESOIN : attendre que le bras soit relevé
      PC_SERIAL.println("OBJET SAISI ET BRAS RELEVE");
      moveForward(BASE_VEL);
      state = AVANCE_MUR20;
      stateStartTime = millis();
      break;
    }

    case AVANCE_MUR20:
      moveForward(BASE_VEL);
      if (MEASURE_THE_US_SENSORS) {
        float dF = measureUS(1);
        if (dF < 20.0) {
          stopMotors();
          state = TURN_LEFT_90_AFTER_GRAB;
          stateStartTime = millis();
        }
      }
      stateStartTime = millis();
      break;


    case TURN_LEFT_90_AFTER_GRAB:
      rotate90Left();
      state = GO_TO_SYM_LINE;
      stateStartTime = millis();
      break;

    case GO_TO_SYM_LINE:
      moveForward(BASE_VEL);
      if (MEASURE_THE_US_SENSORS) {
        float dB = measureUS(1);
        if (PRINTING_ON) { PC_SERIAL.print("GO_TO_SYM_LINE - US arrière : "); PC_SERIAL.println(dB); }
        if (dB < 30.0) {
          stopMotors();
          state = TURN_LEFT_90_FINAL;
          stateStartTime = millis();
        }
      }
      break;

    case TURN_LEFT_90_FINAL:
      rotate90Left();                 // rotation finale
      //moveForward(BASE_VEL);          // AJOUT : avancer après la rotation
      delay(300);                     // ⚠️ AJUSTER SI BESOIN
      stopMotors();
      state = AVANCE_FINAL;
      stateStartTime = millis();
      break;

    case AVANCE_FINAL: {
      unsigned long elapsed = millis() - stateStartTime;
      if (elapsed < lastSearchObjectTime) {
        suivi(22);
      } else {
        stopMotors();
        delay(500);
        state = RELEASE_OBJECT;
        stateStartTime = millis();
      }
      break;
    }



    case RELEASE_OBJECT:
      lowerArm();    // abaisser le bras avant de relâcher l'objet
      delay(500);    // ⚠️ AJUSTER SI BESOIN : attendre que le bras soit en position
      openGripper(); // relâcher l'objet
      delay(500);    // ⚠️ AJUSTER SI BESOIN
      moveForward(-BASE_VEL);
      state = MISSION_END;
      stateStartTime = millis();
      break;

    /*case BACK_HOME:
      moveForward(-BASE_VEL);
      if (MEASURE_THE_US_SENSORS) {
        float dB = measureUS(1);
        if (PRINTING_ON) { PC_SERIAL.print("BACK_HOME - US arrière : "); PC_SERIAL.println(dB); }
        if (dB < 15.0) {
          stopMotors();
          state = MISSION_END;
          stateStartTime = millis();
          PC_SERIAL.println("MISSION TERMINEE");
        }
      }
      break;*/

    case MISSION_END:
      delay(2000);
      stopMotors();
      break;
  }
}