#include <Servo.h>
#include <LowPower.h>

const int triggerPinLeft = 11;
const int echoPinLeft = 10;
const int triggerPinRight = 9;
const int echoPinRight = 8;
const int buzzerPin = 7;
const int servoUpperPin = 6;
const int servoLowerPin = 5;
const int ledPin = 4;

Servo servoUpper;
Servo servoLower;

volatile long durationLeft, durationRight;
volatile int distanceLeft, distanceRight;

// Variables para el control del tiempo del parpadeo del LED
unsigned long previousMillis = 0;
const long interval = 500;  
bool ledState = LOW;


bool buzzerActive = false;
unsigned long buzzerStartMillis = 0;


const int detectionThreshold = 100;  // Ajustable según la necesidad

// Estados del sistema
enum State {IDLE, TRAIN_APPROACHING, TRAIN_CROSSING, TRAIN_PASSED};
volatile State currentState = IDLE;

void setup() {
  // Configuración de los pines de los sensores ultrasónicos
  pinMode(triggerPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(triggerPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);

  // Configuración del pin del buzzer y LED
  pinMode(buzzerPin, OUTPUT);
  pinMode(ledPin, OUTPUT);

  // Adjuntar los servos a los pines designados
  servoUpper.attach(servoUpperPin);
  servoLower.attach(servoLowerPin);

  // Posición inicial de las barreras (abiertas)
  servoUpper.write(0);  
  servoLower.write(0);  

  
  Serial.begin(9600);

  // Configurar el Arduino en modo bajo consumo
  enterLowPowerMode();  // Inicialmente entramos en modo bajo consumo
}

void loop() {
  unsigned long currentMillis = millis();

  // Si estamos en estado IDLE, revisamos si el tren se aproxima
  distanceLeft = measureDistance(triggerPinLeft, echoPinLeft);
  if (distanceLeft < detectionThreshold && currentState == IDLE) {
    currentState = TRAIN_APPROACHING;
    buzzerStartMillis = currentMillis;  // Iniciar temporizador para el zumbador
    buzzerActive = true;
    digitalWrite(buzzerPin, HIGH);  // Activar el zumbador
    lowerBarriers();  // Bajar las barreras inmediatamente
  }

  // Gestión del estado actual del cruce de tren
  switch (currentState) {
    case TRAIN_APPROACHING:
      // Mantener el zumbador activo por 1 segundo (sin delay)
      if (buzzerActive && currentMillis - buzzerStartMillis >= 1000) {
        digitalWrite(buzzerPin, LOW);  // Apagar el zumbador
        buzzerActive = false;
        currentState = TRAIN_CROSSING;
      }
      break;

    case TRAIN_CROSSING:
      // Parpadeo del LED utilizando temporizadores
      if (currentMillis - previousMillis >= interval) {
        previousMillis = currentMillis;
        ledState = !ledState;
        digitalWrite(ledPin, ledState);
      }

      // Leer el sensor derecho para detectar si el tren ha pasado completamente
      distanceRight = measureDistance(triggerPinRight, echoPinRight);
      if (distanceRight > detectionThreshold) {
        currentState = TRAIN_PASSED;
      }
      break;

    case TRAIN_PASSED:
      // El tren ha pasado completamente, levantar las barreras y apagar el LED
      digitalWrite(ledPin, LOW);  // Apagar el LED
      raiseBarriers();  // Levantar las barreras
      currentState = IDLE;
      enterLowPowerMode();  // Volver al modo de bajo consumo
      break;

    case IDLE:
    default:
      // No hacer nada en el estado IDLE
      enterLowPowerMode();  // Continuar en bajo consumo
      break;
  }
}

// Función para medir la distancia con el sensor ultrasónico
int measureDistance(int triggerPin, int echoPin) {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;  // Calcular la distancia en centímetros
}

// Función para bajar las barreras
void lowerBarriers() {
  servoUpper.write(90);  
  servoLower.write(90);  
}

// Función para levantar las barreras
void raiseBarriers() {
  servoUpper.write(0);  
  servoLower.write(0);  
}


void enterLowPowerMode() {
  // Poner el micro en modo de bajo consumo por 8 segundos
  LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);  
}