#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

Servo servo;
LiquidCrystal_I2C lcd(0x27, 16, 2);

#define green_LED_parking_1 2
#define red_LED_parking_1   3
#define green_LED_parking_2 4
#define red_LED_parking_2   5
#define green_LED_parking_3 6
#define red_LED_parking_3   7
#define sensor_parking_1    8
#define sensor_parking_2    9
#define sensor_parking_3    10
#define servo_pin           11 //PWM for Servo
#define sensor_entrance     12

int emptySpaces = 3;

bool parking_1 = 0;
bool parking_2 = 0;
bool parking_3 = 0;

void setup() {
  initIO();
  delay(5000);
}

void loop() {

  chkSensors();

  lcd.setCursor(0, 0);
  //        "****************"
  lcd.print("Parking Empty:");
  lcd.setCursor(0, 1);
  //        "****************"
  lcd.print(emptySpaces);
  lcd.print("          ");

  if (digitalRead(sensor_entrance) == 0) {
    Serial.println(digitalRead(sensor_entrance));
    Serial.print(emptySpaces);
    //check empty spaces
    if (emptySpaces > 0) {
      //open gate
      servo.attach(servo_pin);
      servo.write(90);
      delay(3000);
      servo.write(0);
      delay(500);
      servo.detach();
    }
    else {
      //keep gate closed
      servo.attach(servo_pin);
      delay(500);
      servo.write(0);
      delay(500);
      servo.detach();
    }
  }

}// end of LOOP

void initIO() {
  Serial.begin(9600);
  pinMode(green_LED_parking_1, OUTPUT);
  pinMode(green_LED_parking_2, OUTPUT);
  pinMode(green_LED_parking_3, OUTPUT);
  pinMode(red_LED_parking_1, OUTPUT);
  pinMode(red_LED_parking_2, OUTPUT);
  pinMode(red_LED_parking_3, OUTPUT);
  pinMode(sensor_parking_1, INPUT);
  pinMode(sensor_parking_2, INPUT);
  pinMode(sensor_parking_3, INPUT);
  pinMode(sensor_entrance, INPUT);
  digitalWrite(green_LED_parking_1, 1);
  digitalWrite(green_LED_parking_2, 1);
  digitalWrite(green_LED_parking_3, 1);
  digitalWrite(red_LED_parking_1, 0);
  digitalWrite(red_LED_parking_2, 0);
  digitalWrite(red_LED_parking_3, 0);
  lcd.init();
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Starting...");
  servo.attach(servo_pin);
  servo.write(0);
  servo.detach();
}

void chkSensors() {
  parking_1 = digitalRead(sensor_parking_1);
  parking_2 = digitalRead(sensor_parking_2);
  parking_3 = digitalRead(sensor_parking_3);

  emptySpaces = parking_1 + parking_2 + parking_3;

  if (parking_1 == 0) {
    digitalWrite(green_LED_parking_1, 0);
    digitalWrite(red_LED_parking_1, 1);

  }
  else if (parking_1 == 1) {
    digitalWrite(green_LED_parking_1, 1);
    digitalWrite(red_LED_parking_1, 0);
  }
  if (parking_2 == 0) {
    digitalWrite(green_LED_parking_2, 0);
    digitalWrite(red_LED_parking_2, 1);
  }
  else if (parking_2 == 1) {
    digitalWrite(green_LED_parking_2, 1);
    digitalWrite(red_LED_parking_2, 0);
  }
  if (parking_3 == 0) {
    digitalWrite(green_LED_parking_3, 0);
    digitalWrite(red_LED_parking_3, 1);
  }
  else if (parking_3 == 1) {
    digitalWrite(green_LED_parking_3, 1);
    digitalWrite(red_LED_parking_3, 0);
  }
}