#include <Servo.h>

// Definir el objeto Servo
Servo servoMotor;

// Pines del Servomotor:
#define PIN_SERVO 3
// Pines de los Leds:
#define PIN_AZUL 8
#define PIN_VERDE 11
#define PIN_AMARILLO 12
#define PIN_ROJO 13
#define PIN_BLANCO_1 1
#define PIN_BLANCO_2 2

// Pin de la alarma:
#define PIN_ALARMA 6
// Pines del Sensor de proximidad:
#define PIN_TRIG 5
#define PIN_ECHO 4
// Variables para umbrales de distancia:
int umbralAzul = 325;
int umbralVerde = 200;
int umbralAmarillo = 120;
int umbralRojo = 50;

// Posición original del servomotor (90 grados)
int posicionOriginal = 90;
// Variable para rastrear si el servomotor está activado
bool servomotorActivado = false;


void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_AZUL, OUTPUT);
  pinMode(PIN_VERDE, OUTPUT);
  pinMode(PIN_AMARILLO, OUTPUT);
  pinMode(PIN_ROJO, OUTPUT);

  // Adjuntar el objeto Servo al pin correspondiente
  servoMotor.attach(PIN_SERVO);
  

}

void loop() {
  // Empieza el medimiento del Sensor de Proximidad:
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Lee el resultado del Sensor de Proximidad:
  long duration = pulseIn(PIN_ECHO, HIGH);
  int distancia = duration / 58;

  Serial.print("Distancia en CM: ");
  Serial.println(distancia);

  // Controlar LEDs según distancia:
  
  digitalWrite(PIN_BLANCO_1, HIGH);
  digitalWrite(PIN_BLANCO_2, HIGH);
  
  if (distancia <= umbralRojo) {
    digitalWrite(PIN_AZUL, LOW);
    digitalWrite(PIN_VERDE, LOW);
    digitalWrite(PIN_AMARILLO, LOW);
    digitalWrite(PIN_ROJO, HIGH);
    tone(PIN_ALARMA, 1500, 0); // Tono de alarma
  } else if (distancia <= umbralAmarillo) {
    digitalWrite(PIN_AZUL, LOW);
    digitalWrite(PIN_VERDE, LOW);
    digitalWrite(PIN_AMARILLO, HIGH);
    digitalWrite(PIN_ROJO, LOW);
    tone(PIN_ALARMA, 1500, 800);
    delay(150);
    tone(PIN_ALARMA, 0, 800);
    delay(150);
    tone(PIN_ALARMA, 1500, 800);
    delay(150);
    tone(PIN_ALARMA, 0, 800);
    delay(150);
  } else if (distancia <= umbralVerde) {
    digitalWrite(PIN_AZUL, LOW);
    digitalWrite(PIN_VERDE, HIGH);
    digitalWrite(PIN_AMARILLO, LOW);
    digitalWrite(PIN_ROJO, LOW);
    tone(PIN_ALARMA, 1500, 800);
    delay(450);
    tone(PIN_ALARMA, 0, 800);
    delay(450);
    tone(PIN_ALARMA, 1500, 800);
    delay(450);
    tone(PIN_ALARMA, 0, 800);
    delay(450);
  } else if (distancia <= umbralAzul) {
    digitalWrite(PIN_AZUL, HIGH);
    digitalWrite(PIN_VERDE, LOW);
    digitalWrite(PIN_AMARILLO, LOW);
    digitalWrite(PIN_ROJO, LOW);
    noTone(PIN_ALARMA); // Apagar el tono de alarma
  } else {
    digitalWrite(PIN_AZUL, LOW); // Apagar LED azul si la distancia está por encima de 300 cm
    digitalWrite(PIN_VERDE, LOW);
    digitalWrite(PIN_AMARILLO, LOW);
    digitalWrite(PIN_ROJO, LOW);
    noTone(PIN_ALARMA); // Apagar el tono de alarma
  }

  // Controlar el Servomotor
  if (distancia < 20) {
    // Gira el servomotor 35 grados en sentido antihorario solo si no está activado ya
    if (!servomotorActivado) {
      servoMotor.write(0); // 0 grados es la posición antihoraria máxima
      servomotorActivado = true; // Marcar que el servomotor está activado
    }
  } else if (distancia > 225) {
    // Si la distancia es mayor que 225 cm, volver el servomotor a la posición original (90 grados) y desactivarlo
    servoMotor.write(posicionOriginal);
    servomotorActivado = false; // Marcar que el servomotor ya no está activado
  }
}