#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <AccelStepper.h>
#include <QMC5883LCompass.h>

// Configuración del LCD I2C (dirección del LCD es usualmente 0x27)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Configuración del Keypad 4x4
const byte ROWS = 4; // 4 filas
const byte COLS = 4; // 4 columnas
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins[ROWS] = {9, 8, 7, 6}; // Conectar a los pines de las filas
byte colPins[COLS] = {5, 4, 3, 2}; // Conectar a los pines de las columnas

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Variables para azimut y altitud
String azimuth = "";
String altitude = "";

// Pasos por grado en modo 1/16 microstepping
float stepsPerDegree = 200.00/360.0;

// Variables para el modo actual (-1 = no seleccionado, 0 = azimut, 1 = altitud)
int inputMode = -1; // Inicializar en -1 para requerir selección de modo
bool canInput = true; // Nueva bandera para permitir entrada

// Motores paso a paso (Configura los pines según tus conexiones)
AccelStepper stepperAzimuth(AccelStepper::DRIVER, 10, 11);  // Motor para azimut (STEP, DIR)
AccelStepper stepperAltitude(AccelStepper::DRIVER, 12, 13);  // Motor para altitud (STEP, DIR)

// Magnetómetro QMC5883L
QMC5883LCompass compass;

void setup() {
  lcd.init();         // Iniciar el LCD
  lcd.backlight();    // Encender la retroiluminación del LCD
  lcd.setCursor(0, 0);
  lcd.print("Azimuth = A");
  lcd.setCursor(0, 1);
  lcd.print("Altitude = B");
  
  stepperAzimuth.setMaxSpeed(1000.0); // Velocidad máxima del motor de azimut
  stepperAltitude.setMaxSpeed(1000.0); // Velocidad máxima del motor de altitud
  stepperAzimuth.setAcceleration(100.0); // Aceleración máxima del motor de azimuth
  stepperAltitude.setAcceleration(100.0); // Aceleración máxima del motor de altitud

  compass.init();
  compass.setCalibrationOffsets(-146.00, -30.00, -138.00);
  compass.setCalibrationScales(0.93, 0.92, 1.19);
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    // Permitir reactivar la entrada con A o B
    if (key == 'A' || key == 'B') {
      canInput = true; // Reactivar entrada

      if (key == 'A') {
        inputMode = 0;  // Cambiar a modo Azimut
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Azimuth >");
        lcd.setCursor(0, 1);
        lcd.print("Altitude: ");
        lcd.print(altitude);
        azimuth = ""; // Limpiar la variable
      } else if (key == 'B') {
        inputMode = 1;  // Cambiar a modo Altitud
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Azimuth: ");
        lcd.print(azimuth);
        lcd.setCursor(0, 1);
        lcd.print("Altitude >");
        altitude = ""; // Limpiar la variable
      }
    } 

    if (key == 'C') {
      // Mover motor de azimut a 0 según el magnetómetro
      compass.read();
      int currentAzimuth = compass.getAzimuth();

      // Calcular pasos necesarios para mover al punto 0
      int stepsToZero = -currentAzimuth * stepsPerDegree;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Moviendo a 0");
      lcd.setCursor(0, 1);
      lcd.print("Azimuth: ");
      lcd.print(currentAzimuth);

      stepperAzimuth.moveTo(stepsToZero);

      // Mover el motor
      while (stepperAzimuth.distanceToGo() != 0) {
        stepperAzimuth.run();
      }

      delay(2000); //Espera para que la persona pueda leer

      // Obtener grados actuales exactos del motor
          float CurrentAzimuth = stepperAzimuth.currentPosition() / stepsPerDegree;
          float CurrentAltitude = stepperAltitude.currentPosition() / stepsPerDegree;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Azimuth a 0");
      delay(1000);
      lcd.clear();
      lcd.print("Azimuth: ");
      lcd.print(-CurrentAzimuth);
      lcd.setCursor(0, 1);
      lcd.print("Altitud: ");
      lcd.print(-CurrentAltitude);
    }

    if (key == 'D') {
      // Establecer la posición actual de los motores como el nuevo punto 0
      stepperAzimuth.setCurrentPosition(0);
      stepperAltitude.setCurrentPosition(0);

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Nuevo punto 0");
      delay(1000);
      lcd.clear();
      // Obtener grados actuales exactos del motor
      float CurrentAzimuth = stepperAzimuth.currentPosition() / stepsPerDegree;
      float CurrentAltitude = stepperAltitude.currentPosition() / stepsPerDegree;

      lcd.print("Azimuth: ");
      lcd.print(-CurrentAzimuth);
      lcd.setCursor(0, 1);
      lcd.print("Altitud: ");
      lcd.print(-CurrentAltitude);
    }


    if (canInput) {
      // Procesar otras entradas como antes
      if (key == '#') {
        if (inputMode != -1) { // Solo si se ha seleccionado A o B
          // Convertir azimut y altitud a enteros y mover los motores
          int azimuthValue = azimuth.toFloat() * stepsPerDegree;
          int altitudeValue = altitude.toFloat() * stepsPerDegree;
          
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Moviendo...");

          // Ajusta la conversión
          stepperAzimuth.moveTo(-azimuthValue);
          stepperAltitude.moveTo(-altitudeValue);

          // Mover los motores simultáneamente
          while (stepperAzimuth.distanceToGo() != 0 || stepperAltitude.distanceToGo() != 0) {
            stepperAzimuth.run();
            stepperAltitude.run();
          }

          // Obtener grados actuales exactos del motor
          float CurrentAzimuth = stepperAzimuth.currentPosition() / stepsPerDegree;
          float CurrentAltitude = stepperAltitude.currentPosition() / stepsPerDegree;

          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Finalizado");
          delay(1000);
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Azimuth: ");
          lcd.print(-CurrentAzimuth);
          lcd.setCursor(0, 1);
          lcd.print("Altitud: ");
          lcd.print(-CurrentAltitude);
          
          canInput = false; // Deshabilitar la entrada después del movimiento
        }
      } else if (key == '*') {
        if (inputMode == 0 && azimuth.indexOf('.') == -1) {
          azimuth += ".";
          lcd.setCursor(9, 0);
          lcd.print(azimuth);
        } else if (inputMode == 1 && altitude.indexOf('.') == -1) {
          altitude += ".";
          lcd.setCursor(9, 1);
          lcd.print(altitude);
        }
      } else if (key >= '0' && key <= '9') {
        // Solo permitir entrada numérica si se seleccionó A o B
        if (inputMode == 0) {
          azimuth += key;
          lcd.setCursor(9, 0);
          lcd.print(azimuth);
        } else if (inputMode == 1) {
          altitude += key;
          lcd.setCursor(9, 1);
          lcd.print(altitude);
        }
      }
    }
  }
}