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

#define TRIG_PIN 5
#define ECHO_PIN 18
#define BUZZER_PIN 17
#define SERVO_PIN 13
#define LED_RED 26
#define LED_GREEN 25
#define LED_YELLOW 33

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

long duration = 0;
int distanceCm = 0;

bool gateIsOpen = false;
bool buzzerForced = false;
unsigned long gateAutoCloseAt = 0;
unsigned long buzzerOffAt = 0;
unsigned long messageUntil = 0;
unsigned long lastDistanceReadAt = 0;

const uint16_t OPEN_ANGLE = 90;
const uint16_t CLOSE_ANGLE = 0;
const uint32_t GATE_OPEN_MS = 5000;
const uint32_t DENY_SHOW_MS = 3000;
const uint32_t DISTANCE_INTERVAL_MS = 120;

const bool LOCAL_AUTO_CLOSE_ENABLED = false;

// Tambah konstanta Sensor
const uint32_t SENSOR_REPORT_INTERVAL_MS = 150;
const int OBJECT_TRIGGER_CM = 35;
unsigned long lastSensorReportAt = 0;

String serialLine;

void setBuzzer(bool on) {
  digitalWrite(BUZZER_PIN, on ? HIGH : LOW);
}

void beepMs(uint32_t ms) {
  setBuzzer(true);
  buzzerOffAt = millis() + ms;
  buzzerForced = false;
}
// Tambah function baru
void sendSensorState() {
  StaticJsonDocument<128> sensor;
  bool objectPresent = (distanceCm > 0 && distanceCm <= OBJECT_TRIGGER_CM);
  sensor["distance_cm"] = distanceCm;
  sensor["object_present"] = objectPresent;
  serializeJson(sensor, Serial);
  Serial.println();
}

void showDefaultMessage() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" SMART PARKING ");
  lcd.setCursor(0, 1);
  lcd.print(" SCAN PLATE... ");

  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_GREEN, LOW);
  if (distanceCm < 20) digitalWrite(LED_YELLOW, HIGH);
  else digitalWrite(LED_YELLOW, LOW);

  messageUntil = 0;
}

String fit16(const char* txt) {
  String s = (txt && strlen(txt) > 0) ? String(txt) : String("UNKNOWN");
  s.trim();
  if (s.length() > 16) s = s.substring(0, 16);
  return s;
}

void openGate(const char* plate) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" ACCESS GRANTED ");
  lcd.setCursor(0, 1);
  lcd.print(fit16(plate));

  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_RED, LOW);
  digitalWrite(LED_YELLOW, LOW);

  gateServo.write(OPEN_ANGLE);
  gateIsOpen = true;
  gateAutoCloseAt = LOCAL_AUTO_CLOSE_ENABLED ? millis() + GATE_OPEN_MS : 0;
  messageUntil = gateAutoCloseAt;
  beepMs(200);
}

void denyGate(const char* plate) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" ACCESS DENIED ");
  lcd.setCursor(0, 1);
  lcd.print(fit16(plate));

  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);

  beepMs(1000);
  gateIsOpen = false;
  gateAutoCloseAt = 0;
  messageUntil = millis() + DENY_SHOW_MS;
}

void closeGate() {
  gateServo.write(CLOSE_ANGLE);
  gateIsOpen = false;
  gateAutoCloseAt = 0;
  showDefaultMessage();
}

// Ubah readDistance() biar timeout jelas
void readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH, 25000);
  if (duration > 0) {
    distanceCm = (int)(duration * 0.034f / 2.0f);
  } else {
    distanceCm = -1; // no echo / timeout
  }

  if (!gateIsOpen && messageUntil == 0) {
    digitalWrite(LED_YELLOW, (distanceCm > 0 && distanceCm < 20) ? HIGH : LOW);
  }
}

void sendAck(const char* cmd, bool ok, const char* msg = "") {
  StaticJsonDocument<128> out;
  out["cmd"] = cmd ? cmd : "";
  out["ok"] = ok;
  out["msg"] = msg;
  serializeJson(out, Serial);
  Serial.println();
}

void processJsonLine(const String& line) {
  if (line.length() == 0) return;

  StaticJsonDocument<256> doc;
  DeserializationError err = deserializeJson(doc, line);
  if (err) {
    sendAck("PARSE", false, "invalid_json");
    return;
  }

  const char* cmd = doc["cmd"] | "";
  const char* plate = doc["plate"] | "UNKNOWN";

  if (strcmp(cmd, "OPEN_GATE") == 0) {
    openGate(plate);
    sendAck(cmd, true, "gate_opened");
  } else if (strcmp(cmd, "DENY_GATE") == 0) {
    denyGate(plate);                 // ini sudah beep 1000 ms
    sendAck(cmd, true, "access_denied");
  } else if (strcmp(cmd, "CLOSE_GATE") == 0) {
    closeGate();
    sendAck(cmd, true, "gate_closed");
  } else if (strcmp(cmd, "BUZZER_ON") == 0) {
    setBuzzer(true);
    buzzerForced = true;
    sendAck(cmd, true, "buzzer_on");
  } else if (strcmp(cmd, "BUZZER_OFF") == 0) {
    setBuzzer(false);
    buzzerForced = false;
    buzzerOffAt = 0;
    sendAck(cmd, true, "buzzer_off");
  } else {
    sendAck(cmd, false, "unknown_cmd");
  }
}

void readSerialCommands() {
  while (Serial.available() > 0) {
    char c = (char)Serial.read();
    if (c == '\n') {
      processJsonLine(serialLine);
      serialLine = "";
    } else if (c != '\r') {
      serialLine += c;
      if (serialLine.length() > 300) serialLine = "";
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(20);
  Wire.begin(21, 22);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);

  ESP32PWM::allocateTimer(0);
  gateServo.setPeriodHertz(50);
  gateServo.attach(SERVO_PIN, 500, 2400);
  gateServo.write(CLOSE_ANGLE);

  lcd.init();
  lcd.backlight();
  showDefaultMessage();

  Serial.println("ESP32 Smart Parking Ready");
}

// Di loop(), tambahkan kirim sensor periodik
void loop() {
  unsigned long now = millis();

  readSerialCommands();

  if (!buzzerForced && buzzerOffAt > 0 && now >= buzzerOffAt) {
    setBuzzer(false);
    buzzerOffAt = 0;
  }

  if (LOCAL_AUTO_CLOSE_ENABLED && gateIsOpen && gateAutoCloseAt > 0 && now >= gateAutoCloseAt) {
    closeGate();
  }

  if (!gateIsOpen && messageUntil > 0 && now >= messageUntil) {
    showDefaultMessage();
  }

  if (now - lastDistanceReadAt >= DISTANCE_INTERVAL_MS) {
    lastDistanceReadAt = now;
    readDistance();
  }

  if (now - lastSensorReportAt >= SENSOR_REPORT_INTERVAL_MS) {
    lastSensorReportAt = now;
    sendSensorState();
  }
}