//Movimiento de corona modificado para usar Driver MDA860e y stepper motor Nema23 con caja de reduccion
//Incluye menúes para avance a ángulo y avance discreto en dos direcciones

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

// Configura los pines para el controlador al MDA860e
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);

// Usamos display I2C
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 20, 4);

// Configura el teclado matricial
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {13, 12, 11, 10}; //Conecta los pines del teclado a estos pines digitales
byte colPins[COLS] = {9, 8, 7, 6};

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

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 - 26-06-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 Angulo de avance");
  lcd.setCursor(0, 1);
  lcd.print("A=2\337   B=-2\337");
  lcd.setCursor(0,2);
  lcd.print("C=10\337  D=-10\337");
  lcd.setCursor(0, 3);
  lcd.print("*=Menu Ant #=Enter");
  
  char option = readMenuFromKeypad();
  int angleDegrees;

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

    case '#':
      
      break;
    case 'A':
      
      
      
      
      break;

    case 'B':
      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);

}