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

// Definições dos pinos dos sensores ultrassônicos HC-SR04
#define TRIG_PIN_02 4
#define ECHO_PIN_02 16

#define TRIG_PIN_03 17
#define ECHO_PIN_03 5

#define TRIG_PIN_04 18
#define ECHO_PIN_04 19

// Definições dos pinos dos LEDs RGB
#define R_PIN_02 12
#define G_PIN_02 14

#define R_PIN_03 27
#define G_PIN_03 26

#define R_PIN_04 25
#define G_PIN_04 33

// Definições das distâncias para mudar as cores dos LEDs
#define DIST_VERDE_MAX 50
#define DIST_VERMELHO_MIN 51
#define DIST_VERMELHO_MAX 400

// Definições do Display LCD I2C
#define LCD_ADDRESS 0x27 // Endereço I2C do display
#define LCD_COLUMNS 20   // Número de colunas do display
#define LCD_ROWS 4       // Número de linhas do display

// Definições do estacionamento
#define NUM_VAGAS 3 // Número total de vagas disponíveis

// Inicializa os objetos NewPing para cada sensor
NewPing sonar_02(TRIG_PIN_02, ECHO_PIN_02);
NewPing sonar_03(TRIG_PIN_03, ECHO_PIN_03);
NewPing sonar_04(TRIG_PIN_04, ECHO_PIN_04);

// Inicializa o display LCD I2C
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

// Inicializa o servo
Servo myServo; // Inicializa o servo
#define TRIGGER_PIN 2
#define ECHO_PIN 0
#define MAX_DISTANCE 400 // Distância máxima permitida em centímetros
#define SERVO_PIN 15
#define OPEN_ANGLE 0    // Ângulo de abertura para a esquerda
#define CLOSE_ANGLE 90  // Ângulo de fechamento
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // Inicializa o sensor HC-SR04

void setup() {
  // Inicializa os pinos dos LEDs como saída
  pinMode(R_PIN_02, OUTPUT);
  pinMode(G_PIN_02, OUTPUT);

  pinMode(R_PIN_03, OUTPUT);
  pinMode(G_PIN_03, OUTPUT);

  pinMode(R_PIN_04, OUTPUT);
  pinMode(G_PIN_04, OUTPUT);

  // Inicializa o display LCD I2C
  lcd.init();
  lcd.backlight(); // Liga o backlight do display

  // Define o texto inicial no display
  lcd.setCursor(0, 0);
  lcd.print("Vagas disponiveis:");
  lcd.setCursor(0, 1);
  lcd.print(NUM_VAGAS);

  // Inicializa o servo
  myServo.attach(SERVO_PIN); // Associa o servo ao pino GPIO
  Serial.begin(115200);
}

void loop() {
  // Realiza a medição de distância para cada sensor
  int distancia_02 = sonar_02.ping_cm();
  int distancia_03 = sonar_03.ping_cm();
  int distancia_04 = sonar_04.ping_cm();

  // Atualiza as cores dos LEDs de acordo com as distâncias medidas
  atualizarLED(R_PIN_02, G_PIN_02, distancia_02);
  atualizarLED(R_PIN_03, G_PIN_03, distancia_03);
  atualizarLED(R_PIN_04, G_PIN_04, distancia_04);

  // Realiza a medição de distância para o sensor ultrassônico principal
  delay(50); // Aguarda um curto período para estabilização
  int distance = sonar.ping_cm(); // Mede a distância em centímetros
  
  // Controla o servo com base na distância medida
  if (distance >= 50 && distance <= 400) { // Se a distância estiver no intervalo desejado
    myServo.write(OPEN_ANGLE); // Abre o servo para a esquerda
    delay(3000); // Aguarda 10 segundos com o servo aberto
    myServo.write(CLOSE_ANGLE); // Fecha o servo 90 graus
    delay(15); // Pequeno atraso após fechar o servo
  }
}

// Função para atualizar as cores dos LEDs com base na distância medida
void atualizarLED(int pinoR, int pinoG, int distancia) {
  if (distancia >= 0 && distancia <= DIST_VERDE_MAX) {
    // Distância dentro da faixa verde, LED verde aceso, vermelho apagado
    digitalWrite(pinoR, HIGH);
    digitalWrite(pinoG, LOW);
  } else if (distancia >= DIST_VERMELHO_MIN && distancia <= DIST_VERMELHO_MAX) {
    // Distância dentro da faixa vermelha, LED vermelho aceso, verde apagado
    digitalWrite(pinoR, LOW);
    digitalWrite(pinoG, HIGH);
  } else {
    // Distância fora das faixas definidas, desliga ambos os LEDs
    digitalWrite(pinoR, HIGH); // Desliga o LED vermelho
    digitalWrite(pinoG, HIGH); // Desliga o LED verde
  }

  // Atualiza o número de vagas disponíveis no display
  int vagas_ocupadas = contarVagasOcupadas();
  int vagas_disponiveis = NUM_VAGAS - vagas_ocupadas;
  lcd.setCursor(0, 1);
  lcd.print(vagas_disponiveis);
}

// Função para contar o número de vagas ocupadas
int contarVagasOcupadas() {
  int contador = 0;
  contador += digitalRead(R_PIN_02) == LOW;
  contador += digitalRead(R_PIN_03) == LOW;
  contador += digitalRead(R_PIN_04) == LOW;
  return contador;
}