#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <AccelStepper.h>
#include <ESP32Servo.h>

#define EN 18
#define SERVO_A 17
#define DIR_A 8
#define STEP_A 3
#define IOA_0 10 //En no sujeción
#define IOA_1 9 //En sujeción
#define SERVO_B 36
#define DIR_B 38
#define STEP_B 46
#define IOB_0 35 //En no sujeción
#define IOB_1 48 //En sujeción
#define SERVO_C 13
#define DIR_C 21
#define STEP_C 47
#define INF 12
#define IOC_0 37 //En no sujeción
#define IOC_1 14 //En sujeción
#define TFT_CS 1
#define TFT_DC 42
#define TFT_RST 2
#define TFT_SCK 40
#define TFT_MOSI 41
#define TFT_MISO -1 // no conectado

#define VEL_CAL 100
#define VelNema 1000 // Velocidad del motor a pasos
#define VelServo 1000 // Velocidad del servomotor

//Declaración de objetos
  Servo ServoA, ServoB, ServoC;
  AccelStepper StepperA (AccelStepper::DRIVER, STEP_A, DIR_A);
  AccelStepper StepperB (AccelStepper::DRIVER, STEP_B, DIR_B);
  AccelStepper StepperC (AccelStepper::DRIVER, STEP_C, DIR_C);

void setup() {
  Serial.begin(115200);
  //Asignación de pines por módulo
  pinMode(EN, OUTPUT);
  //Módulo A
  pinMode(SERVO_A, OUTPUT);
  pinMode(DIR_A, OUTPUT);
  pinMode(STEP_A, OUTPUT);
  pinMode(IOA_0, INPUT_PULLUP);
  pinMode(IOA_1, INPUT_PULLUP);
  //Módulo B
  pinMode(SERVO_B, OUTPUT);
  pinMode(DIR_B, OUTPUT);
  pinMode(STEP_B, OUTPUT);
  pinMode(IOB_0, INPUT_PULLUP);
  pinMode(IOB_1, INPUT_PULLUP);
  //Módulo C
  pinMode(SERVO_C, OUTPUT);
  pinMode(DIR_C, OUTPUT);
  pinMode(STEP_C, OUTPUT);
  pinMode(IOC_0, INPUT_PULLUP);
  pinMode(IOC_1, INPUT_PULLUP);
  //Módulo D
  //Asignación de servomotores
  ServoA.attach(SERVO_A);
  ServoB.attach(SERVO_B);
  ServoC.attach(SERVO_C);
  //Habilitación de motores
  digitalWrite(EN, LOW);
  Serial.println("Ingrese 1 para Empezar");
  Serial.println("Ingrese 2 para Calibrar");
  Serial.println("Ingrese 3 para Retroceder");
  Serial.println("Ingrese 4 para Emergencia");
}
int PrimeraVez = 1;
int PasosA, PasosB, PasosC, pos_actualA, pos_actualB, pos_actualC, anguloA, anguloB, anguloC;
void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available() > 0){
    char comando = Serial.read();
    if(comando == '1'){
      if(PrimeraVez == 1){
        //Calibración inicial de los módulos
        PrimeraVez = 0;
        PasosA = Calibrar_Motores(STEP_A, DIR_A, IOA_0, IOA_1);
        PasosB = Calibrar_Motores(STEP_B, DIR_B, IOB_0, IOB_1);
        PasosC = Calibrar_Motores(STEP_C, DIR_C, IOC_0, IOC_1);
        pos_actualA = 0;
        pos_actualB = 0;
        pos_actualC = 0;
        anguloA = 0;
        anguloB = 0;
        anguloC = 0;
      }
      moverA(PasosA, STEP_A, DIR_A, pos_actualA);
      moverA(PasosB, STEP_B, DIR_B, pos_actualB);
      moverA(PasosC, STEP_C, DIR_C, pos_actualC);
      moverServo(30, ServoA, anguloA);
      moverServo(150, ServoB, anguloB);
      moverServo(90, ServoC, anguloC);
      
      
    }
    else if(comando == '2'){


    }
    else if(comando == '3'){

    }
    else if(comando =='4'){

    }
  }
  delay(10); // this speeds up the simulation
}

// Función: un paso del motor
void unPaso(int STEP) {
  digitalWrite(STEP, HIGH);
  delayMicroseconds(VelNema);
  digitalWrite(STEP, LOW);
  delayMicroseconds(VelNema);
}

// Función: mover el motor a una posición dada
void moverA(int destino, int STEP, int DIR, int pos_actual) {
  int pasos = abs(destino - pos_actual);
  if (pasos == 0) {
    Serial.println("Ya estás en esa posición.");
    return;
  }
  digitalWrite(DIR, (destino > pos_actual) ? LOW : HIGH);
  for (int i = 0; i < pasos; i++) {
    unPaso(STEP);
  }
  pos_actual = destino;
  Serial.print("Posición actual del motor = ");
  Serial.println(pos_actual);
}

// Función: mover el servo al nuevo ángulo
void moverServo(int nuevoAngulo, Servo Servomotor, int angulo) {
  if (nuevoAngulo < 0 || nuevoAngulo > 180) return;
  int paso = (nuevoAngulo > angulo) ? 1 : -1;
  for (int i = angulo; i != nuevoAngulo; i += paso) {
    Servomotor.write(i);
    delayMicroseconds(VelServo);
  }
  angulo = nuevoAngulo;
  Serial.printf("Servo movido a %d°\n", angulo);
}

int Calibrar_Motores(int STEP, int DIR, int IO1, int IO2){
  int contador = 0;
  Serial.println("Buscando sensor de sujeción");
  digitalWrite(DIR, LOW); // Mueve hacia el lado lejano
  while (digitalRead(IO2) == HIGH) {
    unPaso(STEP);
  }

  // Buscar el sensor de no sujeción
  Serial.println("Buscando sensor de no sujeción");
  digitalWrite(DIR, HIGH); // Mueve hacia el motor
  while (digitalRead(IO1) == HIGH) {
    unPaso(STEP);
    contador++;
  }
  Serial.print("Longitud total del recorrido en pasos = ");
  Serial.println(contador);
  return contador;
}