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

// define del interruptor
#define SW_ 0

// ========== CONFIGURACIÓN LCD ==========
#define LCD_ADDR 0x27
#define LCD_COLS 20
#define LCD_ROWS 4
LiquidCrystal_I2C lcd(LCD_ADDR, LCD_COLS, LCD_ROWS);

// ========== DEFINICIÓN DE PINES ==========
enum MotorPins {
  DIR_PIN,    // 0
  STEP_PIN,   // 1
  ENABLE_PIN, // 2
  MS1_PIN,    // 3
  MS2_PIN,    // 4
  MS3_PIN     // 5
};

// Configuración de pines (ajustar según tu hardware)
const byte motorPins[] = {15, 2, 4, 16, 17, 5};
const byte inputPins[] = {35, 0}; // POT, SWITCH

// ========== VARIABLES GLOBALES ==========
struct MotorState {
  int direction;      // 0=CW, 1=CCW
  int microstep;      // 1, 2, 4, 8, 16
  int speed;          // ms entre pasos
  long stepCount;     // contador de pasos absolutos
  bool enabled;       // estado de habilitación
};

MotorState motor = {0, 1, 100, 0, false};

// ========== PROTOTIPOS DE FUNCIONES ==========
void initHardware();
void updateLCD();
void motorInit(const byte pins[]);
void motorEnable(bool state);
void motorMove(long steps, int speed, int microstep);
void setMicrostep(int divisor);
void emergencyStop();

// ========== SETUP ==========
void setup() {
  Serial.begin(115200);
  initHardware();
  
  // Inicialización del motor
  motorInit(motorPins);
  motorEnable(false);
  
  // Ejemplo de movimiento

  int pasos = - 400;
  int micropasos = 2;
  int delay_x_paso = 200;   // esta en microseconds x 10
   
  // Configuración inicial
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Control Motor PAP");
  
  lcd.setCursor(0, 1);
  lcd.print("Sentido: ");
  lcd.print(pasos < 0 ? "ANTIHORARIO" : "HORARIO");
  
  lcd.setCursor(0, 2);
  lcd.print("Microstep: 1/");
  lcd.print(micropasos);
  updateLCD();

  // Ejemplo de movimiento
  
  motorEnable(true);
  motorMove(pasos, micropasos, delay_x_paso);
}

// ========== LOOP ==========
void loop() {
  if(digitalRead(SW_)){
    motorMove(800, 4, 100);
  }else{
    motorMove(-800, 4, 50);
  }

}

// ========== IMPLEMENTACIÓN DE FUNCIONES ==========

void initHardware() {
  // Configurar pines de entrada
  for (size_t i = 0; i < sizeof(inputPins); i++) {
    pinMode(inputPins[i], i == 1 ? INPUT_PULLUP : INPUT);
  }
}

void updateLCD() {
  
  lcd.setCursor(0, 3);
  lcd.print("Pasos:      ");
  lcd.setCursor(0, 3);
  lcd.print("Pasos: ");
  lcd.print(motor.stepCount);
}

void motorInit(const byte pins[]) {
  for (int i = 0; i <= MS3_PIN; i++) {
    pinMode(pins[i], OUTPUT);
    digitalWrite(pins[i], LOW);
  }
  // Configuración inicial por seguridad
  digitalWrite(pins[ENABLE_PIN], HIGH); // Motor deshabilitado inicialmente
}

void motorEnable(bool state) {
  digitalWrite(motorPins[ENABLE_PIN], state ? LOW : HIGH);
  motor.enabled = state;
  Serial.println(state ? "Motor HABILITADO" : "Motor DESHABILITADO");
}

void motorMove(long steps, int microstep, int speed) {
  if (!motor.enabled) motorEnable(true);
  
  // Configurar dirección
  motor.direction = (steps < 0) ? 0 : 1;
  digitalWrite(motorPins[DIR_PIN], motor.direction);
  
  // Configurar microstepping
  setMicrostep(microstep);
  
  // Normalizar valores
  steps = abs(steps);
  speed = constrain(speed, 1, 3000); // Limitar velocidad entre 1-1000ms
  
  // Movimiento del motor
  for (long i = 0; i < steps; i++) {
    digitalWrite(motorPins[STEP_PIN], HIGH);
    delayMicroseconds(500); // Pulso mínimo de 1-2μs según datasheet
    digitalWrite(motorPins[STEP_PIN], LOW);
    
    // Actualizar contador de pasos (considerando dirección)
    motor.stepCount += (motor.direction ? -1 : 1);
    
    // Delay controla la velocidad (ajustar según necesidad)
    if (speed > 0) {
      delayMicroseconds(speed * 100); // Convertir a microsegundos
    }
    
    // Actualizar LCD cada 10 pasos para no saturar
    //if (i % 20 == 0) updateLCD();
  }
  
  updateLCD(); // Actualización final
}

void setMicrostep(int divisor) {
  // Validar divisor de microstep
  const int validDivisors[] = {1, 2, 4, 8, 16};
  bool valid = false;
  
  for (size_t i = 0; i < sizeof(validDivisors)/sizeof(validDivisors[0]); i++) {
    if (divisor == validDivisors[i]) {
      valid = true;
      break;
    }
  }
  
  if (!valid) {
    Serial.println("Divisor de microstep no válido. Usando Full Step (1)");
    divisor = 1;
  }
  
  motor.microstep = divisor;
  
  // Configurar pines MS según el divisor
  switch(divisor) {
    case 1:  // Full step
      digitalWrite(motorPins[MS1_PIN], LOW);
      digitalWrite(motorPins[MS2_PIN], LOW);
      digitalWrite(motorPins[MS3_PIN], LOW);
      break;
    case 2:  // Half step
      digitalWrite(motorPins[MS1_PIN], HIGH);
      digitalWrite(motorPins[MS2_PIN], LOW);
      digitalWrite(motorPins[MS3_PIN], LOW);
      break;
    case 4:  // Quarter step
      digitalWrite(motorPins[MS1_PIN], LOW);
      digitalWrite(motorPins[MS2_PIN], HIGH);
      digitalWrite(motorPins[MS3_PIN], LOW);
      break;
    case 8:  // Eighth step
      digitalWrite(motorPins[MS1_PIN], HIGH);
      digitalWrite(motorPins[MS2_PIN], HIGH);
      digitalWrite(motorPins[MS3_PIN], LOW);
      break;
    case 16: // Sixteenth step
      digitalWrite(motorPins[MS1_PIN], HIGH);
      digitalWrite(motorPins[MS2_PIN], HIGH);
      digitalWrite(motorPins[MS3_PIN], HIGH);
      break;
  }
  
  Serial.print("Microstep configurado a 1/");
  Serial.println(divisor);
}

void emergencyStop() {
  digitalWrite(motorPins[ENABLE_PIN], HIGH); // Deshabilitar motor
  motor.enabled = false;
  Serial.println("EMERGENCY STOP ACTIVADO");
}