#include <AccelStepper.h>
#include <MultiStepper.h>

// Define el tipo de interfaz y los pines de control para dos motores
#define PIN_STEP0 3
#define PIN_DIR0 2
#define PIN_STEP1 20
#define PIN_DIR1 21

// Límites de ángulo para cada motor PARA LA VERIFICACION DE ENTRADA
//Motor 0.
const float anguloMin0 = -90.0;
const float anguloMax0 = 90.0;

//Motor 0.
const float anguloMin1 = -90.0;
const float anguloMax1 = 90.0;

//Motor 0.
const float anguloMin2 = -90.0;
const float anguloMax2 = 90.0;

//Declaracion de los Objetos de los Motores y la Instancia de MultiStepper.
AccelStepper stepper0(AccelStepper::DRIVER, PIN_STEP0, PIN_DIR0);
AccelStepper stepper1(AccelStepper::DRIVER, PIN_STEP1, PIN_DIR1);
MultiStepper steppers;

const float pasosPorGrado = 1.8; // Ajusta este valor a tu configuración

String inputString = ""; // Una cadena para almacenar los comandos entrantes
boolean inputComplete = false; // Para saber si se ha recibido toda la entrada

/* La logica del Codigo es sencilla, se crearon las Instancias de los Motores paso a paso
Para trabajar con el Controlador, se definieron los Pines STEP y DIR del Controlador (OJO EN LA SIMULACION ES DISTINTO, EN LA REALIDAD HAY QUE VERIFICAR LOS PINOUTS DEL SHIELD)
Se creó una funcion qué moviera los Motores en Sincronia haciendo la conversión de pasos a Grados.
Y se creo una interfaz de Comunicación con el Usuario, para que se pudiera Debuggear el codigo, y controlar el motor de manera individual.

También falta agregarle la variable por el Incremento de la fuerza, que son las poleas.
(Hice una modificacion de la Posicion de la Cintura, por una Polea de 1 a 3. Por eso se divide entre 3.)
*/
void setup() {
  Serial.begin(9600);
  //Hay que revisar los Datasheets de los motores, para calibrar la velocidad y Aceleracion de cada uno.
  stepper0.setMaxSpeed(200);
  stepper0.setAcceleration(200);
  stepper1.setMaxSpeed(200);
  stepper1.setAcceleration(200);
  
  steppers.addStepper(stepper0);
  steppers.addStepper(stepper1);

  // Imprime la configuración inicial de los motores

  Serial.println("Configuracion del stepper0 (Cintura):");
  Serial.print("Stepper0 - MAX SPEED: ");
  Serial.print(stepper0.maxSpeed());
  Serial.print(", ACCELERATION: ");
  Serial.print(stepper0.acceleration());
  Serial.print(", PIN STEP: ");
  Serial.print(PIN_STEP0);
  Serial.print(", PIN DIR: ");
  Serial.println(PIN_DIR0);
  
  Serial.println("Configuracion del stepper1(Hombro):");
  Serial.print(stepper1.maxSpeed());
  Serial.print(", ACCELERATION: ");
  Serial.print(stepper1.acceleration());
  Serial.print(", PIN STEP: ");
  Serial.print(PIN_STEP1);
  Serial.print(", PIN DIR: ");
  Serial.println(PIN_DIR1);

  Serial.println("Introduce comandos en el formato: angulo1,tipo1,angulo2,tipo2,angulo3,tipo3");
  inputString.reserve(200); // Reserva espacio para la entrada
}

void loop() {
   if (inputComplete) {
    procesarComando();
    inputString = ""; // Limpia la cadena para el próximo comando
    inputComplete = false; // Espera por una nueva entrada
  }
  // Ejemplo: Mover motor 1 a 90 grados absoluto, motor 2 a 45 grados relativo
  //moverMotorAAngulo(90, 'P', 45, 'R', 0, 'N'); // El tercer ángulo no se usa en este ejemplo
 // delay(4000); // Espera para observar el movimiento
}

void moverMotorAAngulo(float angulo1, char tipoMovimiento1, float angulo2, char tipoMovimiento2, float angulo3, char tipoMovimiento3) {
  long posiciones[2]; // Array para almacenar las posiciones objetivo, asumiendo dos motores

  // Procesa el primer ángulo y tipo de movimiento
  if (tipoMovimiento1 == 'P') {
    posiciones[0] = angulo1 / 5.289; // Posición Absoluta
  } else if (tipoMovimiento1 == 'R') {
    posiciones[0] = stepper0.currentPosition() + (angulo1 / 5.289); // Posición Relativa
  }

  // Procesa el segundo ángulo y tipo de movimiento
  if (tipoMovimiento2 == 'P') {
    posiciones[1] = angulo2 / 30; // Posición Absoluta
  } else if (tipoMovimiento2 == 'R') {
    posiciones[1] = stepper1.currentPosition() + (angulo2 / 30); // Posición Relativa
  }

  // El tercer ángulo y tipo de movimiento no se aplican directamente debido a que solo hay dos motores en este ejemplo.
  // Para aplicarlo, necesitarías un tercer motor y añadirlo al array de posiciones y al objeto MultiStepper.

  // Configura las nuevas posiciones objetivo
  steppers.moveTo(posiciones);
  steppers.runSpeedToPosition(); // Ejecuta el movimiento
}

void serialEvent() {
  while (Serial.available()) {
    char inChar = (char)Serial.read(); // Lee el caracter entrante
    inputString += inChar; // Añade el caracter a la cadena de entrada
    if (inChar == '\n') { // Si el caracter es un salto de línea, indica el fin de la entrada
      inputComplete = true;
    }
  }
}

void procesarComando() {
  // Primero, verifica si se introdujo el comando "stop"
    inputString.trim();

   if (inputString == "stop") {
    stepper0.stop(); // Detiene el motor 0
    stepper1.stop(); // Detiene el motor 1
    Serial.println("Motores detenidos.");
    return; // Sale de la función después de detener los motores
  }


  if (inputString.length() == 0) {
    Serial.println("No se introdujo ningun comando.");
    return; // Sale de la función si la entrada está vacía
  }

  // Divide la entrada en sus componentes
  int primerComa = inputString.indexOf(',');
  int segundaComa = inputString.indexOf(',', primerComa + 1);
  int tercerComa = inputString.indexOf(',', segundaComa + 1);
  int cuartaComa = inputString.indexOf(',', tercerComa + 1);
  int quintaComa = inputString.indexOf(',', cuartaComa + 1);

  if (primerComa == -1 || segundaComa == -1 || tercerComa == -1 || cuartaComa == -1 || quintaComa == -1) {
    Serial.println("Formato de comando incorrecto o incompleto.");
    return; // Sale de la función si el formato del comando no es correcto
  }


  // Extrae cada parte del comando
  float angulo1 = inputString.substring(0, primerComa).toFloat();
  char tipo1 = inputString.substring(primerComa + 1, segundaComa).charAt(0);
  float angulo2 = inputString.substring(segundaComa + 1, tercerComa).toFloat();
  char tipo2 = inputString.substring(tercerComa + 1, cuartaComa).charAt(0);
  float angulo3 = inputString.substring(cuartaComa + 1, quintaComa).toFloat();
  char tipo3 = inputString.substring(quintaComa + 1).charAt(0);

  // Verifica si los caracteres de tipo de movimiento están en blanco
  if (tipo1 == '\0' || tipo2 == '\0' || tipo3 == '\0') {
    Serial.println("Error: Uno o más tipos de movimiento no se han especificado.");
    return; // Sale de la función si falta algún tipo de movimiento
  }

  bool fueraDeRango = false; // Indicador si algún ángulo está fuera de rango

  // Verifica si los ángulos están dentro de los límites permitidos y especifica cuál está fuera de rango
  if (angulo1 < anguloMin0 || angulo1 > anguloMax0) {
    Serial.println("Error: Ángulo 1 (Motor 1) fuera de los límites permitidos.");
    fueraDeRango = true;
  }
  if (angulo2 < anguloMin1 || angulo2 > anguloMax1) {
    Serial.println("Error: Ángulo 2 (Motor 2) fuera de los límites permitidos.");
    fueraDeRango = true;
  }
  if (angulo3 < anguloMin2 || angulo3 > anguloMax2) {
    Serial.println("Error: Ángulo 3 (Motor 3) fuera de los límites permitidos.");
    fueraDeRango = true;
  }

  // Sale de la función si algún ángulo está fuera de los límites, después de especificar cuál(es)
  if (fueraDeRango) {
    return;
  }

  // Si pasa todas las validaciones, llama a la función moverMotorAAngulo
  moverMotorAAngulo(angulo1, tipo1, angulo2, tipo2, angulo3, tipo3);
}