// Pure STM32 Parallel 1602 LCD Drive (No external libraries required)

const int RS_PIN = 7;
const int EN_PIN = 8;
const int D4_PIN = 9;
const int D5_PIN = 10;
const int D6_PIN = 11;
const int D7_PIN = 12;

const int TRIG_PIN   = 5;   // D5
const int ECHO_PIN   = 18;  // D18
const int BUZZER_PIN = 19;  // D19
const int LED_PIN    = 2;   // D2

const float TTC_CRITICAL_SEC = 5.0;  // 5 seconds buffer
const float DIST_MIN_METERS  = 15.0; // Set high so default 4.0m automatically triggers ALARM

float previousDistanceMeters = 0.0;
unsigned long previousTimeMs = 0;

void pulseEnable() {
  digitalWrite(EN_PIN, LOW);
  delayMicroseconds(1);
  digitalWrite(EN_PIN, HIGH);
  delayMicroseconds(1);
  digitalWrite(EN_PIN, LOW);
  delayMicroseconds(100);
}

void write4Bits(uint8_t value) {
  digitalWrite(D4_PIN, (value >> 0) & 0x01);
  digitalWrite(D5_PIN, (value >> 1) & 0x01);
  digitalWrite(D6_PIN, (value >> 2) & 0x01);
  digitalWrite(D7_PIN, (value >> 3) & 0x01);
  pulseEnable();
}

void lcdCommand(uint8_t cmd) {
  digitalWrite(RS_PIN, LOW);
  write4Bits(cmd >> 4);
  write4Bits(cmd & 0x0F);
  delay(2);
}

void lcdChar(char ch) {
  digitalWrite(RS_PIN, HIGH);
  write4Bits(ch >> 4);
  write4Bits(ch & 0x0F);
  delayMicroseconds(50);
}

void lcdPrint(const char* str) {
  while (*str) {
    lcdChar(*str++);
  }
}

void lcdSetCursor(uint8_t col, uint8_t row) {
  uint8_t row_offsets[] = {0x00, 0x40};
  lcdCommand(0x80 | (col + row_offsets[row]));
}

void lcdInit() {
  pinMode(RS_PIN, OUTPUT);
  pinMode(EN_PIN, OUTPUT);
  pinMode(D4_PIN, OUTPUT);
  pinMode(D5_PIN, OUTPUT);
  pinMode(D6_PIN, OUTPUT);
  pinMode(D7_PIN, OUTPUT);

  delay(50);
  digitalWrite(RS_PIN, LOW);
  digitalWrite(EN_PIN, LOW);

  write4Bits(0x03); delay(5);
  write4Bits(0x03); delayMicroseconds(150);
  write4Bits(0x03);
  write4Bits(0x02); // 4-bit mode

  lcdCommand(0x28); // 2 lines, 5x8 font
  lcdCommand(0x0C); // Display ON, Cursor OFF
  lcdCommand(0x01); // Clear Display
  delay(2);
}

void setup() {
  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  digitalWrite(TRIG_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);
  digitalWrite(LED_PIN, LOW);

  lcdInit();
  lcdSetCursor(0, 0);
  lcdPrint("FOG RADAR READY");
  lcdSetCursor(0, 1);
  lcdPrint("STM32 ACTIVE...");
  delay(1000);
  lcdCommand(0x01);

  previousTimeMs = millis();
}

void loop() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  float currentDistanceMeters = (duration > 0) ? ((duration * 0.0343 / 2.0) / 100.0) : 4.0;

  unsigned long currentTimeMs = millis();
  float deltaTimeSec = (currentTimeMs - previousTimeMs) / 1000.0;
  if (deltaTimeSec <= 0.001) deltaTimeSec = 0.05;

  float closingSpeedMps = (previousDistanceMeters - currentDistanceMeters) / deltaTimeSec;
  if (abs(closingSpeedMps) < 0.05) closingSpeedMps = 0.0;

  float ttcSec = (closingSpeedMps > 0.05) ? (currentDistanceMeters / closingSpeedMps) : 999.0;

  // Evaluate critical alert condition
  bool criticalAlert = (currentDistanceMeters < DIST_MIN_METERS) || (ttcSec <= TTC_CRITICAL_SEC);

  digitalWrite(BUZZER_PIN, criticalAlert ? HIGH : LOW);
  digitalWrite(LED_PIN, criticalAlert ? HIGH : LOW);

  int distInt = (int)(currentDistanceMeters * 10);
  int ttcInt  = (int)(ttcSec * 10);

  // Line 1: Distance & TTC
  lcdSetCursor(0, 0);
  lcdPrint("D:");
  
  char buf[8];
  sprintf(buf, "%d.%dm ", distInt / 10, distInt % 10);
  lcdPrint(buf);

  lcdPrint("TTC:");
  if (ttcSec > 50.0) {
    lcdPrint("SAFE ");
  } else {
    sprintf(buf, "%d.%ds ", ttcInt / 10, ttcInt % 10);
    lcdPrint(buf);
  }

  // Line 2: Status
  lcdSetCursor(0, 1);
  if (criticalAlert) {
    lcdPrint("** ALARM WARN **");
  } else {
    lcdPrint("STATUS: CLEAR   ");
  }

  // Serial Monitor Output
  Serial.print("Dist: "); Serial.print(currentDistanceMeters, 2);
  Serial.print(" m | Speed: "); Serial.print(closingSpeedMps, 2);
  Serial.print(" m/s | TTC: ");
  if (ttcSec > 50.0) Serial.print("SAFE  ");
  else { Serial.print(ttcSec, 1); Serial.print(" s  "); }

  if (criticalAlert) Serial.println(" | >>> ALARM TRIGGERED <<<");
  else Serial.println(" | STATUS: CLEAR");

  previousDistanceMeters = currentDistanceMeters;
  previousTimeMs = currentTimeMs;

  delay(100);
}