// Libraries for CAN communications
#include <can-serial.h>
#include <mcp2515_can.h>
#include <mcp2515_can_dfs.h>
#include <mcp_can.h>
#include <Servo.h>
#include <math.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 = 9;
const int CAN_INT_PIN = 2;
#endif

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

#include <SPI.h>

#define MY_PI 3.14

#define PERIOD_IN_MICROS 10000 

#define VEL_MAX 20000

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

  // Here change with the motor ID if it has been changed with the firmware.
#define MOTOR_MAX_VEL_CMD 300000 
#define MOTOR_MAX_VOLTAGE_CMD 200 
#define MOTOR_MAX_POS_DEG 120.0
#define MOTOR_MIN_POS_DEG -120.0

Servo myservo;

/******************  GLOBAL VARIABLES *********************/
// Global motor state variables
int trigPin_f = 4; 
int echoPin_f = 3; 

int trigPin_r = 6; 
int echoPin_r = 5; 

int broche_pince = 13;


int begin_push = A0;
int g_int = A1;

int in_pince = A3;

long duration; 

double distance_f; 
double distance_r;
double last_distance; 
double currentMotorPosDeg[3];
double currentMotorVelDegPerSec[3];
double last_currentMotorPosDeg[3];
double previousMotorPosDeg[3];
double currentMotorPosEncoder[3];
double offsetMotorPosEnconder[3];
double currentNumOfMotorRevol[3];

double x = 0;
double x_arrivee=0;
double y = 0;
double zone_a[2] = {-1,-1};
double obj[2] = {-1, -1}; 
double teta = 0;
double d_teta = 0;
double rayon = 2.480;
double dist_roue = 9.0;
double erreur_teta = 0;
double teta_cible = 0.0;
double vitesse = 0;
double v_omega = 0;
int GI = 0;
int pos = 0;

int cas = 1;
int test = 0;
int test2= 0;
int test3 = 0; //ces variables test nous servent à ne passer qu'un nombre choisi de fois dans les if de chaque étape/cas 
int test4 = 0;
int test5 = 0;
int test6 = 0;
int compteur_balise = 0; //ce compteur sert à éviter le bruit lié au bugg de transmission moteur-CAN à partir de la fin de la 3eme rotation : il faut que le capteur détecte plusieurs fois la variation de distance pour qu'on considère détecter la balise 
int compteur_recul = 0; //ce compteur nous sert à reculer suffisemment longtemps après avoir reposé le totem pour nous arreter ensuite : pas très propre mais rapide à coder
double dist_rep = 0;
double dist_rep_f = 0; //distances repères f pour front 

int x_rep = 0;
int y_rep = 0;

int cpt = 0;
double alpha_prim = 0;

double h = 9.1; 
double dist_roue_pince = 20.36;
double alpha0 = 75;
double consigne = 8.0; // consigne c'est ce qu'on donne au moteur qui gère la rotation du bras 


double x0 = 0;


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

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); 

}

void motorOFF(int motor_ID){

  unsigned char msg[MAX_DATA_SIZE] = {
    0x80,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00
  };

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

}

/*** This function sends a velocity command. Unit = hundredth of degree per second *****/
void sendVelocityCommand(long int vel, int motor_ID) {
  if(vel > VEL_MAX)
    vel = VEL_MAX;
  if(vel < -VEL_MAX){
    vel = -VEL_MAX;
  }
  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); 
}


/*** This function reads the motor states and affects the following global variables *****/
/*     previousMotorPosDeg : previous position in Deg (set to the new value, will be "previous" for next call);
/*     currentNumOfMotorRevol = number of  revoutions (old value +/- 1 if one jump has been observed ont the encoder read)
/*     currentEncodPos = current Encoder Pos value (read)
/*     currentMotorPosDeg = Motor position in degrees
/*     currentMotorVelDegPerSec = Motor position velocity in degrees per s
***********************************************************************/

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;
  double currentMotorVelRaw[3];

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

  unsigned long start = millis();
  while (CAN_MSGAVAIL != CAN.checkReceive()) {
    if (millis() - start > 50) {
        Serial.print("Timeout: Pas de message CAN reçu sur moteur ");
        Serial.println(motor_ID);
        return;
    }
  }


  // 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[motor_ID - 1] = (double)data4_5;
    data6_7 = cdata[6] + pow(2, 8) * cdata[7];
    currentMotorPosEncoder[motor_ID - 1] = (double)data6_7;
  }

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

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

  if ((currentMotorPosDeg[motor_ID - 1] - previousMotorPosDeg[motor_ID - 1]) < -20.0) {
    currentNumOfMotorRevol[motor_ID - 1]++;
    currentMotorPosDeg[motor_ID - 1] = ((double)(currentNumOfMotorRevol[motor_ID - 1])) * 360.0 + ((double)currentMotorPosEncoder[motor_ID - 1]) * 180.0 / 32768.0;
  }
  if ((currentMotorPosDeg[motor_ID - 1] - previousMotorPosDeg[motor_ID - 1]) > 20.0) {
    currentNumOfMotorRevol[motor_ID - 1]--;
    currentMotorPosDeg[motor_ID - 1] = ((double)(currentNumOfMotorRevol[motor_ID - 1])) * 360.0 + ((double)currentMotorPosEncoder[motor_ID - 1]) * 180.0 / 32768.0;
  }
  previousMotorPosDeg[motor_ID - 1] = currentMotorPosDeg[motor_ID - 1]; // writing in the global variable for next call
}


/********************* THIS FUNCTION IS EXECUTED FIRST AND CONTAINS INITIALIZATION ***********/
void setup() {
  myservo.attach(8);
  int i;
  char serialReceivedChar;
  int nothingReceived;
  
  pinMode(trigPin_f, OUTPUT); // Sets the trigPin as an Output 

  pinMode(echoPin_f, INPUT); 

  pinMode(trigPin_r, OUTPUT); // Sets the trigPin as an Output 

  pinMode(echoPin_r, INPUT); 

  pinMode(begin_push, INPUT);

  pinMode(g_int, INPUT);

   

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



  // 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;
    offsetMotorPosEnconder[i] = 0;
    last_currentMotorPosDeg[i] = 0;
  }
  // Send motot off then motor on command to reset
  motorOFF(1);
  readMotorState(1);
  motorOFF(2);
  readMotorState(2);
  Serial.println("ok");
  motorOFF(3);
  readMotorState(3);
  Serial.println("ok");  

 int begin = 0;
  while(begin < 900){
    begin = analogRead(begin_push);
  }
  delay(1000);

  counterForPrinting = 0;
  printingPeriodicity = 30; // The variables will be sent to the serial link one out of printingPeriodicity loop runs.  
  current_time = micros(); 
  initial_time=current_time;
  motorON(1);
  readMotorState(1);
  motorON(2);
  readMotorState(2);
  motorON(3);
  readMotorState(3);


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


  for(int i = 0; i < 3; i++){
    offsetMotorPosEnconder[i] = currentMotorPosEncoder[i];
    currentNumOfMotorRevol[i] = 0;
    previousMotorPosDeg[i] = 0.0;
  }
  sendVelocityCommand((long int)(0),1);
  readMotorState(1); 
  sendVelocityCommand((long int)(0),2);
  readMotorState(2); 
  sendVelocityCommand((long int)(0),3);
  readMotorState(3); 
  delay(500);
 
  

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



void pince(){
  if(pos == 0){
    for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15 ms for the servo to reach the position
    }
    pos = 180;
  }
  else{
    for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
      myservo.write(pos);              // tell servo to go to position in variable 'pos'
      delay(15);                       // waits 15 ms for the servo to reach the position
    }
    pos = 0;
  }
}

void loop() {

  int i;
  unsigned int sleep_time;
  double elapsed_time_in_s;
  int d_capt_roue = 0;

  old_time=current_time;
  current_time=micros();
  elapsed_time_in_s = (double)(current_time-initial_time);
  elapsed_time_in_s *= 0.000001;
  if(v_omega > MY_PI/120){
    v_omega = MY_PI/120;
  }
   if(v_omega <  -MY_PI/120){
    v_omega = -MY_PI/120;
  }
  sendVelocityCommand(-((1/rayon) * vitesse - (dist_roue/2*rayon) * v_omega) * (180/MY_PI) * 100, 1);
  readMotorState(1);
  sendVelocityCommand(((1/rayon) * vitesse + (dist_roue/2*rayon) * v_omega) * (180/MY_PI) * 100, 2);
  readMotorState(2);
  double d_d = (-(currentMotorPosDeg[0] - last_currentMotorPosDeg[0])) * (MY_PI / 180) * rayon; //delta de distance avancée par la roue droite entre deux tours de loop
  double d_g = (currentMotorPosDeg[1] - last_currentMotorPosDeg[1]) * (MY_PI / 180) * rayon;
  d_teta = (d_d - d_g) / dist_roue; //delta d'angle de l'axe "devant" par rapport aux distances parcouru par les roues en 1 tour de loop
  y -= ((d_d + d_g) * cos((MY_PI - d_teta) / 2 - teta)) / 2; //coordonnées sur la map (on commence par avancer selon x voir video)   
  x += ((d_d + d_g) * sin((MY_PI - d_teta) / 2 - teta)) / 2;
  teta += d_teta;
  erreur_teta = teta - teta_cible;
  v_omega =  1.0 *erreur_teta;

  int general_interupt = analogRead(g_int);
  if(general_interupt > 10000)
      GI = 1;
  if(GI == 0){
  digitalWrite(trigPin_f, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin_f, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(trigPin_f, LOW);
  duration = pulseIn(echoPin_f, HIGH); 
  distance_f = (duration * 0.034 / 2) + dist_roue_pince;

  digitalWrite(trigPin_r, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin_r, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(trigPin_r, LOW);
  duration = pulseIn(echoPin_r, HIGH); 
  distance_r = (duration * 0.034 / 2) + 10.079; 

  h =(sin((alpha0 - currentMotorPosDeg[2])*MY_PI/180)*15) -5.39;
  dist_roue_pince = (cos((alpha0 - currentMotorPosDeg[2])*MY_PI/180)*15) + 14.12;
  sendVelocityCommand(-1000 * (consigne - h), 3);
  readMotorState(3);


   for(int i = 0; i < 3; i ++){
    last_currentMotorPosDeg[i] = currentMotorPosDeg[i];
  }

  if(cpt == 0){
    dist_rep = distance_r;
    cpt = 300;
  }
  //================================================================Serial.println(cas);
  switch(cas){
    case(1):
      if(test == 0){
        for (pos = 180; pos >= 0; pos -= 1) { 
          myservo.write(pos);             
          delay(5);                      
        }
          pos = 0;
          x = 150 - (distance_f - d_capt_roue);
          test = 1;
      }
      
      vitesse = 5; //on avance 
      /*Serial.print(" x = "); // verifications des variations de coordonnées 
      Serial.println(x);
      Serial.print(" y = ");
      Serial.println(y);*/
      
      if(test2 == 1 && abs(dist_rep - distance_r) >= 2){ // à lire après le if suivant ! 
        test2 = 0;
        test = 0;
        cas = 2;
        zone_a[0] = (zone_a[0] + x) / 2; 
        dist_rep = distance_r;
        Serial.print("zone_a = ");
        Serial.println(x);
      }
      if(abs(dist_rep - distance_r) > 2){ //detection de balise, dist_rep c'est la valeur de y qu'on a prit au tout début de la ligne droite puis on compare avec les nouvelles
        test2 = 1;
        dist_rep = distance_r;
        zone_a[0] = x; // c'est la valeur de x qu'on va utiliser pour poser le totem
        
      }
    break;
    case(2):
      if((teta + MY_PI/2) > 0.01){ //on tourne tant qu'on est pas au teta voulu (cible)
        vitesse = 0;
        teta_cible = -MY_PI/2;
      }
      else{
        if(test == 0){
          y = 150 - distance_f; 
          dist_rep = distance_r; //on prend la distance repere au tout début de la ligne droite 
          test = 1;
        }
        vitesse = 4; //on avance
        
        /*Serial.print(" x = ");                      
        Serial.println(x);
        Serial.print(" y = ");
        Serial.println(y);*/
        
        if(test2 == 1 && abs(y - y_rep - 1) < 0.01){ //je ne remets pas les commentaires pour les boucles identiques aux cas précédents 
          vitesse = 0;
          test2 = 0;
          test = 0;
          test3 = 0;
          test4 = 0;
          test5 = 0;
          cas = 3;
          x_rep = x;
          dist_rep_f = distance_f;
          obj[1] = (obj[1] + y) / 2;
        }
        if(abs(dist_rep - distance_r) > 2){
          obj[1] = y;
          test2 = 1;
          y_rep = y;
        }
      }
    break;

    
    case(3):
      if (test5==0) {
        teta=-MY_PI/2;
        test5=1;
        //Serial.println("init teta cas 3 ok");
      }
      
      if((teta + MY_PI) > 0.01){ //tourner de PI/2
        teta_cible = -MY_PI;
        
        vitesse = 0;
        
      }
      else{ //si c'est tourné
        if(test == 0){ 
          consigne = 7.0; // on baisse un peu la pince 8->7
          obj[0] = x - distance_f; // on note la coordonnee en x du totem
          test = 1;
          dist_rep = distance_r; // 1er tour de boucle on prend une dist referente 
        }
        vitesse = 4; 
        
        /*Serial.print(" x = ");
        Serial.println(x);
        Serial.print(" y = ");
        Serial.println(y);*/
        
        if((abs(x - obj[0]) < 5 || distance_f < 5 + dist_roue_pince) && test3 == 0){ //si quasi arrivé en coordonnée ou si le capteur détecte limite proche
            x_rep = x;
            test3 = 1;
        }
        if(test3 == 1 && test4 == 0){ //detecté pour la 1ere fois? 
            alpha_prim = asin((7.0 + 5.39)/15);
            x0 = (cos(alpha_prim)*15) + 14.12; // dist d'avance horizontale pour descendre au bon endroit 
            vitesse = -4;
            if(abs(x - x0 - dist_roue_pince) < 0.01){ //si assez reculé 
              consigne = 7.0; //descend la pince
              vitesse = 0;
            }
            if(abs(h -consigne) < 0.1){ // descent la pince 
              vitesse = 0;
              test2 = 1;
              test4 = 1;
            }
        }
        else if(test2 == 1 && test4 == 1){ //il avance doucement (à la bonne hauteur)
          vitesse = 0.5;
          if(x_rep - x + 3 < 0.1 || distance_f > 200){ // coord ou physique
            vitesse = 0;
            pince(); //saisit l'objet 
            consigne = 8.0; //remonte le bras  ( en cm)
            test=0;
            test2 = 0;
            test3 = 0;
            test4 = 0; //zréinitialisation des test pour la boucle d'après
            test5=0;
            cas = 4;
          }
        }
      }
    break;

    case(4):
      if (test4==0) {
        teta=-MY_PI;
        test4=1;
        //Serial.println("init teta ok");
      }
      
      if((teta + (1.5*MY_PI)) < -0.01){ //il tourne à 3PI/2
        vitesse = 0;
        teta_cible = -1.5*MY_PI;
        //Serial.println(teta+(1.5*MY_PI));
        //Serial.println("trop tourné");
        test=0;
      }

      if((teta + (1.5*MY_PI)) > 0.01){ // tourne  
        vitesse = 0;
        teta_cible = -1.5*MY_PI;
        //Serial.println(teta+(1.5*MY_PI));
        test=0;
      }
      else { //si t'as tourné 
        
        if(test == 0){
          //Serial.println("j'avance"); 
          dist_rep = distance_r; // dist fixe pour comparaison balise 
          test = 1; //test pour pas retomber dans la boucle 
        }
        vitesse = 4;

        if((test2 == 1)&&(compteur_balise==50)){  //déja expliqué dans les initialisations
          vitesse = 0;
          test = 0;
          cas = 5;
          compteur_balise = 0;
        }

        if(abs(dist_rep - distance_r) > 2){ 
          //Serial.println("balise detectée");
          compteur_balise = compteur_balise + 1;
          zone_a[1] = y;
          test2 = 1;
          y_rep = y;
        }

        
      }
    break;
    case(5):
      if(test==0) {
        teta = -1.5*MY_PI;
        test2 = 0;
        test3 = 0;
        test4 = 0;
        test5 = 0;
        test = 1;
        //Serial.println("init cas5 ok");
      }

      if((test5==1) && (abs(teta) > 0.01)) { //tourne, 
        vitesse = 0;
        teta_cible =  0; 
        //Serial.print("correction delta teta = ");
        //Serial.println(teta);
      }
      if((test5==0) && (teta > -2*MY_PI)) { //on veut qu'il tourne à teta=-2PI pour que ça refasse teta=0
        vitesse = 0;
        teta_cible =  -2*MY_PI; 
        //Serial.print("virage cas5, delta teta = ");
        //Serial.println(teta);

      }
      else{
        test5=1;
        if (test6 ==0) {
          vitesse = 3;
          
          /*Serial.println("j'avance cas5");
          Serial.print(" x = ");
          Serial.println(x);
          Serial.print(" y = ");
          Serial.println(y);*/
        }
        
        //Serial.print("delta x = ");
        //Serial.println(abs(x - zone_a[0]));
        if((abs(zone_a[0] - x) < 0.1) && test4 == 0 && test3==0){ //arrivé en x
          vitesse = 0;
          test4 = 1;
          test2 = 1;   
        }

        if(test2 == 1 && test3 == 0){ //au bon endroit + premiere fois 
          alpha_prim = asin((6.5 + 5.39)/15);
          x0 = (cos(alpha_prim)*15) + 14.12; //calcul du x0 pour la descente 
          vitesse = -4;
          if(x_rep - x - 3 < 0.1){ //descend le bras 
            consigne = 7;
            vitesse = 0;
          }
          if((abs(h -consigne) < 0.1)&& (test3==0)) { 
            vitesse = 0;
            test3 = 1;
            pince(); //on lache le totem
            vitesse = -0.5;
            test6=1;
          }
          
          if(test3 == 1){  
            vitesse = 0;
            consigne = 7;
            cas = 6;
          }
          
        }
      
      }
    break;
    case(6):
      if (compteur_recul<500) {
        compteur_recul = compteur_recul + 1; //il recule pour ne pas s'arreter avec le totem entre la pince écartée 
        vitesse = -0.5;
      }
      else {
        vitesse = 0; //il s'arrete
        v_omega = 0;
      }
      
    }
  }
  else{
    vitesse = 0;
    v_omega = 0;
    motorOFF(1);
    readMotorState(1);
    motorOFF(2);
    readMotorState(2);
    motorOFF(3);
    readMotorState(3);
  } 
  if(teta > 2*MY_PI){
    teta -= 2* M_PI;
  }
    
  if(teta < -2*MY_PI) {
    teta += 2* M_PI;
  }
    


    
  sleep_time = PERIOD_IN_MICROS-((micros()-current_time));
  if ( (sleep_time >0) && (sleep_time < PERIOD_IN_MICROS) ) {
    delayMicroseconds(sleep_time);
  }

  cpt--;
}