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

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

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

// Configuración Teclado
const uint8_t ROWS = 4;
const uint8_t COLS = 4;

char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
// pines correspondientes a las columnas y filas

const uint8_t rowPins[ROWS] = { 13, 12, 11, 10};
const uint8_t colPins[COLS] =  {9, 8, 7, 6};

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() {
  Wire.begin();
  Serial.begin (9600);
    
  lcd.begin(20, 4); // Inicializa el display LCD (20 columnas x 4 filas) 
  lcd.backlight();
 
  // Configura la velocidad máxima y la aceleración
  stepper.setMaxSpeed(1500); // Ajusta la velocidad máxima según tus necesidades
  stepper.setSpeed(1000);
  stepper.setAcceleration(50); // Ajusta la aceleración según tus necesidades
  //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-29-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("Ang ingresado:" + angle);
  lcd.setCursor(0, 2);
  lcd.print("C= Clear #=Enter");
  lcd.setCursor(0, 3);
  lcd.print("*= Menu Anterior");
  
  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;
  }
}
float angleDegrees = stepper.currentPosition();
void renderMenuDiscreet()
{
angleDegrees = stepper.currentPosition();
Serial.print("angleDegrees0= ");
Serial.println(angleDegrees);
delay(500);
  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");
  
  char option = readMenuFromKeypad();
  
 
  switch(option)
  {
    case '*':
      angle = "";
      currentScreen = Screen::MainMenu;
      break;

    case '#':
      
      break;
    case 'A':
      Serial.print("angleDegrees1= ");
      Serial.println(angleDegrees);
      delay(1000);
      angleDegrees = angleDegrees *1.8 + 10.0;   //PARA MOTOR 200 pasos Wokwi***********
      //angleDegrees = angleDegrees *0.04737 + 10.0;   //PARA MOTOR NEMA******************
      //angleDegrees = angleDegrees *0.003347 + 10.0;  //PARA GOETHE**********************
      Serial.print("angleDegrees2= ");
      Serial.println(angleDegrees);
      
      rotateMotor(angleDegrees);
      Serial.print("angleDegrees3= ");
      Serial.println(angleDegrees); 
      delay(1000);   
     
      break;

    case 'B':
     
      Serial.print("angleDegrees1= ");
      Serial.println(angleDegrees);
      delay(1000);
      angleDegrees = angleDegrees *1.8 - 10;
      Serial.print("angleDegrees2= ");
      Serial.println(angleDegrees);
      rotateMotor(angleDegrees);
      Serial.print("angleDegrees3= ");
      Serial.println(angleDegrees); 
      delay(1000);      
     
      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;
  
  Serial.print("CurrentPosition en rotateMotor= ");
  Serial.println(currentsteps);
  //*********************************************************************************
  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);
  Serial.print ("    currentPosition: ");
  Serial.println (stepper.currentPosition());
    
  // Espera un tiempo para mostrar la posición final (opcional)
  delay(1000); // 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);

}