// 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>
#include <math.h>

#define MY_PI 3.14159265359

#define PERIOD_IN_MICROS 10000 // 10 ms

#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 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;
int test4 = 0;
double dist_rep = 0;
double dist_rep_f = 0;

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;


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

  // 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() {
  
  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() {
    myservo.attach(8);
  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;
  double d_g = (currentMotorPosDeg[1] - last_currentMotorPosDeg[1]) * (MY_PI / 180) * rayon;
  d_teta = (d_d - d_g) / dist_roue;
  y -= ((d_d + d_g) * cos((MY_PI - d_teta) / 2 - teta)) / 2;
  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;
        if(test2 == 1 && abs(dist_rep - distance_r) >= 2){
          test2 = 0;
          test = 0;
          cas = 2;
          zone_a[0] = (zone_a[0] + x) / 2;
          dist_rep = distance_r;
        }
        if(abs(dist_rep - distance_r) > 2){
          test2 = 1;
          dist_rep = distance_r;
          zone_a[0] = x;
        }
        break;
      case(2):
      if((teta + MY_PI/2) > 0.01){
        vitesse = 0;
        teta_cible = -MY_PI/2;
      }
      else{
        if(test == 0){
          y = 150 - distance_f; 
          dist_rep = distance_r;
          test = 1;
        }
        vitesse = 4;

      if(test2 == 1 && abs(y - y_rep - 1) < 0.01){
        vitesse = 0;
        test2 = 0;
        test = 0;
        test3 = 0;
        test4 = 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((teta + MY_PI) > 0.01){
          teta_cible = -MY_PI;
          vitesse = 0;
        }
        else{
          if(test == 0){
            consigne = 7.0;
            obj[0] = x - distance_f;
            test = 1;
            dist_rep = distance_r;
          }
          vitesse = 4;
          if((abs(x - obj[0]) < 5 || distance_f < 5 + dist_roue_pince) && test3 == 0){
              x_rep = x;
              test3 = 1;
          }
          if(test3 == 1 && test4 == 0){
              alpha_prim = asin((7.0 + 5.39)/15);
              x0 = (cos(alpha_prim)*15) + 14.12;
              vitesse = -4;
              if(abs(x - x0 - dist_roue_pince) < 0.01){
                consigne = 7.0;
                vitesse = 0;
              }
              if(abs(h -consigne) < 0.1){
                vitesse = 0;
                test2 = 1;
                test4 = 1;
              }
          }
          else if(test2 == 1 && test4 == 1){
            vitesse = 0.5;
            if(x_rep - x + 3 < 0.1 || distance_f > 200){
              vitesse = 0;
              pince();
              consigne = 8.0;
              test2 = 0;
              cas = 4;
            }
          }
        }
        break;

      case(4):
      if((teta + MY_PI/2) > 0.01){
        vitesse = 0;
        teta_cible = -MY_PI/2;
      }
      else{
        if(test == 0){
          consigne = 9.0;
          dist_rep = distance_r;
          test = 1;
        }
      vitesse = 4;
      if(test2 == 1){
        vitesse = 0;
        test2 = 0;
        test = 0;
        cas = 5;
        x_rep = x;
        dist_rep_f = distance_f;
        zone_a[1] = (zone_a[1] + y) / 2;
      }
      if(abs(dist_rep - distance_r) > 2){
          zone_a[1] = y;
          test2 = 1;
          y_rep = y;
        }
      }
      break;
    case(5):
      if((teta + 2 * MY_PI) > 0.01){
        vitesse = 0;
        teta_cible = - 2 * MY_PI;
      }
      else{
        vitesse = 4;
        if(abs((x + dist_roue_pince) - zone_a[0]) < 0.1 && test == 0){
            vitesse = 0;
            test = 1;
            test2 = 1;
        }
        if(test2 == 1 && test3 == 0){
          alpha_prim = asin((6.5 + 5.39)/15);
          x0 = (cos(alpha_prim)*15) + 14.12;
          vitesse = -4;
          if(x_rep - x - 3 < 0.1){
            consigne = 7.5;
            vitesse = 0;
          }
          if(abs(h -consigne) < 0.1){
            vitesse = 0;
            test3 == 1;
            x_rep = x;
            pince();
            vitesse = -0.5;
          }
          if(abs(x - x_rep - 2) < 0.1 && test3 == 1){
            vitesse = 0;
            consigne = 8;
            cas = 6;
          }
          
        }
       
      }
      break;
    case(6):
      vitesse = 0;
      v_omega = 0;
    }
  }
  else{
    vitesse = 0;
    v_omega = 0;
    motorOFF(1);
    motorOFF(2);
    motorOFF(3);
  } 
  while(teta > 2*MY_PI)
    teta -= 2* M_PI;
  while(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--;
}