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

const int stepPin = 4;
const int speed_switchA = 26;
const int speed_switchB = 27;
const int stepMode = 25;
int Full_Steps_Revolution = 200;
const int Half_Steps_Revolution = Full_Steps_Revolution * 2;
int MS1 = 19;
const int Dir = 13;


const int speed1 = 50;
const int speed2 = 25;
const int speed3 = 10;
const int speed4 = 5;

const float half_step_divisor = 2.0;


LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  pinMode(stepPin, OUTPUT);
  pinMode(speed_switchA, INPUT_PULLDOWN);
  pinMode(speed_switchB, INPUT_PULLDOWN);
  pinMode(stepMode, INPUT_PULLDOWN);
  pinMode(MS1, OUTPUT);
  pinMode(Dir, INPUT);

  Serial.begin(115200);


  lcd.init();
  lcd.backlight();
}

void loop() {
  bool switchA_State = digitalRead(speed_switchA);
  bool switchB_State = digitalRead(speed_switchB);
  bool stepMode_State = digitalRead(stepMode);
  bool Dir_State = digitalRead(Dir);

  float selectedSpeed;
  String speed_Display;
  String Direction;

  if (switchA_State == LOW && switchB_State == LOW) {
    selectedSpeed = speed1;
    speed_Display = "1";
  } else if (switchA_State == LOW && switchB_State == HIGH) {
    selectedSpeed = speed2;
    speed_Display = "2";
  } else if (switchA_State == HIGH && switchB_State == LOW) {
    selectedSpeed = speed3;
    speed_Display = "3";
  } else {
    selectedSpeed = speed4;
    speed_Display = "4";
  }

  int steps_by_Revolution;
  String step_Mode_Display;

  // Modo de pasos completos:
  if (stepMode_State == LOW) {
    digitalWrite(MS1, LOW);
    steps_by_Revolution = Full_Steps_Revolution;
    step_Mode_Display = "Paso Completo";
  } else {
    // Modo de medios pasos:
    digitalWrite(MS1, HIGH);
    steps_by_Revolution = Half_Steps_Revolution;
    selectedSpeed /= half_step_divisor;
    step_Mode_Display = "Medio Paso";
  }

  if (Dir_State == LOW) {
    Direction = "| Izq";
  } else {
    Direction = "| Der";
  }

 
  Serial.print("Velocidad de Paso ");
  Serial.print(speed_Display);
  Serial.print(": ");
  Serial.print(selectedSpeed * 2);
  Serial.print(" ms | ");
  Serial.println(step_Mode_Display);


  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Vel #");
  lcd.print(speed_Display);
  lcd.print(": ");
  lcd.print(selectedSpeed * 2);
  lcd.print("ms");

  lcd.setCursor(0, 1);
  lcd.print(step_Mode_Display);


  for (int i = 0; i < steps_by_Revolution; i++) {
    digitalWrite(stepPin, HIGH);
    delay(selectedSpeed);
    digitalWrite(stepPin, LOW);
    delay(selectedSpeed);
  }

  // Calculo para el tiempo de revolución:
  unsigned long Revolution_Time = steps_by_Revolution * (selectedSpeed * 2);


  Serial.print("Tiempo de revolucion: ");
  Serial.print(Revolution_Time / 1000);
  Serial.print(" segundos | ");
  Serial.print("Cantidad de pasos realizados: ");
  Serial.println(steps_by_Revolution);
  Serial.print("Direccion de Giro: ");
  Serial.println(Direction);
  Serial.println("\n");


  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Tiempo Rev: ");
  lcd.print(Revolution_Time / 1000);
  lcd.print("s");

  lcd.setCursor(0, 1);
  lcd.print("Pasos: ");
  lcd.print(steps_by_Revolution);
  lcd.print(" ");
  lcd.print(Direction);

  delay(5000);
}