// Pines en ESP32
const int stepPin = 18;
const int dirPin = 19;
const int switchTest = 23; 

// Variables de control
float Vel = 0;  // Velocidad en mm/s
float Dist = 0; // Distancia en mm
float vueltas = 0; // Vueltas del motor para alcanzar la distancia
float RPS = 0; // Revoluciones por segundo del motor
float velocidadAngularRPS = 0; // Velocidad angular del eje en RPS
float tiempoRecorrido = 0; // Tiempo necesario en segundos
int pasos = 0;
int delayMicrosegundos = 1000;
bool sentido = HIGH;
bool nuevaOrden = false;

// Parámetros físicos
const float diametroEje = 10.0;  // mm (Diámetro del eje del motor)
const float pi = 3.1416;
const float circunferenciaEje = pi * diametroEje; // mm por vuelta del eje
const float diametroEngrane = 7.0;  // mm (Diámetro del engranaje del motor)
const float circunferenciaMotor = pi * diametroEngrane;  // mm por vuelta del motor
const int pasosPorVuelta = 200;  // Pasos completos por vuelta (NEMA 17)
const float mmPorPaso = circunferenciaMotor / pasosPorVuelta; // mm avanzados por paso

void setup() {
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(switchTest, INPUT_PULLUP);

  Serial.begin(115200);
  Serial.println("🔄 Sistema Iniciado. Ingrese Velocidad y Distancia.");
}

void loop() {
  while (true) {
    Serial.println("➡ Ingrese: distancia(mm) velocidad(mm/s) dirección(0=CCW, 1=CW)");
    Serial.println("✅ Ejemplo: 100 50 1 (ENTER para confirmar)");
    Serial.println("🔘 Ingrese 'Y' para ejecutar la última orden.");

    while (!Serial.available());
    String comando = Serial.readStringUntil('\n'); 
    comando.trim(); 

    if (comando.equalsIgnoreCase("Y")) {
      if (nuevaOrden) {
        Serial.println("🔵 Ejecutando orden almacenada...");
        moverMotor(pasos, delayMicrosegundos, sentido);
      } else {
        Serial.println("⚠️ No hay valores válidos almacenados. Ingrese primero distancia y velocidad.");
      }
      continue;
    } 

    float distancia, velocidad;
    int dir;
    char inputBuffer[32];
    comando.toCharArray(inputBuffer, 32);

    if (sscanf(inputBuffer, "%f %f %d", &distancia, &velocidad, &dir) == 3) {
      if (distancia > 0 && velocidad > 0 && (dir == 0 || dir == 1)) {
        Dist = distancia;
        Vel = velocidad;
        pasos = Dist / mmPorPaso; 
        vueltas = Dist / circunferenciaMotor; 
        RPS = Vel / circunferenciaMotor; 
        velocidadAngularRPS = Vel / circunferenciaEje; 
        tiempoRecorrido = Dist / Vel; 
        delayMicrosegundos = constrain((1000000 * mmPorPaso) / Vel, 300, 4000);
        sentido = (dir == 1) ? HIGH : LOW;
        nuevaOrden = true;

        Serial.println("✅ Nueva orden almacenada:");
        Serial.print("📏 Distancia: "); Serial.print(Dist); Serial.print(" mm | ");
        Serial.print("⚡ Velocidad lineal: "); Serial.print(Vel); Serial.print(" mm/s | ");
        Serial.print("🔄 Dirección: "); Serial.println(dir ? "Horario" : "Antihorario");
        Serial.print("🔄 Vueltas del motor: "); Serial.print(vueltas); Serial.println(" vueltas.");
        Serial.print("⚙️ Revoluciones del motor por segundo (RPS): "); Serial.print(RPS); Serial.println(" RPS.");
        Serial.print("🔁 Velocidad angular del eje: "); Serial.print(velocidadAngularRPS); Serial.println(" RPS.");
        Serial.print("⏱ Tiempo estimado de recorrido: "); Serial.print(tiempoRecorrido); Serial.println(" segundos.");
        Serial.println("🔘 Ingrese 'Y' para ejecutar la orden almacenada.");
      } else {
        Serial.println("⚠️ Datos fuera de rango. Use formato correcto: distancia velocidad direccion (0 o 1).");
      }
    } else {
      Serial.println("⚠️ Entrada inválida. Use solo números y espacios en el formato correcto.");
    }
  }
}

void moverMotor(int totalPasos, int delayUs, bool sentido) {
  digitalWrite(dirPin, sentido);
  Serial.print("➡ Sentido: ");
  Serial.println(sentido ? "Horario" : "Antihorario");

  for (int i = 0; i < totalPasos; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delayUs);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delayUs);
  }

  Serial.println("🏁 Movimiento finalizado.");
  Serial.print("🔄 Vueltas completadas: "); Serial.print(vueltas); Serial.println(" vueltas.");
  Serial.print("⚙️ Velocidad angular del eje: "); Serial.print(velocidadAngularRPS); Serial.println(" RPS.");
  Serial.print("🚀 Revoluciones del motor por segundo: "); Serial.print(RPS); Serial.println(" RPS.");
}