// MUHAMMAD FA'IZ DWI ADHISKI
// XI TEK 1

// ================================

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

#define MOTOR1 27
#define MOTOR2 19

#define TRIG1 25
#define ECHO1 33

#define TRIG2 0
#define ECHO2 4

float suara = 0.034;
long durasi1, jarak1, durasi2, jarak2;

bool bootState = 0;
bool enterState = 0;
bool exitState = 0;
int parkirSlot[15] = {
  0,0,0,
  0,0,0,
  0,0,0,
  0,0,0,
  0,0,0
};

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo motor1;
Servo motor2;

namespace Parkir {
  void begi() {
    Serial.begin(9600);

    lcd.init();
    lcd.backlight();
    lcd.clear();

    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);

    pinMode(TRIG1, OUTPUT);
    pinMode(ECHO1, INPUT);
    pinMode(TRIG2, OUTPUT);
    pinMode(ECHO2, INPUT);
  }

  void set() {
    for(int i = 0;i < 15; i++) {
      if(!parkirSlot[i]) {
        parkirSlot[i]++;
        break;
      }
    }
  }

  void del() {
    for(int i = 0;i < 15; i++) {
      if(parkirSlot[i]) {
        parkirSlot[i]--;
        break;
      }
    }
  }

  int parkirJumlah() {
    int slot = 0;
    for(int i = 0;i < 15; i++) {
      if(parkirSlot[i]) {
        slot++;
      }
    }

    return slot;
  }

  void lcds() {
    lcd.setCursor(0, 0);
    lcd.printstr("KENDARAAN: ");
    lcd.print(parkirJumlah());

    lcd.setCursor(0, 1);
    lcd.printstr("SISA PARKIR: ");
    lcd.print((sizeof(parkirSlot) / sizeof(parkirSlot[0])) - parkirJumlah());
  }

  void ultrasonic() {
    digitalWrite(TRIG1, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG1, HIGH);
    delayMicroseconds(5);
    digitalWrite(TRIG1, LOW);
    delayMicroseconds(5);


    durasi1 = pulseIn(ECHO1, HIGH);
    jarak1 = (durasi1 * suara / 2) + 1;

    // Serial.print("Jarak 1: ");
    // Serial.println(jarak1);

    digitalWrite(TRIG2, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG2, HIGH);
    delayMicroseconds(5);
    digitalWrite(TRIG2, LOW);
    delayMicroseconds(5);

    durasi2 = pulseIn(ECHO2, HIGH);
    jarak2 = (durasi2 * suara / 2) + 1;

    // Serial.print("Jarak 2: ");
    // Serial.println(jarak2);

    if(!bootState) {
      bootState = 1;
      delay(100);
    }
  }

  void main() {
    ultrasonic();
    // Serial.print(enterState);
    // Serial.print("(enter) : ");
    // Serial.println(jarak1);
    // Serial.print(exitState);
    // Serial.print("(exit) : ");
    // Serial.println(jarak2);
 
    // Masuk
    if(jarak1 <= 14 && bootState) {
      enterState = true;

      motor1.write(0);
    }
    else if(jarak1 > 14 && enterState) {
      for(int i = 0; i < 15; i++) {
        if(!parkirSlot[i]) {
          parkirSlot[i] = 1;
          break;
        }
      }

      enterState = false;
      delay(3000);
      motor1.write(90);
    }

    // Keluar
    if(jarak2 <= 14) {
      exitState = true;

      motor2.write(0);
    }
    else if(jarak2 > 14 && exitState) {
      for(int i = 0; i < 15; i++) {
        if(parkirSlot[i]) {
          parkirSlot[i] = 0;
          break;
        }
      }

      exitState = false;
      delay(3000);
      motor2.write(90);
    }

    lcds();
  }
}

void setup() {
  Parkir::begi();
}

void loop() {
  Parkir::main();
  delay(100);
}