// Programme Chassi Robot Riom Robotique
// Version  : 5=6
// Etat : (En cours de dev / à tester / Fonctionnel)
//
//      à tester
//  en cours de test i2c, 02/03/2024 je recoit bien la trame i2c avec les donées dans le bon sens
//
//*********************************************************************
//******************** Integration et config WIFI *********************
//*********************************************************************
// serveur - depuis machine reseau http://192.XXX.XXX.XX?variable extrait la variable
//#include <M5Core2.h>
#include "Wire.h"
#define I2C_DEV_ADDR 0x55

//Definition des Variantes : 
#define serialP;
#define gst_I2C;

volatile byte * I2C_memory;

//*********************************************************************
//******************** Integration et config Moteur *******************
//*********************************************************************
#include <AccelStepper.h>
#define pul1 D10
#define dir1 D9
#define pul2 D2
#define dir2 D1
#define pul3 D8
#define dir3 D7
#define pul4 D6
#define dir4 D3

// AccelStepper stepperX(1.8, dir1, pul1); // X
// AccelStepper stepperY(1.8, dir2, pul2); // Y
// AccelStepper stepperZ(1.8, dir3, pul3); // Z
// AccelStepper stepperA(1.8, dir4, pul4); // A
AccelStepper stepperX(1.8, pul1, dir1); // X
AccelStepper stepperY(1.8, pul2, dir2); // Y
AccelStepper stepperZ(1.8, pul3, dir3); // Z
AccelStepper stepperA(1.8, pul4, dir4); // A

int Men= D0; //Motor All enable pin

// Data pour la lecture de donnée
String nom = "Arduino";
String msg;
int inChar;
int data[] = {0, 0};
int com[] = {0, 0, 0};

// -------------- Direction des moteurs ( pour un sens de marche commun )
int dirX= +1;   
int dirY= +1;
int dirZ= +1;
int dirA= +1;

int cli_led=0; // initial value of input
int Move = 0;
String rc;

int16_t Target[] = {0,0,0,0,0};   //Tableau de la commande de position (X,Y,Rotation,speed,accel)
int16_t AxeX[] = {0,0,0,0};   // distance en X pour chaque moteur
int16_t AxeY[] = {0,0,0,0};   // distance en Y pour chaque moteur
int16_t AxeRot[] = {0,0,0,0}; // distance en R pour chaque moteur
int16_t Vecteur[] = {0,0,0,0};// distance en X pour chaque moteur

int PosX = 0;     //Position X du robot
int PosY = 0;     //Position Y du robot
int Rot = 0;      //Rotation du robot
int vitesse = 2000;
int accel = 2000;


uint8_t Mot[] = {0,0,0,0,0,0,0,0,0,0,0,0} ;   //11*8bits  Mot de commande I2C du Chassi ****Cmd, X, Y, Rot, Vit, Acc****
uint8_t c[] = {0,0,0,0,0,0,0,0,0,0,0} ;
bool Interrupt = 0; // commande d'intéruption de mouvement (Arret d'Urgence) premier bit du mot de Cmd
bool Modif_Mvt = 0; // commande de modification de mouvement deuxieme bit du mot de Cmd
bool fin_mvt = 0;
int h=0;


void setup()
{  
  pinMode(Men,OUTPUT);
  stop();
  #ifdef serialP
  Serial.begin(115200);
  #endif
  
  // X
   stepperX.setMaxSpeed(vitesse);   stepperX.setAcceleration(accel);
  // Y
   stepperY.setMaxSpeed(vitesse);   stepperY.setAcceleration(accel);
  // Z   
   stepperZ.setMaxSpeed(vitesse);   stepperZ.setAcceleration(accel);
  // A  
   stepperA.setMaxSpeed(vitesse);   stepperA.setAcceleration(accel);

  
  //Serial.println(Men);
  I2C_Slave_init(I2C_DEV_ADDR);
  I2C_memory = get_i2c_data();


  //digitalWrite(Men, LOW);// Low Level Enable
  pinMode(pul1, OUTPUT);
  pinMode(pul2, OUTPUT);
  pinMode(pul3, OUTPUT);
  pinMode(pul4, OUTPUT);
  pinMode(dir1, OUTPUT);
  pinMode(dir2, OUTPUT);
  pinMode(dir3, OUTPUT);
  pinMode(dir4, OUTPUT);

}


void loop()
{ 
  static char wait = 0;
  static int64_t time;
  
  for(int i=0; i<8; i++){
    Serial.printf("0x%x ", I2C_memory[i]);
  }
  Serial.printf("\n");


  if(((I2C_memory[0] & 0x02)==2) && Modif_Mvt==0){
    Modif_Mvt = 1;
  }
  if(Modif_Mvt == 1){
    //Mot = I2C_memory;
    Target[0]= (I2C_memory[1] << 8) | I2C_memory[2];
    Target[1]= (I2C_memory[3] << 8) | I2C_memory[4];
    Target[2]= (I2C_memory[5] << 8) | I2C_memory[6];
    Target[3]= (I2C_memory[7] << 8) | I2C_memory[8];
    Target[4]= (I2C_memory[9] << 8) | I2C_memory[10];
    for(int i=1; i<9; i++){
      I2C_envoi_8bits(0,i);
    }
    Position_Calculation(); //************* Calcul du Vecteur XY (incrémental)
    Mvmt();                 //************* Envoie de la position à ateindre pour chaque moteur + Execution du mouvement
    stop(); // Servo Off
    
    I2C_memory[0] = 0x04;
    I2C_envoi_8bits(I2C_memory[0] ,0);
  }

  
  // else{
  //   I2C_memory = get_i2c_data();    
  //   I2C_envoi_8bits(I2C_memory[0] | 1<<2,0);
  //   I2C_envoi_8bits(I2C_memory[0] | 1<<1,0);
  //   for(int i=1; i<9; i++){
  //     I2C_envoi_8bits(0,i);
  
  if((I2C_memory[0] & 0x02)==0 && Modif_Mvt==1){
    Modif_Mvt = 0;
  }
  
  //Dé-asservissement des moteurs
  
  
  
}

void Position_Calculation(){
  PosX = Target[0];
  AxeX[0] = {PosX};   //x
  AxeX[1] = {PosX};  //y
  AxeX[2] = {-PosX};  //z
  AxeX[3] = {-PosX};   //a
  PosY = Target[1];
  AxeY[0] = {PosY};   //x
  AxeY[1] = {PosY};   //y
  AxeY[2] = {PosY};   //z
  AxeY[3] = {PosY};   //a
  Rot = Target[2];
  AxeRot[0] = {-Rot};   //x
  AxeRot[1] = {Rot};   //y
  AxeRot[2] = {Rot};   //z
  AxeRot[3] = {-Rot};   //a

  vitesse = Target[3];
  accel = Target[4];
  
  for (int i=0; i<4; i++){
    Vecteur[i] = (AxeX[i] + AxeY[i] + AxeRot[i])/2;
    Serial.printf("Vecteur[%d]: %d\n", i, Vecteur[i]);
  }

  stepperX.moveTo(stepperX.currentPosition() + Vecteur[0]*dirX);
  stepperY.moveTo(stepperY.currentPosition() + Vecteur[1]*dirY);
  stepperZ.moveTo(stepperZ.currentPosition() + Vecteur[2]*dirZ);
  stepperA.moveTo(stepperA.currentPosition() + Vecteur[3]*dirA);

  stepperX.setMaxSpeed(vitesse);  stepperX.setAcceleration(accel);    // X
  stepperY.setMaxSpeed(vitesse);  stepperY.setAcceleration(accel);    // Y
  stepperZ.setMaxSpeed(vitesse);  stepperZ.setAcceleration(accel);    // Z  
  stepperA.setMaxSpeed(vitesse);  stepperA.setAcceleration(accel);    // A
  
  
  #ifdef serialP
  // Serial.print("Target 0 ");Serial.print("\tTarget 1");;Serial.print("\tTarget 2");;Serial.print("\tTarget 3");Serial.println("\tTarget 4");
  // Serial.print(Target[0]);Serial.print("\t\t");Serial.print(Target[1]);Serial.print("\t\t");Serial.print(Target[2]);Serial.print("\t\t");Serial.print(Target[3]);Serial.print("\t\t");Serial.println(Target[4]);
  // Serial.println();
  // Serial.print("X ");Serial.print("\t\tY");;Serial.print("\t\tZ");;Serial.print("\t\tA");Serial.println("\t\tRot");
  // Serial.print(Vecteur[0]);Serial.print("\t\t");Serial.print(Vecteur[1]);Serial.print("\t\t");Serial.print(Vecteur[2]);Serial.print("\t\t");Serial.print(Vecteur[3]);Serial.print("\t\t");Serial.println(Vecteur[4]);
  // delay(100);
  #endif
  for (int i=0; i<4; i++){
  Target[i] = 0;
  }
  // RAZ du flag de mouvement modifié
  Modif_Mvt = 0;
  
  for (int i=0; i < sizeof(Mot); i++){
    Mot[i] = 0;
  }
}
void Mvmt(){
  int64_t time = esp_timer_get_time();
  
  // Mise en Servo ON des moteurs
  digitalWrite(Men,LOW);// Low Level Enable
  if(stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0){
    //Execution du mouvement global (resort de la boucle une fois arrivé)
    #ifdef serialP
      // Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif");
      // Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt);
    #endif
    while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) /*&& (Interrupt != 1 || Modif_Mvt != 1)*/){
        stepperX.run(); // X
        stepperY.run(); // Y
        stepperZ.run(); // Z
        stepperA.run(); // A 
        //-------------------
        #ifdef serialP
        //  Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif");
        //  Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt);
        // delay(100);
        #endif
    }
  }  
  // if(Modif_Mvt == 1){
  //   Position_Calculation();
  //   Serial.println("\t\tRe Calculation !!!");
  // }
}
void stop(){
  if(Interrupt== 1){
    //delay(1000);
    Interrupt = 0;
  }
  #ifdef serialP
  // h++;
  // Serial.print("**********STOP****** ");Serial.println(h);
  #endif
 digitalWrite(Men,HIGH);// Low Level Enable
 delay(500);
  }
