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

// Configuración básica
#define I2C_ADDR 0x27
LiquidCrystal_I2C lcd(I2C_ADDR, 20, 4);

// DIR, STEP, ENABLE, MS1, MS2, MS3
#define DIR 0
#define STEP 1
#define ENABLE 2
#define MS1 3
#define MS2 4
#define MS3 5

// Pines de conexión
byte motorPins[] = {15, 2, 4, 16, 17, 5}; // DIR, STEP, ENABLE, MS1, MS2, MS3
const byte inputPins[] = {35, 0}; // POT, SWITCH


void setup() {
  // Configurar pines
  
  pinMode(inputPins[0], INPUT);
  pinMode(inputPins[1], INPUT_PULLUP);
  
  // Inicializar motor
  Motor_PAP_Init(motorPins);
  // Iniciar LCD
  lcd.init();
  lcd.backlight();
  lcd.print("Test de Motor PAP");
  lcd.setCursor(0,1);
  lcd.print("Sentido: HORARIO");
  lcd.setCursor(0,2);
  lcd.print("nPasos : 100");
  lcd.setCursor(0,3);
  lcd.print("Velocidad : 100");
  
  Serial.begin(115200);
  Motor_PAP_Enable(false);
  Motor_PAP(-200, 2, 100);

}

void loop() {
}



// Variables de control

// Funcion para el motor PAP
// DIR, STEP, ENABLE, MS1, MS2, MS3
// void Motor_PAP_Init(byte MotorPines[6]);
// void Motor_PAP(int nPasos, int Velocidad, int divisorStep);

void Motor_PAP_Init(byte MotorPines[6]){
  for (int x =0; x < 5; x++){
    pinMode(MotorPines[x], OUTPUT);
    digitalWrite(MotorPines[x], 0);
  }
}

void Motor_PAP_Enable(bool habilitado){
  if(habilitado){
    digitalWrite(motorPins[ENABLE], HIGH);
    Serial.println("Motor Habilitado ");
  }else{
    digitalWrite(motorPins[ENABLE], LOW);
    Serial.println("Motor Deshabilitado ");
  }

}


void Motor_PAP(int nPasos, int divisorStep, int Velocidad){
  if (nPasos >= 0){ // Horario
    digitalWrite(motorPins[DIR], LOW);
    Serial.println("Sentido Horario: ");
  }else{ // Antihorario
    digitalWrite(motorPins[DIR], HIGH);
    Serial.println("Sentido AntiHorario: ");
  }
  
  Serial.print("Divisor de Paspo: ");
  Serial.println(divisorStep);

  switch(divisorStep){
     case 1:
      digitalWrite(motorPins[MS1], LOW);
      digitalWrite(motorPins[MS2], LOW);
      digitalWrite(motorPins[MS3], LOW);
      break;
    case 2:
      digitalWrite(motorPins[MS1], HIGH);
      digitalWrite(motorPins[MS2], LOW);
      digitalWrite(motorPins[MS3], LOW);
      break;
    case 4:
      digitalWrite(motorPins[MS1], LOW);
      digitalWrite(motorPins[MS2], HIGH);
      digitalWrite(motorPins[MS3], LOW);
      break;
    case 8:
      digitalWrite(motorPins[MS1], HIGH);
      digitalWrite(motorPins[MS2], HIGH);
      digitalWrite(motorPins[MS3], LOW);
      break;
    case 16:
      digitalWrite(motorPins[MS1], LOW);
      digitalWrite(motorPins[MS2], LOW);
      digitalWrite(motorPins[MS3], HIGH);
      break;
    case 32:
      digitalWrite(motorPins[MS1], HIGH);
      digitalWrite(motorPins[MS2], LOW);
      digitalWrite(motorPins[MS3], HIGH);
      break;
    case 64:
      digitalWrite(motorPins[MS1], LOW);
      digitalWrite(motorPins[MS2], HIGH);
      digitalWrite(motorPins[MS3], HIGH);
      break;
    case 128:
      digitalWrite(motorPins[MS1], HIGH);
      digitalWrite(motorPins[MS2], HIGH);
      digitalWrite(motorPins[MS3], HIGH);
      break;
    default:
      // Full step (200 pasos/vuelta)
      digitalWrite(motorPins[MS1], LOW);
      digitalWrite(motorPins[MS2], LOW);
      digitalWrite(motorPins[MS3], LOW);
      break;
  }


  for(int nVeces = 0; nVeces<abs(nPasos);nVeces++){
    digitalWrite(motorPins[STEP], HIGH);
    Serial.println("Paso 1 logico ");
    delay(Velocidad);
    digitalWrite(motorPins[STEP], LOW);
    Serial.println("Paso 0 logico ");
    delay(Velocidad);
    Serial.print("Paso Nro: ");
    Serial.println(nVeces);
  }
}



/*
void setMicrostep(byte mode) {
  microstep = (mode >= 1 && mode <= 16) ? mode : 1;
  digitalWrite(motorPins[3], microstep & 1);
  digitalWrite(motorPins[4], microstep & 2);
  digitalWrite(motorPins[5], microstep & 4);
  updateLCD();
}
*/