#include <ESP32Servo.h>

const int EntradaAbrir = 23, SaidaAbrir = 22;
const int TRIG_PIN = 2, ECHO_PIN = 15;
const int servoInPin = 14, servoOutPin = 27;
const int buttonEntrada = 13, buttonSaida = 12;
const int segmentoA = 33, segmentoB = 32, segmentoC = 4, segmentoD = 18;
const int segmentoE = 5, segmentoF = 25, segmentoG = 26;

int lastDistance = 0;
int count = 9, precont = -1;
int pos = 0;
bool logip = HIGH, comparadorp = HIGH, logis = HIGH, comparadors = HIGH;

Servo servoIn, servoOut;

void setup() {
  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.println("Vagas Disponíveis");
  setupDisplay();
  pinMode(buttonEntrada, INPUT_PULLUP);
  pinMode(buttonSaida, INPUT_PULLUP);
  servoIn.attach(servoInPin);
  servoOut.attach(servoOutPin);
  pinMode(EntradaAbrir, OUTPUT);
  pinMode(SaidaAbrir, OUTPUT);
}

void loop() {
  delay(15);

  // Ultrasonic sensor
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  int distance = duration / 58;
  if (distance != lastDistance) {
    Serial.print("Distância em CM: ");
    Serial.println(distance);
    lastDistance = distance;
  }
  if (distance >= 150) {
    digitalWrite(EntradaAbrir, HIGH);
    digitalWrite(SaidaAbrir, LOW);
  }
  else if (distance < 150) {
    digitalWrite(SaidaAbrir, HIGH);
    digitalWrite(EntradaAbrir, LOW);
  }

  // Contagem de estacionamento
  logip = digitalRead(buttonEntrada);
  logis = digitalRead(buttonSaida);
  if (count > 0 && count < 9) {
    if (logip == LOW) {
      for (pos = 90; pos <= 180; pos += 5) {
        servoIn.write(pos);
        delay(20);
      }
      for (pos = 180; pos >= 90; pos -= 3) {
        servoIn.write(pos);
        delay(40);
      }
    }
    if (logis == LOW) {
      for (pos = 90; pos <= 180; pos += 5) {
        servoOut.write(pos);
        delay(20);
      }
      for (pos = 180; pos >= 90; pos -= 3) {
        servoOut.write(pos);
        delay(40);
      }
    }
  }
  if (logip == LOW && comparadorp != logip) {
    count--;
    comparadorp = logip;
    if (count <= 0) {
      count = 0;
    }
  }
  if (logip == HIGH && logip != comparadorp) {
    comparadorp = logip;
  }
  if (logis == LOW && logis != comparadors) {
    comparadors = logis;
    count++;
    if (count >= 9) {
      count = 9;
    }
  }
  if (logis == HIGH && logis != comparadors) {
    comparadors = logis;
  }
  if (count != precont) {
    Serial.print("O número de vagas: ");
    Serial.println(count);
    if (count > 9) {
      count = 9;
      Serial.print("O número de vagas: ");
      Serial.println(count);
    }
  }
  precont = count;

  if (count > 9) {
    count = 9;
  }
  else if (count < 0) {
    count = 0;
  }

  if (count <= 0) {
    displayOn(true, true, true, true, true, true, false);
  }
  else if (count == 1) {
    displayOn(false, true, true, false, false, false, false);
  }
  else if (count == 2) {
    displayOn(true, true, false, true, true, false, true);
  }
  else if (count == 3) {
    displayOn(true, true, true, true, false, false, true);
  }
  else if (count == 4) {
    displayOn(false, true, true, false, false, true, true);
  }
  else if (count == 5) {
    displayOn(true, false, true, true, false, true, true);
  }
  else if (count == 6) {
    displayOn(true, false, true, true, true, true, true);
  }
  else if (count == 7) {
    displayOn(true, true, true, false, false, false, false);
  }
  else if (count == 8) {
    displayOn(true, true, true, true, true, true, true);
  }
  else if (count >= 9) {
    displayOn(true, true, true, false, false, true, true);
  }
}

void setupDisplay() {
  pinMode(segmentoA, OUTPUT);
  pinMode(segmentoB, OUTPUT);
  pinMode(segmentoC, OUTPUT);
  pinMode(segmentoD, OUTPUT);
  pinMode(segmentoE, OUTPUT);
  pinMode(segmentoF, OUTPUT);
  pinMode(segmentoG, OUTPUT);
}

void displayOn(bool a, bool b, bool c, bool d, bool e, bool f, bool g) {
  digitalWrite(segmentoA, a);
  digitalWrite(segmentoB, b);
  digitalWrite(segmentoC, c);
  digitalWrite(segmentoD, d);
  digitalWrite(segmentoE, e);
  digitalWrite(segmentoF, f);
  digitalWrite(segmentoG, g);
}