#include <UltraPing.h>
#include <Wire.h>
#include <hd44780.h>
#include <hd44780ioClass/hd44780_I2Cexp.h>

int CeilingHeight;
int P1S;
int P1G;
int P2G;
int P3G;
int P4G;
int P2S;
int P1R;
int P2R;
int P3R;
int P4R;
int AVAILBLE_PARKING;
int P3S;
int TAKEN_PARKING;
int P4S;
UltraPing *p1;
UltraPing *p2;
UltraPing p3(13, 12);
UltraPing p4(2, 3);
hd44780_I2Cexp lcd;

void setup() {
  p1 = new UltraPing(A0, A1);
  p2 = new UltraPing(A2, A3);
  CeilingHeight = 250;
  P1G = 4;
  pinMode(P1G, OUTPUT);
  P1R = 5;
  pinMode(P1R, OUTPUT);
  P2G = 6;
  pinMode(P2G, OUTPUT);
  P2R = 7;
  pinMode(P2R, OUTPUT);
  P3G = 8;
  pinMode(P3G, OUTPUT);
  P3R = 9;
  pinMode(P3R, OUTPUT);
  P4G = 10;
  pinMode(P4G, OUTPUT);
  P4R = 11;
  pinMode(P4R, OUTPUT);
  lcd.begin(20, 4);
  lcd.setCursor(6, 1);
  lcd.print("BXX-3004a");
  lcd.setCursor(2, 2);
  lcd.print("Parking  counter");
  delay(3000);
  lcd.clear();
}

void loop() {
  if (p1->ping_cm() <= CeilingHeight) {
    p12("LOW");
    P1S = 1;
  } else {
    p12("HIGH");
    P1S = 0;
  }
  if (p2->ping_cm() <= CeilingHeight) {
    p22("LOW");
    P2S = 1;
  } else {
    p22("HIGH");
    P2S = 0;
  }
  if (p3.ping_cm() <= CeilingHeight) {
    p32("LOW");
    P3S = 1;
  } else {
    p32("HIGH");
    P3S = 0;
  }
  if (p4.ping_cm() <= CeilingHeight) {
    p42("LOW");
    P4S = 1;
  } else {
    p42("HIGH");
    P4S = 0;
  }
  TAKEN_PARKING = (P1S + P2S) + (P3S + P4S);
  AVAILBLE_PARKING = 4 - ((P1S + P2S) + (P3S + P4S));
  PRINTSTATUS();
  delay(1000);
}

void PRINTSTATUS() {
  lcd.setCursor(0, 0);
  lcd.print("Available:");
  lcd.print(AVAILBLE_PARKING);
  lcd.setCursor(0, 1);
  lcd.print("Taken:");
  lcd.print(TAKEN_PARKING);
  lcd.setCursor(0, 2);
  lcd.print("P1");
  lcd.setCursor(6, 2);
  lcd.print("P2");
  lcd.setCursor(12, 2);
  lcd.print("P3");
  lcd.setCursor(18, 2);
  lcd.print("P4");
  lcd.setCursor(0, 3);
  lcd.print(P1S);
  lcd.setCursor(6, 3);
  lcd.print(P2S);
  lcd.setCursor(12, 3);
  lcd.print(P3S);
  lcd.setCursor(18, 3);
  lcd.print(P4S);
}

void p12(String stat) {
  if (stat == "HIGH") {
    digitalWrite(P1G, HIGH);
    digitalWrite(P1R, LOW);
  } else {
    digitalWrite(P1G, LOW);
    digitalWrite(P1R, HIGH);
  }
}

void p22(String stat1) {
  if (stat1 == "HIGH") {
    digitalWrite(P2G, HIGH);
    digitalWrite(P2R, LOW);
  } else {
    digitalWrite(P2G, LOW);
    digitalWrite(P2R, HIGH);
  }
}

void p32(String stat2) {
  if (stat2 == "HIGH") {
    digitalWrite(P3G, HIGH);
    digitalWrite(P3R, LOW);
  } else {
    digitalWrite(P3G, LOW);
    digitalWrite(P3R, HIGH);
  }
}

void p42(String stat4) {
  if (stat4 == "HIGH") {
    digitalWrite(P4G, HIGH);
    digitalWrite(P4R, LOW);
  } else {
    digitalWrite(P4G, LOW);
    digitalWrite(P4R, HIGH);
  }
}