/*****************************************************************************/
/**************** AUTOMATIC CONTROL - POLYTECH SORBONE - JAN 2025 ************/
/*********************** AUTHORS : L. BRIETZKE & G. MOREL ********************/
/************************* Sequence 1 - Problem 1 ****************************/
/**************************** YOU'RE IN CONTROL *******************************/
/*****************************************************************************/

/**
 * @file robot_control.ino
 * @brief Automatic robot control sequence for obstacle pickup and drop.
 */

// Libraries for CAN communications
#include <can-serial.h>
#include <mcp2515_can.h>
#include <mcp2515_can_dfs.h>
#include <mcp_can.h>
#if defined(SEEED_WIO_TERMINAL) && defined(CAN_2518FD)
const int SPI_CS_PIN = BCM8;
const int CAN_INT_PIN = BCM25;
#else
const int SPI_CS_PIN = 10;
const int CAN_INT_PIN = 2;

#endif

#include <Servo.h>
Servo monservo;


#include "mcp2515_can.h"
mcp2515_can CAN(SPI_CS_PIN);  // Set CS pin
#define MAX_DATA_SIZE 8

#include <SPI.h>
#include <math.h>

#define MY_PI 3.14159265359

#define PERIOD_IN_MICROS 5000 // 5 ms

#if !defined(FALSE)
#define FALSE 0
#define TRUE 1
#endif


#define MOTOR_MAX_VEL_CMD 300000 
#define MOTOR_MAX_VOLTAGE_CMD 200 


// Capteurs ultrasons (2: capteur avant)
const int trig_pin = 6;			
const int echo_pin = 7;
const int trig_pin_2 = 4;
const int echo_pin_2= 5;
float timingD = 0.0;
float timingP = 0;

// Enums de mouvements et d'étapes
enum Mouvement { ARRETER, AvanceAvecUneDistanceCstAuMur, tournerG, RecupObs, PoseObs, Detectobs, reculer };
enum Etape { etap0, etap1, etap2, etap3, etap4, etap5, etap6, etap7, etap8, etap9, etap10, etap11, etap12, etap13, etap14, etap15, etap16, etap17, etap18, etap19, etap20, etap21, etap22, etap23, etap24, etap25, etap26 };
Mouvement Mouv = ARRETER;
int MesureDistanceD = 20; 				// valeur du capteur droit 
int DistanceD = MesureDistanceD;
int Condition = etap0;
int MesureDistanceP = 0;


/******************  GLOBAL VARIABLES *********************/
// Global motor state variables
double currentMotorPosDeg[3];
double currentMotorVelDegPerSec[3];
double previousMotorPosDeg[3];
int currentMotorPosEncoder[3];
int offsetMotorPosEncoder[3];
int currentNumOfMotorRevol[3];

// Variables globales
double motor1VelocityCommandInDegPerSec;        // commande en vitesse moteur1
double motor2VelocityCommandInDegPerSec;        // commande en vitesse moteur2
double Consignevitesse;                         // change la vitesse du ro sur tout le circuit


// General purpose global variables
int counterForPrinting;
int printingPeriodicity;
unsigned long current_time, old_time, initial_time;

/**
 * @brief Allume le moteur.
 * @param motor_ID ID du moteur
 */
void motorON(int motor_ID){
  unsigned char msg[MAX_DATA_SIZE] = {
    0x88,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00
  };

  CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); 

}

/**
 * @brief Éteint le moteur.
 * @param motor_ID ID du moteur
 */
void motorOFF(int motor_ID){

  unsigned char msg[MAX_DATA_SIZE] = {
    0x80,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00
  };
  Serial.print("Motor ID: ");
  Serial.println(motor_ID);
  CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); 



}

/**
 * @brief Envoie une commande de vitesse.
 * @param vel Vitesse en centièmes de degré/s
 * @param motor_ID ID du moteur
 */
void sendVelocityCommand(long int vel, int motor_ID) {

  long int local_velocity;
  local_velocity = vel;

  unsigned char *adresse_low = (unsigned char *)(&local_velocity);

  unsigned char msg[MAX_DATA_SIZE] = {
    0xA2,
    0x00,
    0x00,
    0x00,
    *(adresse_low),
    *(adresse_low + 1),
    *(adresse_low + 2),
    *(adresse_low + 3)

  };

  CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); 
}


/**
 * @brief Lit l'état du moteur et met à jour les variables globales.
 * @param motor_ID ID du moteur
 */
void readMotorState(int motor_ID)  {
  uint32_t id;
  uint8_t type;
  uint8_t len;
  byte cdata[MAX_DATA_SIZE] = { 0 };
  int data2_3, data4_5, data6_7;
  int currentMotorVelRaw;
  int i;

  
  // wait for data
  while (CAN_MSGAVAIL != CAN.checkReceive());


  // read data, len: data length, buf: data buf
  CAN.readMsgBuf(&len, cdata);


  id = CAN.getCanId();
  type = (CAN.isExtendedFrame() << 0) | (CAN.isRemoteRequest() << 1);

  // Check if the received ID matches the motor ID
  if ((id - 0x140) == motor_ID) { 
    data4_5 = cdata[4] + pow(2, 8) * cdata[5];
    currentMotorVelRaw = (int)data4_5;
    data6_7 = cdata[6] + pow(2, 8) * cdata[7];
    currentMotorPosEncoder[i] = (int)data6_7;
  }

  // Conversion of the velocity and writing in the global cariable
  currentMotorVelDegPerSec[i] = ((double)(currentMotorVelRaw)); 




  // Conversion of the position (with motor revolution counting) and wirting in the global variable
  int relativeEncoder = currentMotorPosEncoder[i] - offsetMotorPosEncoder[i];
  currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])*360.0) + (((double)relativeEncoder) * 180.0 / 32768.0);  // On convertit en degré

  if ((currentMotorPosDeg[i] - previousMotorPosDeg[i]) < -20.0) {
    currentNumOfMotorRevol[i]++;
    currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])) * 360.0 + ((double)relativeEncoder) * 180.0 / 32768.0;
  }
  if ((currentMotorPosDeg[i] - previousMotorPosDeg[i]) > 20.0) {
    currentNumOfMotorRevol[i]--;
    currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])) * 360.0 + ((double)relativeEncoder) * 180.0 / 32768.0;
  }

  previousMotorPosDeg[i] = currentMotorPosDeg[i]; // writing in the global variable for next call
}


/**
 * @brief Fonction d'initialisation.
 */
void setup() {
  int i;
  char serialReceivedChar;
  int nothingReceived;



  // Initialization of the serial link
  Serial.begin(115200);

  // Initialization of the analog input pins

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




  Serial.println("");
  Serial.println("");
  Serial.println("");
  Serial.println("");
  Serial.println("");
  Serial.println("***********************************************************************");
  Serial.println("***********************************************************************");
  Serial.println("***********************************************************************");
  Serial.println(" Serial link and CAN initialization went ok! Power ON the motor");
  Serial.println("***********************************************************************");
  Serial.println("***********************************************************************");
  Serial.println("***********************************************************************");
  Serial.println("***********************************************************************");
  Serial.println("");


  // Initialization of the motor command and the position measurement variables
  for (int i = 0; i < 3; i++) {
    previousMotorPosDeg[i] = 0.0;
    currentNumOfMotorRevol[i] = 0;
    offsetMotorPosEncoder[i] = 0;

  }


  // Send motot off then motor on command to reset

  motorOFF(1);
  delay(500);
  readMotorState(1);
  motorOFF(2);
  delay(500);
  readMotorState(2);
  motorOFF(3);
  delay(500);
  readMotorState(3);


  Serial.println("");
  Serial.println("***********************************************************************");
  Serial.println(" Turn the rotor in its ZERO position they type 'S'");
  Serial.println("***********************************************************************");

  nothingReceived = TRUE;
  while (nothingReceived==TRUE){
    serialReceivedChar = Serial.read();
    if(serialReceivedChar == 'S') {
      nothingReceived = FALSE;
    }
  }



  monservo.attach(3);


  motorON(1);
  readMotorState(1);
  delay(500);
  motorON(2);
  readMotorState(2);
  delay(500);
  motorON(3);
  readMotorState(3);
  delay(500);
  
  
  sendVelocityCommand((long int)(0),1);
  //delay(500);
  readMotorState(1); 
  sendVelocityCommand((long int)(0),2);
  //delay(500);
  readMotorState(2); 
  sendVelocityCommand((long int)(0),3);
  //delay(500);
  readMotorState(3); 

  pinMode(echo_pin, INPUT);		// Initialisation des capteurs Devant et Pince
  pinMode(trig_pin, OUTPUT);
  digitalWrite(trig_pin, LOW);

  pinMode(echo_pin_2, INPUT);
  pinMode(trig_pin_2, OUTPUT);
  digitalWrite(trig_pin_2, LOW);





  Serial.println("End of Initialization routine.");
  counterForPrinting = 0;
  printingPeriodicity = 10; // The variables will be sent to the serial link one out of printingPeriodicity loop runs.  
  current_time = micros(); 
  initial_time=current_time;

}



/**
 * @brief Boucle principale du robot.
 */
void loop() {
  delay(500);
  int i;
  //capteurs
  digitalWrite(trig_pin, LOW);
  delay(2);
  digitalWrite(trig_pin, HIGH);
  delay(10);
  digitalWrite(trig_pin, LOW);
  timingD = pulseIn(echo_pin, HIGH);
  digitalWrite(trig_pin_2, LOW);
  delay(2);
  digitalWrite(trig_pin_2, HIGH);
  delay(10);
  digitalWrite(trig_pin_2, LOW);
  timingP = pulseIn(echo_pin_2, HIGH);

  //CALCUL DISTANCES
  MesureDistanceD = (timingD * 0.034) / 2;
  MesureDistanceP = (timingP * 0.034) / 2;
  

  // Mesure distanceD
  if (MesureDistanceD > 300){
    MesureDistanceD = 300;
  }
   if (MesureDistanceP > 300){
    MesureDistanceP = 300;
  }

  Serial.print("MesureDistanceD  ");
  Serial.println(MesureDistanceD);

  Serial.print("MesureDistanceP  ");
  Serial.println(MesureDistanceP);

  Serial.print("DistanceD  ");
  Serial.println(DistanceD);
  // Variable modifiable

  int ConsigneVitesse = 6000;
    //distance avec l'obstacle au moment se s'arreter pour le prendre    à tester

  // Erreur
    float Erreur = DistanceD - MesureDistanceD ;
    /*if (Erreur > 10 || Erreur < -10 ){
      Erreur = 1;
      Serial.println("ERREUR ABERRANTE");
    }*/


  switch (Mouv) {

    case AvanceAvecUneDistanceCstAuMur:
      motor1VelocityCommandInDegPerSec = ConsigneVitesse ;
      motor2VelocityCommandInDegPerSec = -ConsigneVitesse ;

      sendVelocityCommand(motor1VelocityCommandInDegPerSec, 1);
      readMotorState(1); 
      sendVelocityCommand(motor2VelocityCommandInDegPerSec, 2);
      readMotorState(2);
      break;

    case reculer:
      motor1VelocityCommandInDegPerSec = -ConsigneVitesse ;
      motor2VelocityCommandInDegPerSec = ConsigneVitesse ;

      sendVelocityCommand(motor1VelocityCommandInDegPerSec, 1);
      readMotorState(1); 
      sendVelocityCommand(motor2VelocityCommandInDegPerSec, 2);
      readMotorState(2);
      break;


    case tournerG:          
      sendVelocityCommand(-3000,1);
      readMotorState(1);
      sendVelocityCommand(-3000,2);
      readMotorState(2);
      delay(12700);   // temps necessaire pour tourner de 90 degrés
      break;	

    case RecupObs:                                                   // permet de récuperer l'obstacle							//attendre que la pince se place
      monservo.write(0);
      sendVelocityCommand(4000, 3);     // placement a la bonne hauteur
      delay(200);
      readMotorState(3);
	    delay(2000);							// temps a travailler pour que le pince soit bien placer

      // Avancer 
      sendVelocityCommand(2000, 1);
      delay(20);
      readMotorState(1);
	    sendVelocityCommand(-2000, 2);
      	delay(20);
     	readMotorState(2);
	    delay(7000);
						// attendre que le robot soit placer deva,t la pince
      monservo.write(84); 					// attraper l'objet en avancer doucement
      delay(100);

	    sendVelocityCommand(-3000 , 3);						
	    delay(20);
      readMotorState(3);

      break;


    case Detectobs:
      sendVelocityCommand(-1000 , 3);						// 
	    delay(200);
      readMotorState(3);
      break;


    case PoseObs:
      sendVelocityCommand(5000 , 3);						// redescender la pince
      delay(20);
      readMotorState(3);
      delay(3000);					// arret la pince en position basse
      monservo.write(0); 					// lacher l'objet en avancer doucement
      break;
        

    case ARRETER:
      sendVelocityCommand(0, 1);
      delay(200);
      readMotorState(1);
      sendVelocityCommand(0, 2);
      delay(200);
      readMotorState(2);                                                          
      break;
  }

Serial.print("Condition : ");
Serial.println(Condition);
// Condition

  switch (Condition){

    case etap0:                        // On attent un peu
      Mouv=ARRETER;
      Condition = etap1;
      break;

    case etap1:                        // On récupère la distance avec le mur
      DistanceD = MesureDistanceD;
      Condition = etap2;
      break;

    case etap2:                        // On avance avec un distance cst avec le mur jusqu'à se rapprocher du mur
       Mouv = AvanceAvecUneDistanceCstAuMur;
       if (MesureDistanceP < 15){
        Condition = etap3;
       } 
      break;

    case etap3:                       // On tourne à gauche      
      delay(500);            
      Mouv = tournerG;
      Condition = etap4;
      break;

    case etap4:                        // On attent un peu
      Mouv=ARRETER;
      Condition = etap5;
      break;
    
    case etap5:                        // On récupère la distance avec le mur
      DistanceD = MesureDistanceD;
      Condition = etap6;
      break;
    
    case etap6:                        // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle)
       Mouv = AvanceAvecUneDistanceCstAuMur;
       if (Erreur > 2 || Erreur < -2){
        Condition = etap7;
       }
      break;
    
    case etap7:                        // On tourne à gauche
      delay(3300); 
      Mouv = tournerG;
      Condition = etap8;
      break;
    
    case etap8:                        // On attent un peu
      Mouv=ARRETER;
      Condition = etap9;
      break;
    case etap9:                        // On récupère la distance avec le mur
      DistanceD = MesureDistanceD;
      Condition = etap10;
      break;
    
    case etap10:                        // On avance avec un distance cst avec le mur jusqu'à se rapprocher du mur
      Mouv = AvanceAvecUneDistanceCstAuMur;
      if (MesureDistanceP < 13 ){
	      Mouv = ARRETER;		
        Condition = etap11;
       }
      break;
    
    case etap11:                         // on a est devant l'obj mtn, on le recup	
      Mouv = Detectobs;
      if (MesureDistanceP > 18){         // ajuster la valeur de détection pour passer à l'étape de récuperation de l'obj
        Condition = etap12;
       }
      break;

    case etap12:                        // On récupère l'obj
      Mouv = RecupObs;
      Condition = etap13;
      break;

    
    case etap13:                        // On tourne à gauche 
      Mouv = tournerG;
      Condition = etap14;
      break;
    
    case etap14:                        // On attent un peu
      Mouv=ARRETER;
      Condition = etap15;
    break;

    case etap15:                        // On récupère la distance avec le mur
      delay(1500);
      DistanceD = MesureDistanceD;
      Condition = etap16;
    break;
    
    case etap16:                        // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle)
      Mouv = AvanceAvecUneDistanceCstAuMur;
      if (Erreur > 1 || Erreur < -1 ){
        Condition = etap17;
      }
      break;
    
    case etap17:                        // On tourne à gauche  
      delay(2800);              
      Mouv = tournerG;
      Condition = etap18;
      break;

    case etap18:                        // On attent un peu
      Mouv=ARRETER;
      Condition = etap19;
    break;

    case etap19:                        // On récupère la distance avec le mur
      DistanceD = MesureDistanceD;
      Condition = etap20;
    break;

    case etap20:                        // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle)                
      Mouv = AvanceAvecUneDistanceCstAuMur;
      if (Erreur > 2 || Erreur < -2){                  // On est au niveau de là où on doit poser l'obj
        Condition = etap21;
      }
      break;
    
    case etap21:                        // On attent un peu              
      Mouv = ARRETER;
      Condition = etap22;
      break;
    
    case etap22:
      Mouv = reculer;
      Condition =etap23;
      break;

    case etap23:                        // On attent un peu      
      delay(10000);         
      Mouv = ARRETER;
      Condition = etap24;
      break;
    
    case etap24:                        // On pose l'obj            
      Mouv = PoseObs;
      Condition = etap25;
      break;
    
    case etap25:
      Mouv = reculer;
      Condition = etap26;
      break;

    case etap26:                        // On attent un peu pour montrer qu'on est content     
      delay(5000);        
      Mouv = ARRETER;
      break;
  }
}