/* Simulacion de AragFour: Araña Robótica Cuadrúpeda

Descripción del Proyecto:
AragFour es un robot cuadrúpedo diseñado para diversas aplicaciones, incluyendo operaciones de rescate en terrenos irregulares y exploración terrestre. Este innovador proyecto utiliza un kit de desarrollo ESP-32 como sistema controlador, permitiendo la comunicación y el control del robot a través de Bluetooth mediante una aplicación especializada para smartphones.

Características Técnicas:
- Actuadores: Equipado con 12 servomotores MG90S que facilitan el sistema de locomoción.
- Material de Construcción: Fabricado en acrílico transparente, proporcionando un acabado estético y profesional.
- Control y Comunicación: El ESP-32 permite una conexión eficiente y control remoto a través de una aplicación móvil dedicada.

Estado del Proyecto:
AragFour se encuentra en constante desarrollo, con el objetivo de optimizar su rendimiento y ampliar sus capacidades operativas. 

 Cableado: 
  -. Pata [1]: Naranja.
  -. Pata [2]: Amarillo.
  -. Pata [3]: Verde.
  -. Pata [4]: Azul.

  Alumnos de Ingenieria de Teleconunicaciones:
    * Heber Lopez V-31.023.609
    * Jose Betancourt V-30.197.503
    * Rafael Bolivar V-30.801.414
*/

#include <ESP32Servo.h>

typedef struct {
  int coxaPin, femurPin, tibiaPin;
  Servo coxa, femur, tibia;
} Pata;

double ejeX = 115; 
double ejeY = 0;
double desfaceZ = 100;

double gama, alfa, beta, hipoXY, lineaAlfa, ejeZ;

int valor = 0;

// Logitudes de las extremidades
#define COXA 45
#define FEMUR 70
#define TIBIA 100
#define NUMPATAS 4

Pata patas[NUMPATAS] = {{32, 14, 21}, {33, 25, 26}, {27, 16, 17}, {5, 18, 19}}; // pines de los servos
 
byte t = 0;

void setup() {
// Attach servos
  for (int i = 0; i < NUMPATAS; i++) {
    patas[i].coxa.attach(patas[i].coxaPin);
    patas[i].femur.attach(patas[i].femurPin);
    patas[i].tibia.attach(patas[i].tibiaPin);
  }
  
  Serial.begin(9600);
}
/* Enviamos datos seriales para llamar las funciones de locomocion 
  teniendo en cuenta el switch dentro de la funcion Loop */
void loop() {
// Recepcion de datos Seriales
if(Serial.available() > 0){

    unsigned dato = Serial.parseInt();

    switch(dato){
      case 1: avanzar(500); Serial.println("Avanzar"); break;
      case 2: retroceso(500); Serial.println("Retroceso"); break;
      case 3: fundamental(); break;
      case 4: break;
      case 5: break;
      case 6: calibrar(); Serial.println("Calibrar"); break;
      default: break;
    }
/*
// Suponiendo que esperamos tres datos separados por comas; COXA, FEMUR, TIBIA
    String datos = SerialBT.readStringUntil('\n'); // Lee la línea completa hasta el salto de línea
    int valores[3]; // Array para almacenar los tres números
    int index = 0; // Índice para el array

    // Separa los datos basados en la coma y los almacena en el array
    for (int i = 0; i < datos.length(); i++) {
      int found = datos.indexOf(',', i); // Encuentra la próxima coma
      if (found == -1) found = datos.length(); // Si no hay más comas, toma el final de la cadena
      valores[index++] = datos.substring(i, found).toInt(); // Convierte el substring a entero y lo almacena
      i = found; // Actualiza el índice de búsqueda
    }
    
    // Ahora puedes usar los valores como desees
    SerialBT.print("Primer valor: ");
    SerialBT.println(valores[0]);
    SerialBT.print("Segundo valor: ");
    SerialBT.println(valores[1]);
    SerialBT.print("Tercer valor: ");
    SerialBT.println(valores[2]);

   ejeX = valores[0];
   ejeY = valores[1];
   desfaceZ = valores[2];
  
  hipoXY = HipoXY(ejeX, ejeY);
  lineaAlfa = LineaA(desfaceZ, hipoXY, COXA);
  
  gama = Gamma(ejeY, ejeX);
  alfa = Alfa(desfaceZ, lineaAlfa, TIBIA, FEMUR);
  beta = Beta(lineaAlfa, TIBIA, FEMUR);
  
  coxa.write(gama + 90);
  femur.write(alfa);
  tibia.write(map(beta, 0, 180, 180, 0) );

  SerialBT.print(ejeX);
  SerialBT.print("    ");
  SerialBT.print(ejeY);
  SerialBT.print("    ");
  SerialBT.print(gama);
  SerialBT.print("    ");
  SerialBT.print(alfa);
  SerialBT.print("    ");
  SerialBT.println(beta);

  delay(50);*/
}
}

////////// Locomocion del robot ///////////

void avanzar(int interval){
  for (int i = 1; i <= 4; i++) {
    if (i == 1 || i == 3){
      // para 1, pata 3
      calcularAngulos(i, 0, 120, -50, 20);
      delay(interval);
      calcularAngulos(i, 0, 120, -50, 50);
      delay(interval);
    } else {
      // para 2, pata 4
      calcularAngulos(i, 1, 120, -50, 20);
      delay(interval);
      calcularAngulos(i, 1, 120, -50, 50);
      delay(interval);
    }
  }
  fundamental();
}

void retroceso(int interval){
  for (int i = 1; i <= 4; i++) {
    if (i == 1 || i == 3){
      // para 1, pata 3
      calcularAngulos(i, 0, 80, 120, 20);
      delay(interval);
      calcularAngulos(i, 0, 80, 120, 38);
      delay(interval);
    } else {
      // para 2, pata 4
      calcularAngulos(i, 1, 80, 120, 20);
      delay(interval);
      calcularAngulos(i, 1, 80, 120, 38);
      delay(interval);
    }
  }
  fundamental();
}

void calibrar(){
  calcularAngulos(1, 0, 115, 0, 100);
  calcularAngulos(2, 1, 115, 0, 100);
  calcularAngulos(3, 1, 115, 0, 100);
  calcularAngulos(4, 0, 115, 0, 100);
}

void fundamental(){
  calcularAngulos(1, 0, 110, 20, 50);
  calcularAngulos(2, 1, 110, 20, 50);
  calcularAngulos(3, 0, 110, 20, 50);
  calcularAngulos(4, 1, 110, 20, 50);
}

// Función para calcular los ángulos de una pata, flag 0 es sin 180 grados. i numero de pata
void calcularAngulos(int i, int flag, double ejeX, double ejeY, double desfaceZ) {
  // ... cálculos de cinemática inversa
  i = i - 1;
  hipoXY = HipoXY(ejeX, ejeY);
  lineaAlfa = LineaA(desfaceZ, hipoXY, COXA);
  
  gama = Gamma(ejeY, ejeX);
  alfa = Alfa(desfaceZ, lineaAlfa, TIBIA, FEMUR);
  beta = Beta(lineaAlfa, TIBIA, FEMUR);

  // movimiento de la pata
  if (flag == 0){
    patas[i].coxa.write(gama + 90);
    patas[i].femur.write(alfa);
    patas[i].tibia.write(map(beta, 0, 180, 180, 0));
  } else {
    patas[i].coxa.write(180 - (gama + 90));
    patas[i].femur.write(180 - alfa);
    patas[i].tibia.write(180 - map(beta, 0, 180, 180, 0) );
  }
}

double HipoXY(double X,double Y){
  double hipo = sqrt(pow(X,2) + pow(Y,2));
  return hipo;
}

double LineaA(double d_Z, double h_XY, double cox){
  double linea_a = sqrt(  pow(d_Z,2) + pow(h_XY - cox,2) );
  return linea_a;
}

double Gamma(double Y, double X){
  double gam = atan(Y/X) * RAD_TO_DEG;
  return gam;
}

double Alfa(double d_Z, double l_A, double tibia, double femur){
  double alf1 = acos(d_Z/l_A) * RAD_TO_DEG;
  double alf2 = acos(  (pow(tibia,2)-pow(femur,2)-pow(l_A,2))   /   (-2*femur*l_A)  ) * RAD_TO_DEG;
  double alf = alf1 + alf2;
  return alf;
}

double Beta(double l_A, double tibia, double femur){
  beta = acos(  (pow(l_A,2)-pow(tibia,2)-pow(femur,2)  ) / (-2*tibia*femur)   ) * RAD_TO_DEG;
  return beta;
}