// Librerias incluidas. ////////////////////////////////////////////////////////////

#include <Wire.h>
#include <Keypad_I2C.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <LiquidCrystal_I2C.h>

//----------------------------------------------------------------------------------

// Definicionese de los pines.//////////////////////////////////////////////////////

#define ledW 13
#define LIMIT_SWITCH_PIN A1

// Motores con libreria AccelStepper y sin libreria.
AccelStepper stepperX(AccelStepper::DRIVER, 2, 5);
const int enablePinX = 9;

AccelStepper stepperY(AccelStepper::DRIVER, 3, 6);
const int enablePinY = 8;

AccelStepper stepperZ(AccelStepper::DRIVER, 4, 7);
const int enablePinZ = 8;

//----------------------------------------------------------------------------------

// Función para convertir milímetros a pasos.///////////////////////////////////////
int milimetrosAPasos(float milimetros, int pasosPorVuelta) 
{
    return milimetros / (2.35 / pasosPorVuelta);
}

int girarGrados(float grados, int pasosPorVueltaGrados) 
{
    return grados / (360.0 / pasosPorVueltaGrados);
}

//----------------------------------------------------------------------------------

// Contadores: int, float //////////////////////////////////////////////////////////

int copias = 1;
float inicio = 0.0, final = 0.0, largocurva = 0.0;
float gradosA = 0.0, gradosB = 0.0;
float GradoBrazoA = 0, GradoBrazoB = 0;
int contador = 0;
float ContadorGiroBrazo = 0.0;

int pasosPorVuelta = 200;
int pasosPorVueltaGrados = 600;
int pasosPorVueltaGradosBrazo = 16000;

// punto de inicio de los ciclos, final Homing //////////////////////////////////////////////////////////
int PasosRetrocesoHoming = 1345; //

float diametro = 0.0;

//----------------------------------------------------------------------------------

// Prototipos de funciones
void procesarLineaDeDatos(String linea);  // Nueva función para procesar la línea de datos
void calcularGradosA(float largocurva);   // Prototipo para recalcular gradosA
void opcion_b();
void opcion_c();
void opcion_d();
void configuraciones();
void ejecutarOpcion(char opcion);
void opcion_inicio();
void opcion_final();
void inicio_ciclo();
void homing();
void stepperYgo();
void stepperYback();
void stepperAClock();
void stepperAClockWise();
void moverMotorY(AccelStepper &motor, int pasos);
void realizarCurva(AccelStepper &motor, int pasos);
void moverMotorZ(AccelStepper &motor, int pasos);
bool teclaValida(char tecla);
//----------------------------------------------------------------------------------

// Configuracion Teclado Keypad.//////////////////////////////////////////////////////////////////
// Teclado Keypad.//////////////////////////////////////////////////////////////////

const byte ROWS = 4;          
const byte COLS = 4;

char opcion_seleccionada = ' ';
char keys[ROWS][COLS] =
{
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {0, 1, 2, 3};
byte colPins[COLS] = {4, 5, 6, 7};

// Direccion del Keypad.////////////////////////////////////////////////////////////////
Keypad_I2C teclado = Keypad_I2C(makeKeymap(keys), rowPins, colPins, ROWS, COLS, 0x20);
// Direccion del LCD.
LiquidCrystal_I2C lcd(0x27, 16, 2);

//----------------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////////
int menu_nivel = 0;
int tecla = 0;

////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
void setup()
{ 
    // Inicio Keypad y LCD
    teclado.begin();
    lcd.init();
    lcd.backlight();

    pinMode(ledW, OUTPUT);
    digitalWrite(ledW, HIGH);
    pinMode(LIMIT_SWITCH_PIN, INPUT_PULLUP);

    // Motores con AccelStepper
    setupMotor(stepperX, enablePinX, 10000, 1e9);
    setupMotor(stepperY, enablePinY, 10000, 1e9);
    setupMotor(stepperZ, enablePinZ, 10000, 1e9);

    Serial.begin(9600);  // Inicializa la comunicación serial
}

void setupMotor(AccelStepper &stepper, int enablePin, int maxSpeed, int acceleration) {
    pinMode(enablePin, OUTPUT);
    digitalWrite(enablePin, HIGH);
    stepper.setMaxSpeed(maxSpeed);
    stepper.setAcceleration(acceleration);
    stepper.setCurrentPosition(0);
}

////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////

void loop()
{
    lcd.clear();

    // Verificar si hay datos disponibles en el puerto Serial
    if (Serial.available() > 0) {
        // Leer la línea completa de datos del Serial, hasta el salto de línea
        String lineaDatos = Serial.readStringUntil('\n');
        
        // Limpiar la línea de cualquier carácter extra (como el ';')
        lineaDatos.trim();  // Elimina espacios o saltos de línea al inicio y al final
        
        // Mostrar el mensaje en el LCD
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Procesando Datos");

        // Procesar la línea de datos si tiene contenido
        if (lineaDatos.length() > 0) {
            procesarLineaDeDatos(lineaDatos);  // Llamada a la función que procesa los datos
        }
    }

    if (menu_nivel == 0) {
        lcd.setCursor(0, 0);
        lcd.print("A:In/Fi  B:g a/b");
        lcd.setCursor(0, 1);
        lcd.print("C:Curva  D:Copia");

        tecla = 0;
        while (tecla == 0) {
            tecla = teclado.getKey();
            if (teclaValida(tecla)) {
                opcion_seleccionada = tecla;
                menu_nivel = 1;
            }
        }
    }

    if (opcion_seleccionada != ' ') {
        ejecutarOpcion(opcion_seleccionada);
    }
}

////////////////////////////////////////////////////////////////////////////////////
// Procesar la línea de datos recibida por serial y asignar a inicio, largocurva y final
void procesarLineaDeDatos(String linea) {
    int firstComma = linea.indexOf(',');
    int secondComma = linea.indexOf(',', firstComma + 1);
    int thirdComma = linea.indexOf(',', secondComma + 1);

    if (firstComma != -1 && secondComma != -1 && thirdComma != -1) {
        inicio = linea.substring(0, firstComma).toFloat();
        diametro = linea.substring(firstComma + 1, secondComma).toFloat();
        largocurva = linea.substring(secondComma + 1, thirdComma).toFloat();
        final = linea.substring(thirdComma + 1).toFloat();

        Serial.print("Inicio: ");
        Serial.println(inicio);
        Serial.print("Diametro: ");
        Serial.println(diametro);
        Serial.print("Largocurva: ");
        Serial.println(largocurva);
        Serial.print("Final: ");
        Serial.println(final);

        // Ajusta gradosA automáticamente en base al diámetro
        calcularGradosA(diametro);

        // Ejecuta el ciclo después de recibir la línea de datos
        inicio_ciclo();
    } else {
        Serial.println("Error: formato incorrecto en la línea de datos.");
    }
}


////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////

bool teclaValida(char tecla) {
    return tecla == 'A' || tecla == 'B' || tecla == 'C' || tecla == 'D' || tecla == '*' ||
           tecla == '5' || tecla == '4' || tecla == '6' || tecla == '3' || tecla == '2' ||
           tecla == '8' || (tecla >= '0' && tecla <= '9');
}


void ejecutarOpcion(char opcion) {
    switch (opcion) {
        case 'A': opcion_inicio(); break;
        case 'B': opcion_b(); break;
        case 'C': opcion_c(); break;
        case 'D': opcion_d(); break;
        case '*': inicio_ciclo(); break;
        case '5': homing(); break;
        case '6': stepperYgo(); break;
        case '4': stepperYback(); break;
        case '3': opcion_GiroBrazoA(); break; 
        case '2': stepperAClock(); break;
        case '8': stepperAClockWise(); break;
        default: configuraciones(); break;
    }
}

//----------------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
// Funciones de las diferentes ventanas de menus ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////

void opcion_inicio()
{
    inicio = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("Inicio:");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            inicio = (inicio * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }
    opcion_final();
}

void opcion_final()
{
    final = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("Final:");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            final = (final * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }
    menu_nivel = 0;
}

void opcion_GiroBrazoA() {
    GradoBrazoA = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("G_Brazo_+: ");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            GradoBrazoA = (GradoBrazoA * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }

    // Al presionar '#', pasa a la siguiente función para capturar el valor de GradoBrazoB
    opcion_GiroBrazoB();
}

void opcion_GiroBrazoB() {
    GradoBrazoB = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("G_Brazo_-: ");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            GradoBrazoB = (GradoBrazoB * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }

    // Vuelve al menú principal
    menu_nivel = 0;
}


void opcion_b() {
    gradosA = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("Grados_A: ");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            gradosA = (gradosA * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }
    opcion_b2();
}

void opcion_b2() {
    gradosB = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("Grados_B:");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            gradosB = (gradosB * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }
    // Vuelve al menú principal
    menu_nivel = 0;
}

void opcion_c() {
    diametro = 0;  // Variable para almacenar el diámetro
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("Diametro: ");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            diametro = (diametro * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }

    // Calcula gradosA automáticamente usando el diámetro ingresado
    calcularGradosA(diametro);

    // Calcula el largo de la curva como una semicircunferencia
    largocurva = (diametro * 3.1416) / 2;

    // Imprime valores en el Serial para confirmar
    Serial.print("Diametro ingresado: ");
    Serial.println(diametro);
    Serial.print("Largo Curva calculado: ");
    Serial.println(largocurva);
    Serial.print("GradosA ajustado: ");
    Serial.println(gradosA);

    // Vuelve al menú principal después de calcular ambos valores
    menu_nivel = 0;
}





void opcion_d() {
    copias = 0;
    tecla = 0;
    lcd.clear();
    lcd.home();
    lcd.print("COPIAS:");
    lcd.setCursor(3, 1);
    lcd.print("'#'Aceptar");

    lcd.setCursor(11, 0);
    while (tecla != '#') {
        tecla = teclado.getKey();
        if (tecla != NO_KEY && (tecla >= '0' && tecla <= '9')) {
            copias = (copias * 10) + tecla - 48;
            lcd.print(tecla - 48);
        }
    }
    // Vuelve al menú principal
    menu_nivel = 0;
}
//---------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------
void inicio_ciclo() {
    // Actualiza el LCD para mostrar el inicio del ciclo
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Ciclo: Iniciando");

    int pasosinicio = milimetrosAPasos(inicio, pasosPorVuelta);
    int pasosfinal = milimetrosAPasos(final, pasosPorVuelta);
    int pasoslargocurva = milimetrosAPasos(largocurva, pasosPorVuelta);
    int pasosgradosA = girarGrados(gradosA, pasosPorVueltaGrados);
    int pasosgradosB = girarGrados(gradosB, pasosPorVueltaGrados);
    int PasosGradoBrazoA = girarGrados(GradoBrazoA, pasosPorVueltaGradosBrazo);
    int PasosGradoBrazoB = girarGrados(GradoBrazoB, pasosPorVueltaGradosBrazo);

    lcd.setCursor(0, 1);
    lcd.print("Trabajando...");

    digitalWrite(enablePinX, LOW);  // Habilitar el motor

    contador = 1;
    for (int i = 0; i < copias; i++) {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Copia: ");
        lcd.print(contador);

        // Mueve el motor Y (lado A)
        lcd.setCursor(0, 1);
        lcd.print("Moviendo Lado A");
        moverMotorY(stepperY, pasosinicio);

        // Realizar la curva (Bending)
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Curva/Bending");
        realizarCurva(stepperX, pasosgradosA);

        // Mueve el motor Y (curva)
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Haciendo Curva");
        moverMotorY(stepperY, pasoslargocurva);

        // Bending Back "Homing"
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Bending Back");
        homing();

        // Mueve el motor Y (lado B)
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Moviendo Lado B");
        moverMotorY(stepperY, pasosfinal);

        // Giro del brazo A
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Giro Brazo A");
        moverMotorZ(stepperZ, PasosGradoBrazoA);

        // Giro del brazo B
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Giro Brazo B");
        moverMotorZ(stepperZ, -PasosGradoBrazoB);

        contador++;
    }

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Ciclo: Terminado");
    delay(2000);
    menu_nivel = 0;
    digitalWrite(enablePinX, LOW);  // Habilitar el motor
}

//---------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------

void configuraciones() {
    lcd.clear();
    digitalWrite(ledW, LOW);
    lcd.home();
    lcd.setCursor(0, 0);
    lcd.print("I:");
    lcd.setCursor(2, 0);
    lcd.print(inicio);

    lcd.setCursor(8, 0);
    lcd.print("Lc:");
    lcd.setCursor(11, 0);
    lcd.print(largocurva);

    lcd.setCursor(0, 1);
    lcd.print("G:");
    lcd.setCursor(2, 1);
    lcd.print(gradosA);

    lcd.setCursor(6, 1);
    lcd.print("g:");
    lcd.setCursor(8, 1);
    lcd.print(gradosB);

    lcd.setCursor(12, 1);
    lcd.print("C:");
    lcd.setCursor(14, 1);
    lcd.print(copias);

    delay(2000);
    digitalWrite(ledW, HIGH);
    menu_nivel = 0;
}

void moverMotorX(int pasos) {
    stepperX.setMaxSpeed(10000);  // Velocidad máxima para la búsqueda del homing
    stepperX.setAcceleration(8000);  // Aceleración para la búsqueda
    stepperX.setCurrentPosition(0);  // Reinicia la posición actual del motor
    pinMode(LIMIT_SWITCH_PIN, INPUT_PULLUP);
    digitalWrite(enablePinX, LOW);  // Habilitar el motor

    stepperX.moveTo(-pasos);
    while (stepperX.distanceToGo() != 0) {
        stepperX.run();  // Mueve el motor X
    }

    digitalWrite(enablePinX, HIGH);  // Deshabilitar el motor cuando haya terminado
    delay(500);
}

void moverMotorY(AccelStepper &motor, int pasos) {
    motor.setMaxSpeed(30000);
    motor.setAcceleration(16000);
    motor.setCurrentPosition(0);
    digitalWrite(enablePinY, LOW);

    motor.moveTo(pasos);
    while (motor.distanceToGo() != 0) {
        motor.run();
    }

    digitalWrite(enablePinY, HIGH);
    delay(500);
}

void moverMotorZ(AccelStepper &motor, int pasos) {
    motor.setMaxSpeed(30000);
    motor.setAcceleration(16000);
    motor.setCurrentPosition(0);
    digitalWrite(enablePinZ, LOW);

    motor.moveTo(pasos);
    while (motor.distanceToGo() != 0) {
        motor.runToPosition();
    }

    digitalWrite(enablePinZ, HIGH);
}

void realizarCurva(AccelStepper &motor, int pasos) {
    motor.setMaxSpeed(1000);
    motor.setAcceleration(1000);
    motor.setCurrentPosition(0);
    digitalWrite(enablePinX, LOW);

    motor.moveTo(pasos);
    while (motor.distanceToGo() != 0) {
        motor.runToPosition();
    }

    //digitalWrite(enablePinX, HIGH);
}




void homing() {
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("Homing...");
    pinMode(LIMIT_SWITCH_PIN, INPUT_PULLUP);
    digitalWrite(enablePinX, LOW);  // Habilita el motor X

    // Configura la velocidad y aceleración para la búsqueda del homing
    stepperX.setMaxSpeed(10000);  // Velocidad máxima para la búsqueda del homing
    stepperX.setAcceleration(8000);  // Aceleración para la búsqueda
    //stepperX.setPinsInverted(true, false, false); 
    // Mueve el motor hacia el limit switch
    stepperX.setSpeed(-4000);     // Velocidad específica para moverse hacia el limit switch
    while (digitalRead(LIMIT_SWITCH_PIN) == LOW) {
        stepperX.runSpeed();  // Mueve el motor con la velocidad definida por setSpeed()
    }

    // El motor se detiene al alcanzar el limit switch
    stepperX.stop();  // Detiene el motor
    stepperX.setCurrentPosition(0);  // Establece la posición actual en cero

    // Configura la velocidad y aceleración para el retroceso
    stepperX.setMaxSpeed(80000);  // Velocidad máxima para el retroceso
    stepperX.setAcceleration(10000);  // Aceleración para el retroceso


    stepperX.move(PasosRetrocesoHoming);  // Mueve el motor 3000 pasos hacia atrás
    while (stepperX.distanceToGo() != 0) {
        stepperX.run();  // Utiliza run() para aplicar la aceleración y la velocidad de move()
    }

    // Deshabilitar el motor
    digitalWrite(enablePinX, HIGH);  // Apaga el motor

    // Mostrar mensaje en el LCD
    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("Homing: Listo");
    delay(500);

    menu_nivel = 0;
}



void stepperYgo() {
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("Feeding...");

    stepperY.setSpeed(3000);
    digitalWrite(enablePinY, LOW);

    while (true) {
        tecla = teclado.getKey();
        stepperY.runSpeed();
        if (tecla == '6') {
            digitalWrite(enablePinY, HIGH);
            menu_nivel = 0;
            break;
        }
    }
}

void stepperYback() {
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("Feeding...");

    stepperY.setSpeed(-3000);
    digitalWrite(enablePinY, LOW);

    while (true) {
        tecla = teclado.getKey();
        stepperY.runSpeed();
        if (tecla == '4') {
            digitalWrite(enablePinY, HIGH);
            menu_nivel = 0;
            break;
        }
    }
}

void stepperAClock() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Giro AClock...");

    stepperZ.setSpeed(2000);
    digitalWrite(enablePinZ, LOW);

    while (true) {
        tecla = teclado.getKey();
        stepperZ.runSpeed();
        if (tecla == '2') {
            digitalWrite(enablePinZ, HIGH);
            menu_nivel = 0;
            break;
        }
    }
}

void stepperAClockWise() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Giro AClockWise.");

    stepperZ.setSpeed(-2000);
    digitalWrite(enablePinZ, LOW);

    while (true) {
        tecla = teclado.getKey();
        stepperZ.runSpeed();
        if (tecla == '8') {
            digitalWrite(enablePinZ, HIGH);
            menu_nivel = 0;
            break;
        }
    }
}
///////////////////////////////////////////////////////////////////////////////////
// Arrays con los diámetros y los valores correspondientes de gradosA

float diametros[] = {40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, 190, 200};
float gradosA_values[] = {1560, 1400, 1290, 1200, 1137, 1078, 1030, 1000, 970, 940, 920, 890, 882, 867, 850, 827, 809.90};

// Función para realizar la interpolación lineal
float interpolarGradosA(float diametro) {
    int n = sizeof(diametros) / sizeof(diametros[0]);
    
    // Búsqueda lineal para interpolación
    for (int i = 0; i < n - 1; i++) {
        if (diametro >= diametros[i] && diametro <= diametros[i + 1]) {
            // Interpolación lineal entre gradosA_values[i] y gradosA_values[i + 1]
            float slope = (gradosA_values[i + 1] - gradosA_values[i]) / (diametros[i + 1] - diametros[i]);
            return gradosA_values[i] + slope * (diametro - diametros[i]);
        }
    }
    // Si está fuera del rango, devolver el valor extremo más cercano
    if (diametro < diametros[0]) return gradosA_values[0];
    if (diametro > diametros[n - 1]) return gradosA_values[n - 1];
    return 0; // Error o fuera de rango
}


float calcularLongitudCurva(float diametro) {
    return (diametro * 3.1416) / 2; // Calcular la longitud de la curva
}

////////////////////////////////////////////////////////////////////////////////////
// Función para calcular GradosA en base a largocurva
/*
void calcularGradosA(float largocurva) {
    // Aquí se hace la lógica que ya tienes para asignar gradosA
    gradosA = 2374.44 - 16.54 * largocurva + 0.0642 * largocurva * largocurva - 0.0000872 * largocurva * largocurva * largocurva;

    // Asegurarse de que gradosA no sea negativo o innecesario si largocurva es 0
    if (largocurva == 0) {
        gradosA = 0;
    }

    Serial.print("GradosA recalculado: ");
    Serial.println(gradosA);
}
*/
void calcularGradosA(float diametro) {
    // Usa la función de interpolación para ajustar gradosA en base al diámetro
    gradosA = interpolarGradosA(diametro);

    // Imprime el valor recalculado de gradosA
    Serial.print("GradosA recalculado: ");
    Serial.println(gradosA);
}

//----------------------------------------------------------------------------------
// Fin del código optimizado
/*
Nuevos datos de configuraciones:
-Para una semicircunferencia de 40mm el largo curva = 62, y los GradosA = 1580,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 60mm el largo curva = 94, y los GradosA = 1290,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 80mm el largo curva = 125, y los GradosA = 1135,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 100mm el largo curva = 157, y los GradosA = 1030,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 120mm el largo curva = 188, y los GradosA = 970,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 140mm el largo curva = 220, y los GradosA = 920,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 160mm el largo curva = 252, y los GradosA = 882,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 180mm el largo curva = 283, y los GradosA = 850,. así obtenemos el inicio y final paralelos.

-Para una semicircunferencia de 200mm el largo curva = 314, y los GradosA = 825,. así obtenemos el inicio y final paralelos.
*/