// Libraries for CAN communications
#include <can-serial.h>
#include <mcp2515_can.h>
#include <mcp2515_can_dfs.h>
#include <mcp_can.h>
#include <Servo.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 "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_ID 3  // Here change with the motor ID if it has been changed with the firmware.
#define MOTOR_MAX_VEL_CMD 20000 
#define MOTOR_MAX_VOLTAGE_CMD 200 
#define MOTOR_MAX_POS_DEG 120.0
#define MOTOR_MIN_POS_DEG -120.0

#define BUTTON_PIN 2
#define LED_PIN 7

int trigPin1 = 5;
int echoPin1 = 6;
int trigPin2 = 3;
int echoPin2 = 4;

// Global motor state variables
int MOTOR_ID = 3;

double currentMotorPosDeg;
double currentMotorVelDegPerSec;
double previousMotorPosDeg;
int currentMotorPosEncoder;
int offsetMotorPosEnconder;
int currentNumOfMotorRevol;

// PIN ASSIGNMENT
uint8_t analogPinP0=A0; 
uint8_t analogPinP1=A2; 
int currentP0Rawvalue;
int currentP1Rawvalue;

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



unsigned long previousMillis = 0;
const long interval = 1000;  // Intervalle de clignotement (1 sec)
bool ledState = false;

int EtatRobot = 0;
double distance_avant = 0;
double distance_droite = 0;
double prev_distance_droite;
double prev_distance_avant;
bool buttonState;

double targetDistanceMesure;

Servo monServo; 




void motorON(){
  unsigned char msg[MAX_DATA_SIZE] = {
    0x88,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00,
    0x00
  };

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

}

void motorOFF(){

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

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


void readMotorState()  {
  uint32_t id;
  uint8_t type;
  uint8_t len;
  byte cdata[MAX_DATA_SIZE] = { 0 };
  int data2_3, data4_5, data6_7;
  int currentMotorVelRaw;

  // 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 = (int)data6_7;
  }

  if(MOTOR_ID == 3){
    // Conversion of the velocity and writing in the global cariable
    currentMotorVelDegPerSec = ((double)(currentMotorVelRaw)); 

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

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

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



/*
TO DO : 

Code fonctionnel et modulable avec faciliter pour tester les différentes fonctions 


*/



/*
3 moteurs
1 servomoteur
2 capteurs distances
*/

void controlMoteurPosition(double targetPositionDeg){
  MOTOR_ID = 3;
  //readMotorState();

  double positionError = targetPositionDeg - currentMotorPosDeg;
  double Kp = 500.0;
  double velocityCommand = Kp*positionError;

  //Serial.println(currentMotorPosDeg);

  if(velocityCommand > MOTOR_MAX_VEL_CMD){
    velocityCommand = MOTOR_MAX_VEL_CMD;
  }
  if(velocityCommand < -MOTOR_MAX_VEL_CMD){
    velocityCommand = -MOTOR_MAX_VEL_CMD;
  }

  sendVelocityCommand((long int)(velocityCommand)); 
  readMotorState();
}


void vitessMoteurs(double v1, double v2){
  MOTOR_ID = 1; //moteur gauche
  sendVelocityCommand((long int)(v1)); 
  readMotorState();

  MOTOR_ID = 2; //moteur droit
  sendVelocityCommand((long int)(v2)); 
  readMotorState();
}



void avancerToutDroit(){
	vitessMoteurs(5000, -5000); //15000
}

void tournerGauche(){//precisement
	//peut-etre avec delay (stop le programme pour le temps de la fonction)

  digitalWrite(LED_PIN, HIGH);
  Serial.println("debut tourner");
  float tempsRecul = 500; //TUNE
  unsigned long tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRecul) {
    //Serial.println(millis() - tempsDebut);
    vitessMoteurs(-5000, 5000);
  }
  Serial.println("fini reculer");
  delay(500);
  vitessMoteurs(0, 0);

  float tempsRotationMillis = 9600; //4700 9400
	tempsDebut = millis();
	while (millis() - tempsDebut <= tempsRotationMillis) {
    //Serial.println(millis() - tempsDebut);
	  vitessMoteurs(0, -10000); //-10000
  }
  vitessMoteurs(0, 0); 

  digitalWrite(LED_PIN, LOW);
}

double vitesseGauche;
double vitesseDroite;
void avancerParallementAuMur(double targetDistance){
	long int vitesse = 15000; //15000

	double DistanceError = targetDistance - distance_droite; //distance_avant pour test peut-etre
  /*if(DistanceError > -1 &&  DistanceError < 1){
    DistanceError = 0;
  }*/
  Serial.print("Distance Error : ");
  Serial.println(DistanceError);
	double Kp = 1500.0;
	double velocityCommand = Kp*DistanceError;

	if(vitesse + velocityCommand > 20000){
	velocityCommand = 20000;
	}
	if(vitesse - velocityCommand < -20000){
	velocityCommand = -20000;
	}


  vitesseGauche = vitesse - velocityCommand;
  vitesseDroite = -15000; //15000
  
	vitessMoteurs(vitesseGauche, vitesseDroite); //velocityCommand > 0 : tourner gauche
}


double degresMoteurGauche;
double degresMoteurDroit;
double DiffDegres;
double commande;
bool start = true;
double degresGaucheInit;
double degresDroitInit;
void avancerDroitEncodeur(){

  if(start == true){
    start = false;
    MOTOR_ID = 1; //moteur gauche
    readMotorState();
    degresGaucheInit = currentMotorPosDeg;
    MOTOR_ID = 2; //moteur droit
    readMotorState();
    degresDroitInit = currentMotorPosDeg;
  }

  MOTOR_ID = 1; //moteur gauche
  readMotorState();
  degresMoteurGauche = currentMotorPosDeg - degresGaucheInit;

  MOTOR_ID = 2; //moteur droit
  readMotorState();
  degresMoteurDroit = currentMotorPosDeg - degresDroitInit;

  DiffDegres = degresMoteurGauche - degresMoteurDroit; // > 0 : gauche > droit : tourne a droite
  commande = 200 * DiffDegres;

  /*if(DiffDegres > 1){
    vitesseGauche = 5000 - commande;
    vitesseDroite = -5000;
  }
  else if(DiffDegres < -1){
    vitesseGauche = 5000;
    vitesseDroite = -5000 + commande;
  }
  else{
    vitesseGauche = 5000;
    vitesseDroite = -5000;
  }*/

  if(DiffDegres > 1 || DiffDegres < -1){
    vitesseGauche = 5000 - commande;
    vitesseDroite = -5000;
  }
  else{
    vitesseGauche = 5000;
    vitesseDroite = -5000;
  }

  vitessMoteurs(vitesseGauche, vitesseDroite);

}


void aucunMouvement(){
	vitessMoteurs(0, 0);
  /*MOTOR_ID = 3;
  sendVelocityCommand((long int)(0)); 
  readMotorState();*/
}


int sousEtapePince = 0; //1

void etape0(){

	//aucunMouvement();

  controlMoteurPosition(40); //TUNE

	//build-in led qui clignote
	unsigned long currentMillis = millis();
	if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    ledState = !ledState;  // Alterne l'état de la LED
    digitalWrite(LED_PIN, ledState);
  }

  buttonState = digitalRead(BUTTON_PIN);
  if (buttonState == LOW) {  // LOW : bouton est appuyé 
    digitalWrite(LED_PIN, LOW);
    float tempsRotationMillis = 500; 
    unsigned long tempsDebut = millis();
    while (millis() - tempsDebut <= tempsRotationMillis) {
      updateDataDistanceCote();
    } 
    targetDistanceMesure = distance_droite;
    
    /*monServo.write(180); //ferme la pince
    delay(500);
    while (currentMotorPosDeg > 0){ //TUNE
    controlMoteurPosition(0);
    }*/

    EtatRobot = 2; //3
    sousEtapePince = 0;
  }
}

int compteurSeuil = 0;
void etape1(){ //SKIP
	/*
	avancer jusqu'à une certaine distance 
	puis tourner à gauche (90deg)
	*/
  //Serial.println("etape 1");

  double seuil = 10.0;
 
  controlMoteurPosition(40); //TUNE

	
	if (distance_avant > seuil)
	{
		avancerToutDroit();
    //vitessMoteurs(-5000, 5000);
    compteurSeuil = 0;
	}
	if (distance_avant <= seuil)
	{	
		compteurSeuil++;
	}

  if (compteurSeuil >= 3)
  {
    compteurSeuil = 0;
    aucunMouvement();
    delay(500);
    tournerGauche();
    aucunMouvement();
    delay(500);
    targetDistanceMesure = distance_droite;
    EtatRobot = 2;
  }
  
  //vitessMoteurs(-5000, 5000);

}


void etape2(){
	//avancer tout droite jusqu'à trouver le premier plot et tourner à gauche
	/*Serial.println("etape 2");
  tournerGauche();
  EtatRobot = 0;*/

  //targetDistanceMesure = distance_droite; //a mettre dans l'étape précédente + TESTER
  avancerParallementAuMur(targetDistanceMesure);
  //avancerDroitEncodeur(); 
	double seuilBorne = 1.5;

	if (prev_distance_droite - distance_droite > seuilBorne) 
	{	
    aucunMouvement();
    delay(500);
    tournerGauche();
    aucunMouvement();
    delay(500);
    targetDistanceMesure = distance_droite;
    EtatRobot = 3; //=3
  }

}


void AvancerJusquaDistance(){
  if (distance_avant >= 4) //TUNE
  {
    //avancerParallementAuMur(20); //targetDistanceMesure
    vitessMoteurs(5000, -5000);
  }
  else {
    //aucunMouvement();
    vitessMoteurs(0, 0);
    sousEtapePince = 1;
  }
}

int angleBras = 40; //TUNE
int DescenteBras = 0;
void scan(){
  if (distance_avant - prev_distance_avant <= 20) //TUNE
  {
    //lever bras 
    angleBras--;
    controlMoteurPosition(angleBras);
    delay(50);
    Serial.println(distance_avant);
    if(angleBras <= 0){
      DescenteBras = 10;
      sousEtapePince = 2;
    }

  }
  else {
    //aucunMouvement();
    sousEtapePince = 2;
  }
}

void descendreBras(){

  Serial.println("etape 2");
  if (currentMotorPosDeg < angleBras + 18){ //TUNE : angle proportionnel au levage : ex : angleBras + k*(40-angleBras) //+20 //+ DescenteBras
    controlMoteurPosition(angleBras + 18);  //Trouver k pour avoir toujours la meme distance à parcourir
  }
  else {
    //aucunMouvement();
    sousEtapePince = 3;
    delay(500);
  }
}

void avancerPourPince(){

  Serial.println("etape 3");
  digitalWrite(LED_PIN, HIGH);
  float tempsRotationMillis = 2500 + DescenteBras*100; //TUNE
  unsigned long tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRotationMillis) {
    avancerToutDroit();
  }
  vitessMoteurs(0, 0);
  digitalWrite(LED_PIN, LOW);

  monServo.write(180); //ferme la pince

  delay(500);
  sousEtapePince = 4;
}

void leverBras(){
  //MOTOR_ID = 3;
  //readMotorState(); //
  Serial.println("etape 4");
  if (currentMotorPosDeg > 5){ //TUNE
    controlMoteurPosition(5);
  }
  else {
    aucunMouvement();
    //monServo.write(180); //maybe
    delay(500);
    sousEtapePince = 5;
    
  }
}

void etapePince5(){
  Serial.println("etape 5");

  float tempsRotationMillis = 7000; 
  unsigned long tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRotationMillis) {
    avancerToutDroit();
  } 

  aucunMouvement();
  delay(500);
  tournerGauche();
  Serial.println("fini tourner");
  aucunMouvement();
  delay(500);

  tempsRotationMillis = 500; 
  tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRotationMillis) {
    updateDataDistanceCote();
  } 
  targetDistanceMesure = distance_droite;
  EtatRobot = 4; //0
}



void etape3(){
  //Peut-être : dans LOOP : tout le temps mettre a jour a position du bras 
  //et faire varier la valeur dans les fonctions
  switch (sousEtapePince) { 
    case 0 :
      AvancerJusquaDistance();
      break;
    case 1 : 
      scan();
      break;
    case 2 : 
      descendreBras();
      break;
    case 3 : 
      avancerPourPince();
      break;
    case 4 : 
      leverBras();
      break;
    case 5 : 
      etapePince5();
      break;

	}

}



void etape4(){
	//avancer tout droite jusqu'à trouver le premier plot et tourner à gauche
  Serial.println("etape 4 general");
  Serial.print("targetDistanceMesure : ");
  Serial.println(targetDistanceMesure);
  Serial.print("prev_distance_droite : ");
  Serial.println(prev_distance_droite);
  Serial.print("distance_droite : ");
  Serial.println(distance_droite);

	avancerParallementAuMur(targetDistanceMesure); 
	double seuilBorne = 1.5;

	if (prev_distance_droite - distance_droite > seuilBorne) 
	{
    aucunMouvement();
    delay(500);
    tournerGauche();
    aucunMouvement();
    float tempsRotationMillis = 500; 
    unsigned long tempsDebut = millis();
    while (millis() - tempsDebut <= tempsRotationMillis) {
      updateDataDistanceCote();
    } 
    targetDistanceMesure = distance_droite;
    EtatRobot = 5; 
  }
}


void poserObjet(){
	//reculer pendant un certain temps
  float tempsRotationMillis = 9500; //TUNE
  unsigned long tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRotationMillis) {
    vitessMoteurs(-5000, 5000);
    delay(10);
  }
  vitessMoteurs(0, 0);

	//baisser le bras pour que le totem touche le sol
  while (currentMotorPosDeg < 40){ //TUNE
    controlMoteurPosition(40);
  }

	//ouvrir la pince
  monServo.write(0);
  delay(500);

  //reculer pendant un certain temps
  tempsRotationMillis = 2000; //TUNE
  tempsDebut = millis();
  while (millis() - tempsDebut <= tempsRotationMillis) {
    vitessMoteurs(-5000, 5000);
    delay(10);
  }
  vitessMoteurs(0, 0);
}


void etape5(){
	//avancer tout droite jusqu'à trouver le premier plot, poser le totem au bon endroit, puis se reculer

	avancerParallementAuMur(targetDistanceMesure); 
	double seuilBorne = 1.5;

	if (prev_distance_droite - distance_droite > seuilBorne) 
	{	
    aucunMouvement();
    delay(500);
    poserObjet(); //
    EtatRobot = 0; 
  }
}


int tempoPriseMesureAvant = 0;
void updateDataDistanceAvant(){
  tempoPriseMesureAvant++;
  if (tempoPriseMesureAvant >= 50)
  {
    tempoPriseMesureAvant = 0;
    prev_distance_avant = distance_avant;
  }

  distance_avant = capteurDistance(2);
  /*Serial.print("Distance: ");
  Serial.print(distance_avant);
  Serial.println(" cm");
  */
  delay(10);
}

int tempoPriseMesureCote = 0;
void updateDataDistanceCote(){
  tempoPriseMesureCote++;
  if (tempoPriseMesureCote >= 50)
  {
    tempoPriseMesureCote = 0;
    prev_distance_droite = distance_droite;
  }

  distance_droite = capteurDistance(1);
	/*Serial.print("Distance Droite: ");
  Serial.print(distance_droite);
  Serial.println(" cm");*/
  delay(10);  
}

double capteurDistance(int id){
	int trig;
	int echo;
	if (id == 1)
	{
		trig = trigPin1;
		echo = echoPin1;
	}
	if (id == 2)
	{
		trig = trigPin2;
		echo = echoPin2;
	}
	digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  // Mesure de la durée de l'impulsion sur Echo
  long duration = pulseIn(echo, HIGH);

  // Conversion du temps en distance (cm)
  return duration * 0.034 / 2;
}


/********************* THIS FUNCTION IS EXECUTED FIRST AND CONTAINS INITIALIZATION ***********/
void setup() {
  int i;
  char serialReceivedChar;
  int nothingReceived;

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


  pinMode(BUTTON_PIN, INPUT_PULLUP); //modif buttonPin (2)

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  pinMode(trigPin1, OUTPUT); //modif trigPin ()
  pinMode(echoPin1, INPUT); //modif echoPin ()
  pinMode(trigPin2, OUTPUT); //modif trigPin ()
  pinMode(echoPin2, INPUT); //modif echoPin ()

  monServo.attach(8);    // Attache du servomoteur sur la broche 10
  monServo.write(0); //TUNE
  delay(20);
  



  // Initialization of the analog input pins
  pinMode(analogPinP0, INPUT);
  pinMode(analogPinP1, INPUT);
  currentP0Rawvalue = analogRead(analogPinP0);
  currentP1Rawvalue = analogRead(analogPinP1);

  // 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
  previousMotorPosDeg = 0.0;
  currentNumOfMotorRevol = 0;
  offsetMotorPosEnconder = 0;

  // Send motot off then motor on command to reset

  MOTOR_ID = 1;
  motorOFF();
  delay(200); //500
  readMotorState();

  MOTOR_ID = 2;
  motorOFF();
  delay(200);
  readMotorState();

  MOTOR_ID = 3;
  motorOFF();
  delay(200);
  readMotorState();
  


  /*
  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;
    }
  }
  */


  MOTOR_ID = 1;
  motorON();
  readMotorState();
  delay(200);
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState();
  /*offsetMotorPosEnconder = currentMotorPosEncoder;
  currentNumOfMotorRevol = 0;
  previousMotorPosDeg = 0.0;*/
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState(); 

  MOTOR_ID = 2;
  motorON();
  readMotorState();
  delay(200);
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState();
  /*offsetMotorPosEnconder = currentMotorPosEncoder;
  currentNumOfMotorRevol = 0;
  previousMotorPosDeg = 0.0;*/
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState(); 

  MOTOR_ID = 3;
  motorON();
  readMotorState();
  delay(200);
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState();
  offsetMotorPosEnconder = currentMotorPosEncoder;
  currentNumOfMotorRevol = 0;
  previousMotorPosDeg = 0.0;
  sendVelocityCommand((long int)(0));
  delay(200);
  readMotorState(); 



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

}



void loop(){

	//bouton poussoir qui reset les variables
	/*buttonState = digitalRead(BUTTON_PIN);
  if (buttonState == LOW && EtatRobot != 0) {  // LOW : bouton est appuyé 
    EtatRobot=0;
    delay(500);
    Serial.println(EtatRobot);
  }*/
  

  updateDataDistanceAvant();
  updateDataDistanceCote();   

	switch (EtatRobot) { 
    case 0 :
      etape0();
      break;
    case 1 : 
      etape1();
      break;
    case 2 : 
      etape2();
      break;
    case 3 : 
      etape3();
      break;
    case 4 : 
      etape4();
      break;
    case 5 : 
      etape5();
      break;
  }


}