#include <AccelStepper.h>

// Configuración de los pines para los motores
#define MOTOR1_DIR_PIN 11
#define MOTOR1_STEP_PIN 12
#define MOTOR2_DIR_PIN 9
#define MOTOR2_STEP_PIN 10
#define MOTOR3_DIR_PIN 7
#define MOTOR3_STEP_PIN 8
#define MOTOR4_DIR_PIN 5
#define MOTOR4_STEP_PIN 6
#define PAUSE_BUTTON_PIN 2  // Pin para el botón de pausa/reanudar
#define RESET_BUTTON_PIN 3  // Pin para el botón de reset de motores

const int stepsPerRevolution = 400;   // Número de pasos necesarios para una revolución
const float speedRPS = 3.0;           // Velocidad en revoluciones por segundo (RPS)

// Crear instancias de motores utilizando la librería AccelStepper
AccelStepper motor1(AccelStepper::DRIVER, MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);
AccelStepper motor2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);
AccelStepper motor3(AccelStepper::DRIVER, MOTOR3_STEP_PIN, MOTOR3_DIR_PIN);
AccelStepper motor4(AccelStepper::DRIVER, MOTOR4_STEP_PIN, MOTOR4_DIR_PIN);

// Configuración inicial de los motores
void setupMotors() {   
    motor1.setMaxSpeed(speedRPS * stepsPerRevolution); motor1.setAcceleration(1000);
    motor2.setMaxSpeed(speedRPS * stepsPerRevolution); motor2.setAcceleration(1000);
    motor3.setMaxSpeed(speedRPS * stepsPerRevolution); motor3.setAcceleration(1000);
    motor4.setMaxSpeed(speedRPS * stepsPerRevolution); motor4.setAcceleration(1000);
}
// Función que espera a que terminen los movimientos de motores específicos
void waitForMotors(AccelStepper &m1, AccelStepper &m2, AccelStepper &m3, AccelStepper &m4) {
    while (m1.distanceToGo() != 0 || m2.distanceToGo() != 0 || m3.distanceToGo() != 0 || m4.distanceToGo() != 0) {
        if (digitalRead(PAUSE_BUTTON_PIN) == HIGH) { // Comprobar si el botón de pausa se presiona
            while (digitalRead(PAUSE_BUTTON_PIN) == HIGH); // Esperar hasta que el botón sea liberado
        }
        m1.run();
        m2.run();
        m3.run();
        m4.run();
    }
}
// Función que espera a que terminen los movimientos de motores específicos
void resetForMotors(AccelStepper &m1, AccelStepper &m2, AccelStepper &m3, AccelStepper &m4) {
    while (m1.distanceToGo() != 0 || m2.distanceToGo() != 0 || m3.distanceToGo() != 0 || m4.distanceToGo() != 0) {
        if (digitalRead(RESET_BUTTON_PIN) == HIGH) { // Comprobar si el botón de pausa se presiona
                    while (digitalRead(RESET_BUTTON_PIN) == HIGH); // Esperar hasta que el botón sea liberado
            // Resetear todos los motores a la posición cero
            motor1.moveTo(0);
            motor2.moveTo(0);
            motor3.moveTo(0);
            motor4.moveTo(0);
        }
        m1.run();
        m2.run();
        m3.run();
        m4.run();
    }
}

//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// **PREPrimera secuencia**: Motor 1 ajusta su posición para giro de 90 grados
// El mnotor del cabezal suponemos obset a 0 grados (Ya esta en psoición)
void PREfirstSequence() {
    const int steps   = 530; // 5.3 cm de avance en teoria
    motor1.moveTo(motor1.currentPosition() + steps);
    waitForMotors(motor1, motor2, motor3, motor4);
    resetForMotors(motor1,motor2,motor3, motor4);
}

// **Primera secuencia**: Motor 1 y Motor 3 permite adesión de fibra a 90 grados 
void firstSequence() {
    const int totalRevolutions1 = 8; // Revoluciones para el motor 1
    float motor1SpeedRPS = 0.075;     // Velocidad personalizada en RPs
    int step1 = totalRevolutions1 * stepsPerRevolution;

    // Calcular la velocidad del motor 3 para que complete una revolución en el tiempo de motor 1
    const int totalRevolutions3 = 53; // Revoluciones para el motor 1
    float motor3SpeedRPS = 0.5;
    int step3 = totalRevolutions3 * stepsPerRevolution;

    // Configurar velocidad y aceleración de los motores
    motor1.setMaxSpeed(motor1SpeedRPS * stepsPerRevolution);
    motor1.setAcceleration(1000);
    motor3.setMaxSpeed(motor3SpeedRPS * stepsPerRevolution);
    motor3.setAcceleration(1000);

    // Configurar movimiento inicial para motor 1
    motor1.moveTo(motor1.currentPosition() + step1);
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);
    resetForMotors(motor1,motor2,motor3, motor4);

delay(1000);
    // Movimiento 2: Motor 1 regresa al inicio
    motor1.moveTo(motor1.currentPosition() - step1);
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);
    resetForMotors(motor1,motor2,motor3, motor4);
}

// **PRESegunda secuencia**: Motor 1 y 2 ajusta su posición para giro de 15 yu primera pasada de giro de 15
void PREsecondSequence() {
    float motor1SpeedRPS = 0.9;     // Velocidad personalizada en RPS
    int step1 = 530;                // Zonas de esquinas x2 más palana

    float motor2SpeedRPS = 3.0;     // Velocidad personalizada en RPS
    const int totalRevolutions2 = 8; // Revoluciones para el motor 2
    int step2 = totalRevolutions2 * stepsPerRevolution;  // Paso para el motor 2
    float motor4SpeedRPS = 0.5;     // Velocidad personalizada en RPS
    int step4 = 17;


    motor1.moveTo(motor1.currentPosition() - step1);
    motor2.moveTo(motor2.currentPosition() + step2);
    waitForMotors(motor1,motor2,motor3, motor4);
}


void secondSequence() {
float motor1SpeedRPS = 0.9;     // Velocidad personalizada en RPS
float motor3SpeedRPS = 0.075;   // Velocidad personalizada en RPS
float motor2SpeedRPS = 2.0;     // Velocidad personalizada en RPS
float motor4SpeedRPS = 0.5;     // Velocidad personalizada en RPS
int step1 = 530;                // Zonas de esquinas x2 más palan
int step2 = 8*400;  // Paso para el motor 2
int step3= 40;     // Pasos del deposito 
int step4=17;      // Cabezal

// CONFIGURACIÓN DE VELOCIDADES : 
motor1.setMaxSpeed(motor1SpeedRPS * stepsPerRevolution);
motor1.setAcceleration(1000);
motor2.setMaxSpeed(motor2SpeedRPS * stepsPerRevolution);
motor2.setAcceleration(1000);
motor3.setMaxSpeed(motor3SpeedRPS * stepsPerRevolution);
motor3.setAcceleration(1000);
motor4.setMaxSpeed(motor4SpeedRPS * stepsPerRevolution);
motor4.setAcceleration(1000);

//gIRO CABEZAL
step3= 80;

motor3.moveTo(motor3.currentPosition() + step3);
motor4.moveTo(motor4.currentPosition() + step4);
    waitForMotors(motor1,motor2,motor3, motor4);

//ZONA 1
step3= 40;  
    motor1.moveTo(motor1.currentPosition() - step1);
    motor2.moveTo(motor2.currentPosition() - step2);
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);
//zONA 2
step3 =140;
    motor1.moveTo(motor1.currentPosition() - step1);  
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);

//zONA 3
step3= 40; 
    motor1.moveTo(motor1.currentPosition() - step1);
    motor2.moveTo(motor2.currentPosition() + step2);
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);
//Transición 1
step3= 80;
    motor3.moveTo(motor3.currentPosition() + step3);
    motor4.moveTo(motor4.currentPosition() - step4);
    waitForMotors(motor1,motor2,motor3, motor4);

// Transición 2

step3= 120;
    motor3.moveTo(motor3.currentPosition() + step3);
    motor4.moveTo(motor4.currentPosition() - step4);
    waitForMotors(motor1,motor2,motor3, motor4);

// Zona 3 Vuelta 
step3= 40;     // Pasos del deposito 
    motor1.moveTo(motor1.currentPosition() + step1);
    motor2.moveTo(motor2.currentPosition() + step2);
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);
//zONA 2 vuelta
step3 =140;     // Pasos del deposito 
    motor1.moveTo(motor1.currentPosition() + step1);      
    motor3.moveTo(motor3.currentPosition() + step3);
    waitForMotors(motor1,motor2,motor3, motor4);

//zONA 3
step3= 40; 
    motor1.moveTo(motor1.currentPosition() + step1);
    motor2.moveTo(motor2.currentPosition() - step2);
    motor3.moveTo(motor3.currentPosition() + step3);    
    waitForMotors(motor1,motor2,motor3, motor4);
//Transición 1 Vuelta
step3= 80;
    motor3.moveTo(motor3.currentPosition() + step3);
    motor4.moveTo(motor4.currentPosition() + step4);
    waitForMotors(motor1,motor2,motor3, motor4);

}
//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------


//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// Configuración inicial
void setup() {
    pinMode(PAUSE_BUTTON_PIN, INPUT); // Configurar el pin del botón como entrada
    setupMotors();
}

int state = 0;
int previousState = -1;
int repetitionCount = 0;

const int totalRepetitions[] = {1,1, 0, 0};
void (*sequences[])() = {PREfirstSequence, firstSequence, PREsecondSequence, secondSequence};

void loop() {
    if (digitalRead(PAUSE_BUTTON_PIN) == HIGH) {
        while (digitalRead(PAUSE_BUTTON_PIN) == HIGH);
    }

     if (digitalRead(RESET_BUTTON_PIN) == HIGH) {
        while (digitalRead(RESET_BUTTON_PIN) == HIGH);
    }


    if (state != previousState) {
        previousState = state;
    }

    if (repetitionCount < totalRepetitions[state]) {
        sequences[state]();
        repetitionCount++;
    } else {
        repetitionCount = 0;
        state++;
        if (state >= sizeof(totalRepetitions) / sizeof(totalRepetitions[0])) {
            while (true);
        }
    }
}