#include <Stepper.h>
// Pines del motor paso a paso con A4988
const int stepPin = 10;  // Pin para STEP
const int dirPin = 11;   // Pin para dirección
const int enablePin = 12; // Pin para ENABLE (opcional)
//200 pasos por revolucion ya vienen ocnfigurados 
// Pines para limit switches y señal de seguridad
const int limitSwitch1Pin = 7;//limit de adelante
const int limitSwitch2Pin = 8;//limit de atras
bool safetyActivated = false;
bool enable = false;
const int enable_sistema=9;//para simular enable_sistema
const int servo_enable;
void setup() {
  // Configurar pines del motor
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  pinMode(enable_sistema,INPUT_PULLUP);
  digitalWrite(enablePin, HIGH);  // EMPIEZA DESACTIVADO EL motor

  // Configurar pines de los limit switches y señal de seguridad
  pinMode(limitSwitch1Pin, INPUT_PULLUP);
  pinMode(limitSwitch2Pin, INPUT_PULLUP);

  Serial.begin(9600);
}

void loop() {
  bool limitSwitch1Pressed = digitalRead(limitSwitch1Pin) == LOW;
  bool limitSwitch2Pressed = digitalRead(limitSwitch2Pin) == LOW;
  safetyActivated = digitalRead(safetySignalPin) == LOW;
  enable = digitalRead(enablePin) == HIGH;
  posicion_inicial();
  if (safetyActivated) {
    Serial.println("Señal de seguridad activada. Motor apagado.");
    detenerMotor();
  }else{
    if(digitalRead(enable_sistema)== LOW){
      digitalWrite(dirPin, HIGH);  // Establecer dirección a avance
      moveMotor(200);
      if (limitSwitch1Pressed) {
        detenerMotor();
        if(safetyActivated){
          Serial.println("Señal de seguridad activada. Motor apagado.");
          detenerMotor();
        }else{
          if(servo_enable){//1 significa que ya acabo su tarea
          digitalWrite(dirPin, LOW);  // Establecer dirección a RETROCESO
          moveMotor(200);
          if(limitSwitch2Pressed){
            if(safetyActivated){
              Serial.println("Señal de seguridad activada. Motor apagado.");
              detenerMotor();
            }else{
              posicion_inicial();
            }
          }
          }else;//espera al servo
        }
      }
    }else;
  }
  if (limitSwitch1Pressed) {
    // Si se presiona el limit switch 1, retroceder
    Serial.println("Limit switch 1 presionado. Retrocediendo...");
    digitalWrite(dirPin, LOW);  // Establecer dirección a retroceso
    moveMotor(200);  // Cantidad de pasos a mover
  } else if (limitSwitch2Pressed) {
    // Si se presiona el limit switch 2, avanzar
    if (enable) {
      Serial.println("Limit switch 2 presionado. Avanzando...");
      digitalWrite(dirPin, HIGH);  // Establecer dirección a avance
      moveMotor(200);  // Cantidad de pasos a mover
    } else {
      Serial.println("Enable no activado.");
    }
  } else {
    Serial.println("Ir hasta posición inicial...");
    digitalWrite(dirPin, LOW);  // Retroceder a posición inicial
    moveMotor(200);
  }
}

// Función para mover el motor la cantidad de pasos indicada
void moveMotor(int steps) {
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);  // Controlar la velocidad
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
}

void detenerMotor() {
    digitalWrite(enablePin, HIGH); // HIGH desactiva el motor
}
void posicion_inicial(){
  if(limitSwitch2Pressed){
    while(limitSwitch2Pressed==1){ 
      digitalWrite(dirPin, HIGH);  // Establecer dirección a avance
      moveMotor(200);
    }
    detenerMotor();
  }else{
    digitalWrite(dirPin, LOW);  // Establecer dirección a avance
    moveMotor(200);
  }
}