#include <ESP32Servo.h>

const int segmentA = 26, segmentB = 27, segmentC = 13,
  segmentD = 12, segmentE = 14, segmentF = 25, segmentG = 33;

const int ledExitRed = 23, ledExitGreen = 22,
  ledEntranceRed = 21, ledEntranceGreen = 19,
  ledSensorRed = 17, ledSensorGreen = 16;

const int btnCarExit = 5, btnCarEntrance = 4,
  btnTicketOut = 18, btnTicketIn = 2;

const int gateExitPin = 15, gateEntrancePin = 32;

const int trigPin = 3, echoPin = 35;

int btnCarExitLastState = HIGH;
int btnCarEntranceLastState = HIGH;
int btnTicketOutLastState = HIGH;
int btnTicketInLastState = HIGH;

int distanceLimit = 300; // cm

Servo gateExit;
Servo gateEntrance;

int carCount = 0;

void setup() {
  setupPins();

  closeEntrance();
  closeExit();
  Serial.begin(115200);
}

void loop() {
  delay(10);

  if (ticketPulled() && digitalRead(ledEntranceRed) == HIGH) {
    openEntrance();
  }

  if (carEntered() && digitalRead(ledEntranceGreen) == HIGH) {
    closeEntrance();
    carCount++;
  }

  if (ticketInserted() && digitalRead(ledExitRed) == HIGH) {
    openExit();
  }

  if (carExited() && digitalRead(ledExitGreen) == HIGH) {
    closeExit();
    carCount--;
  }

  if (carCount > 9 || carCount < 0) carCount = 0;
  showNumber(carCount);

  (getDistance() > distanceLimit) ? ledColor('g') : ledColor('r');
}

void setupPins() {
  // Display
  pinMode(segmentA, OUTPUT);
  pinMode(segmentB, OUTPUT);
  pinMode(segmentC, OUTPUT);
  pinMode(segmentD, OUTPUT);
  pinMode(segmentE, OUTPUT);
  pinMode(segmentF, OUTPUT);
  pinMode(segmentG, OUTPUT);

  // Buttons
  pinMode(btnCarExit, INPUT_PULLUP);
  pinMode(btnCarEntrance, INPUT_PULLUP);
  pinMode(btnTicketOut, INPUT_PULLUP);
  pinMode(btnTicketIn, INPUT_PULLUP);

  // Leds
  pinMode(ledExitRed, OUTPUT);
  pinMode(ledExitGreen, OUTPUT);
  pinMode(ledEntranceRed, OUTPUT);
  pinMode(ledEntranceGreen, OUTPUT);
  pinMode(ledSensorRed, OUTPUT);
  pinMode(ledSensorGreen, OUTPUT);

  // Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Gates
  gateExit.attach(gateExitPin);
  gateEntrance.attach(gateEntrancePin);
}

void showSegments(bool a, bool b, bool c, bool d, bool e, bool f, bool g) {
  digitalWrite(segmentA, a);
  digitalWrite(segmentB, b);
  digitalWrite(segmentC, c);
  digitalWrite(segmentD, d);
  digitalWrite(segmentE, e);
  digitalWrite(segmentF, f);
  digitalWrite(segmentG, g);
}

void showNumber(int number) {
  // Converts char to int using ASCII table
  // if (number >= 48 && number <= 57) number -= 48;

  switch (number) {
    case 0:
    showSegments(1, 1, 1, 1, 1, 1, 0);
    break;

    case 1:
    showSegments(0, 1, 1, 0, 0, 0, 0);
    break;

    case 2:
    showSegments(1, 1, 0, 1, 1, 0, 1);
    break;

    case 3:
    showSegments(1, 1, 1, 1, 0, 0, 1);
    break;

    case 4:
    showSegments(0, 1, 1, 0, 0, 1, 1);
    break;

    case 5:
    showSegments(1, 0, 1, 1, 0, 1, 1);
    break;

    case 6:
    showSegments(1, 0, 1, 1, 1, 1, 1);
    break;

    case 7:
    showSegments(1, 1, 1, 0, 0, 0, 0);
    break;

    case 8:
    showSegments(1, 1, 1, 1, 1, 1, 1);
    break;

    case 9:
    showSegments(1, 1, 1, 1, 0, 1, 1);
    break;
  }
}

void openEntrance() {
  digitalWrite(ledEntranceRed, LOW);
  digitalWrite(ledEntranceGreen, HIGH);

  for (int angle = 90; angle >= 0; angle--) {
    gateEntrance.write(angle);
    delay(5);
  }
}

void closeEntrance() {
  digitalWrite(ledEntranceGreen, LOW);
  digitalWrite(ledEntranceRed, HIGH);
  
  for (int angle = 0; angle <= 90; angle++) {
  gateEntrance.write(angle);
  delay(5);
  }
}

void openExit() {
  digitalWrite(ledExitRed, LOW);
  digitalWrite(ledExitGreen, HIGH);

  for (int angle = 90; angle >= 0; angle--) {
    gateExit.write(angle);
    delay(5);
  }
}

void closeExit() {
  digitalWrite(ledExitGreen, LOW);
  digitalWrite(ledExitRed, HIGH);

  for (int angle = 0; angle <= 90; angle++) {
    gateExit.write(angle);
    delay(5);
  }
}

bool carExited() {
  int btnState = digitalRead(btnCarExit);
  bool btnPressed = false;
  if (btnCarExitLastState == HIGH && btnState == LOW) btnPressed = true;
  btnCarExitLastState = btnState;
  return btnPressed;
}

bool carEntered() {
  int btnState = digitalRead(btnCarEntrance);
  bool btnPressed = false;
  if (btnCarEntranceLastState == HIGH && btnState == LOW) btnPressed = true;
  btnCarEntranceLastState = btnState;
  return btnPressed;
}

bool ticketInserted() {
  int btnState = digitalRead(btnTicketOut);
  bool btnPressed = false;
  if (btnTicketOutLastState == HIGH && btnState == LOW) btnPressed = true;
  btnTicketOutLastState = btnState;
  return btnPressed;
}

bool ticketPulled() {
  int btnState = digitalRead(btnTicketIn);
  bool btnPressed = false;
  if (btnTicketInLastState == HIGH && btnState == LOW) btnPressed = true;
  btnTicketInLastState = btnState;
  return btnPressed;
}

int getDistance() {
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  int duration = pulseIn(echoPin, HIGH);
  return duration / 58; // cm
}

void ledColor(char ledColorLetter) {
  if (ledColorLetter == 'g') {
    digitalWrite(ledSensorRed, LOW);
    digitalWrite(ledSensorGreen, HIGH);
  } else if (ledColorLetter == 'r') {
    digitalWrite(ledSensorRed, HIGH);
    digitalWrite(ledSensorGreen, LOW);
  }
}