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

const int stepPin = 32; // Pin para el pulso (STEP)
const int dirPin = 33;  // Pin para la dirección (DIR)

// Crea una instancia de AccelStepper para driver MDA860E
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);

// define numero de filas
const uint8_t ROWS = 4;
// define numero de columnas
const uint8_t COLS = 4;
// define la distribucion de teclas
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
/* // pines correspondientes a las filas
uint8_t colPins[COLS] = { 17, 16, 4, 2 };
// pines correspondientes a las columnas
uint8_t rowPins[ROWS] = { 23, 19, 18, 5};
*/
uint8_t colPins[COLS] = { 2, 0, 4, 16 };
// pines correspondientes a las columnas
uint8_t rowPins[ROWS] = { 17, 19, 18, 5};


// crea objeto con los prametros creados previamente
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

LiquidCrystal_I2C lcd(0x27, 20, 4);
enum Screen
{
  IntroScreen,
  MainMenu,
  MenuAngle,
  MenuDiscreet
};

Screen currentScreen = Screen::IntroScreen;

void setup() {
  //Serial.begin(9600);
 // Wire.begin;

  lcd.init();
  lcd.backlight();
  
  // Configura la velocidad máxima y la aceleración
  stepper.setMaxSpeed(1500);
  stepper.setSpeed(1000);
  stepper.setAcceleration(50); 
  //keypad.setDebounceTime(250); // Default debounce is 50 mS
}

void loop() {

  lcd.clear();
  switch(currentScreen)
  {
    case IntroScreen:
      renderIntroScreen();
      break;

    case MainMenu:
      renderMainMenu();
      break;
    
    case MenuAngle:
      renderMenuAngle();
      break;

    case MenuDiscreet:
      renderMenuDiscreet();
      break;
  }
}

void renderIntroScreen()
{
  lcd.print("Control de Corona"); // Muestra Presentación
  lcd.setCursor(0, 1);
  lcd.print("Ver:07 - 21-07-24");
  delay(2000); //Pausa para ver la versión en el display
  
  currentScreen = Screen::MainMenu;
}

void renderMainMenu()
{
  lcd.print("Ingrese una opcion");
  lcd.setCursor(0, 1);
  lcd.print("A=Avance a Angulo");
  lcd.setCursor(0, 2);
  lcd.print("B=Avance Discreto");
  


  char menuSelect = readMenuFromKeypad(); //Espera que se ingrese la opción desde el teclado
  switch (menuSelect) {
    case 'A':
      currentScreen = Screen::MenuAngle;
      break;
    
    case 'B':
      currentScreen = Screen::MenuDiscreet;
      break;
  }
}

String angle;
void renderMenuAngle()
{
  lcd.setCursor(0, 0);
  lcd.print("Ingrese Angulo");
  lcd.setCursor(0, 1);
  lcd.print("C= Clear #=Enter");
  lcd.setCursor(0, 2);
  lcd.print("*= Menu Anterior");
  
  lcd.setCursor(0, 3);
  lcd.print("Ang ingresado:" + angle);

  char option = readMenuFromKeypad();
  int angleDegrees;

  switch(option)
  {
    case 'C':
      angle = "";
      break;
    
    case '*':
      angle = "";
      currentScreen = Screen::MainMenu;
      break;

    case '#':
      angleDegrees = angle.toInt();
      angleDegrees = angleDegrees % 360;
      rotateMotor(angleDegrees);
      angle = "";
      break;
    case 'A':
      break;

    case 'B':
      break;

    case 'D':
      break;

    default:
      angle = angle + option;
  }
}

void renderMenuDiscreet()
{
lcd.setCursor(0, 0);
  lcd.print("Ingrese Opcion");
  lcd.setCursor(0, 1);
  lcd.print("A Avance --->>");
  lcd.setCursor(0,2);
  lcd.print("B Avance <<---");
  lcd.setCursor(0, 3);
  lcd.print("*=Menu Ant");
  int cuenta =2;
  char option = readMenuFromKeypad();
  int angleDegrees;
 
  switch(option)
  {
    case '*':
      angle = "";
      currentScreen = Screen::MainMenu;
      break;

    case '#':
      
      break;
    case 'A':
     //char avance = readMenuFromKeypad(); 
      if (option == 'A') {
       
       
       stepper.moveTo(cuenta);
       stepper.runToPosition();
       cuenta = cuenta+2;

      }
      
      
      break;

    case 'B':
    if (option == 'A') {
       
       
       stepper.moveTo(-cuenta);
       stepper.runToPosition();
       cuenta = cuenta-2;

      }

      break;

    case 'C':
      
      break;
    
    case 'D':
      break;

    default:
      angle = angle + option;
  }


}

void rotateMotor(int angle)
{
    // Calcula los pasos necesarios para el ángulo ingresado
  long stepsNeeded = 0; // pasos necesarios para girar el angulo ingresado por camino mas corto
  long currentsteps = stepper.currentPosition(); // Obtiene la posición actual;
  
  //*********************************************************************************
  long totalSteps = 200; //***Para prueba en wokwi ****
  //long totalSteps = 107555; //484/9*5*400 Para telescopio Goethe
  //long totalSteps = 7600; //Para motor NEMA Enrique
  //*********************************************************************************
  
  long targetsteps = calculateStepsForAngle(angle, totalSteps); // calculo de pasos necesarios para girar el angulo ingresado 

  // Muestra el resultado en el display
  lcd.clear();
  lcd.print("Angulo: ");
  lcd.print(angle);
  lcd.print("\337"); //Imprime el símbolo de grado °   

  // Mueve la corona al ángulo deseado
   if ((targetsteps - currentsteps) > (totalSteps / 2)) {
    stepsNeeded = (targetsteps -totalSteps - currentsteps);
  } else {
    stepsNeeded = targetsteps;
  }  
  stepper.moveTo(stepsNeeded);
  // Run to target position with set speed and acceleration/deceleration:
  stepper.runToPosition();

  lcd.setCursor(0, 1);
  lcd.print("Pasos: ");
  lcd.print(stepsNeeded);
  Serial.print ("    pasos target: ");
  Serial.print (targetsteps);Serial.print ("    pasos needed: ");
  Serial.print (stepsNeeded);
  
  // Espera un tiempo para mostrar la posición final (opcional)
  delay(2000); // Puedes ajustar el tiempo según tus necesidades
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Pos. final: ");
  lcd.print(stepper.currentPosition());
  lcd.setCursor(0, 1);
  lcd.print("Angulo final: ");
  lcd.print(angle);
  lcd.print("\337"); //Imprime el símbolo de grado °
  delay(2000);
}

// Función para leer desde el teclado la opción de menú
char readMenuFromKeypad() {
  String input = "";
  char key = 0;
  while (input.length() < 1) { 
   key = keypad.getKey();
     if (key) {
      input += key;
      delay(100); // Pequeña pausa para evitar lecturas duplicadas
    }
    }
    return key;
}
  

// Función para calcular los pasos necesarios para un ángulo dado
long calculateStepsForAngle(int angulo, long totalSteps) {
  // Calcula los pasos necesarios para el ángulo deseado
  // (360 grados = totalSteps pasos)
  return map(angulo, 0, 360, 0, totalSteps);

}