//Trabajo Practico 5
//Simulacion del cruce de vias de un tren.

#include <Servo.h>  		// incluye libreria de Servo
#include <MsTimer2.h>  // incluye libreria de MsTimer2.h
//#include <LowPower.h> // incluye libreria para Power Saving

Servo servo1;			// crea objeto servo1
Servo servo2;			// crea objeto servo2

int pinServo = 4;		// pin 2 conectado a señal del servo1
int pulsoMin = 570;		// pulso minimo en microsegundos servo1
int pulsoMax = 2400;		// pulso maximo en microsegundos servo1
int pinServo2 = 7;		// pin 2 conectado a señal del servo2
int pulsoMin2 = 570;		// pulso minimo en microsegundos servo2
int pulsoMax2 = 2400;		// pulso maximo en microsegundos servo2
int trig = 6;			// trigger en pin 10 servo1
int echo = 5;			// echo en pin 9 servo1
int trig1 = 9;     // trigger en pin 6 servo2
int echo1 = 8;      // echo en pin 5 servo2
int led = 13;			// led en pin 3
int duracion;     // duracion del pulso servo1
int distancia;    // distancia en centimetros servo1
int duracion1;    // duracion del pulso servo2
int distancia1;   // distancia en centimetros servo1
const int buzzerPin = 12;  // Pin donde se conecta el buzzer

const int anguloServoActuacion = 90;   // Ángulo del servo cuando detecta objeto
const int anguloServoReposo = 0;       // Ángulo del servo en reposo
const int distanciaMin = 0;            // Distancia mínima
const int distanciaMax = 100;           // Distancia máxima en centimetros
const unsigned long interval = 500000; // Intervalo de parpadeo del LED en microsegundos
volatile bool bandera = false; //para que lo guarde y lo busque en la ram, luego
//poder utilizar en la interrupcion y dentro del loop

// Variables para el temporizador
unsigned long startTime = 0;  // Almacena el tiempo anterior para controlar el parpadeo del LED

// Función para controlar los servos
void moverServos(int angulo) {
  servo1.write(angulo);
  servo2.write(angulo);
}

// Función para verificar si la distancia está dentro del rango
bool distanciaEnRango(int distancia) {
  return (distancia >= distanciaMin && distancia <= distanciaMax);
}

//Funcion controlar LED con timer
void ParpadearLed(){
  if (bandera){
  digitalWrite(led, !digitalRead(led));
  }
}

void setup(){
  Serial.begin(9600);  		// inicializacion de comunicacion serial a 9600 bps
  MsTimer2::set(500, ParpadearLed); // Establece el temporizador a 500 ms (1 segundo)
  MsTimer2::start();             // Inicia el temporizador
  
  servo1.attach(pinServo, pulsoMin, pulsoMax);	// inicializacion de servo1
  servo2.attach(pinServo2, pulsoMin2, pulsoMax2);	// inicializacion de servo2
  servo1.write(0);		// ubica el servo1 a 0 grados
  servo2.write(0);    // ubica el servo2 a 0 grados
  
  pinMode(trig, OUTPUT); 	// trigger como salida del servo1
  pinMode(echo, INPUT);		// echo como entrada del servo1
  pinMode(trig1, OUTPUT); 	// trigger como salida del servo2
  pinMode(echo1, INPUT);		// echo como entrada del servo2
  pinMode(led, OUTPUT);		// led como salida
}

void loop(){
  digitalWrite(trig, HIGH); 		// generacion del pulso a enviar
  delayMicroseconds(100);				// al pin conectado al trigger
  digitalWrite(trig, LOW);		// del sensor
  
  duracion = pulseIn(echo, HIGH);	// funcion pulseIn se espera un pulso alto en Echo
  distancia = duracion / 58.2;		// distancia medida en centimetros
  delayMicroseconds(500);					// demora entre datos

  digitalWrite(trig1, HIGH); 		// generacion del pulso a enviar
  delayMicroseconds(100);					// al pin conectado al trigger
  digitalWrite(trig1, LOW);		// del sensor
  
  duracion1 = pulseIn(echo1, HIGH);	// funcion pulseIn se espera un pulso alto en Echo
  distancia1 = duracion1 / 58.2;		// distancia medida en centimetros
  delayMicroseconds(500);					// demora entre datos

  // Verifica si alguna de las dos distancias está en el rango
  if (distanciaEnRango(distancia) || distanciaEnRango(distancia1)) {
    bandera = true;
    moverServos(anguloServoActuacion);  // Mover los servos a 90 grados
    ParpadearLed(); // llamamos a la funcion parpadearled 
    tone(buzzerPin, 1000);  // Reproduce un tono de 1000 Hz
  } else {
    moverServos(anguloServoReposo);  // Mover los servos a 0 grados
    //digitalWrite(led, LOW);  // Apaga el LED
    noTone(buzzerPin);      // Detiene el tono
    bandera = false; // para apagar el led

  }

  //LowPower.powerDown(SLEEP_1s, ADC_OFF, BOD_OFF); // Duerme el microcontrolador por un segundo 
  Serial.println("Me duermo 1 segundo: " + String(millis()));
  delay(1000);
  Serial.println("Me desperte: " + String( millis()));
}