/*
  MuTech Robotics - B1 LiDAR Smart Cane (haptic)
  Board : ESP32 development board (30-pin), powered from a USB power bank
  Front LiDAR  : aimed straight ahead      -> obstacle -> vibration pulses get faster as it gets closer
  Ground LiDAR : aimed down at the ground about 1 m ahead -> sudden drop (pothole / step down) -> long strong buzz
  Output: coin vibration motor MODULE (has its own driver transistor) - never drive a motor straight from a GPIO

  Wiring
    ESP32 VIN -> 5V rail, GND -> GND rail (power bank on the ESP32 USB port; VIN then gives about 5 V)
    Front LiDAR : red -> 5V rail, black -> GND rail, green (TX) -> D27, white (RX) -> D14
    Ground LiDAR: red -> 5V rail, black -> GND rail, green (TX) -> D33, white (RX) -> D32
    Vibration module: VCC -> 3V3, GND -> GND, IN -> D18
  Check your LiDAR's wire colours in its datasheet. Data pins are 3.3 V logic, 115200 baud, 9-byte frames (0x59 0x59 ...).

  Start-up: hold the cane in your normal walking position for 3 seconds - it learns the normal ground distance.
  A walking aid only. Always keep using normal cane technique.
*/
HardwareSerial front(1);
HardwareSerial ground(2);
const int VIB = 18;
const int NEAR_CM = 70, WARN_CM = 200;     // continuous below 70 cm, pulses from 2 m
const int DROP_CM = 20;                    // ground suddenly 20 cm further than normal = drop

struct Lidar { HardwareSerial* port; uint8_t b[9]; int n; int dist; unsigned long t; };
Lidar F = { &front, {0}, 0, -1, 0 }, G = { &ground, {0}, 0, -1, 0 };
int groundNormal = -1;

void poll(Lidar& L) {
  while (L.port->available()) {
    uint8_t c = L.port->read();
    if (L.n < 2 && c != 0x59) { L.n = 0; continue; }
    L.b[L.n++] = c;
    if (L.n == 9) {
      uint8_t s = 0; for (int i = 0; i < 8; i++) s += L.b[i];
      int strength = L.b[4] | (L.b[5] << 8);
      if (s == L.b[8] && strength > 100) { L.dist = L.b[2] | (L.b[3] << 8); L.t = millis(); }
      L.n = 0;
    }
  }
}

void setup() {
  front.begin(115200, SERIAL_8N1, 27, 14);    // RX = D27 (front LiDAR TX), TX = D14
  ground.begin(115200, SERIAL_8N1, 33, 32);   // RX = D33 (ground LiDAR TX), TX = D32
  pinMode(VIB, OUTPUT);
  // learn the normal ground distance
  long sum = 0; int cnt = 0; unsigned long t0 = millis();
  while (millis() - t0 < 3000) { poll(G); if (G.dist > 0 && millis() - G.t < 50) { sum += G.dist; cnt++; G.t = 0; } }
  groundNormal = cnt ? sum / cnt : 120;
  for (int i = 0; i < 2; i++) { digitalWrite(VIB, HIGH); delay(120); digitalWrite(VIB, LOW); delay(120); }   // ready
}

void loop() {
  poll(F); poll(G);
  unsigned long now = millis();
  bool drop = G.dist > 0 && now - G.t < 300 && G.dist > groundNormal + DROP_CM;
  int d = (F.dist > 0 && now - F.t < 300) ? F.dist : 9999;
  bool on = false;
  if (drop) on = true;                                  // long strong buzz
  else if (d < NEAR_CM) on = true;                      // non-stop
  else if (d < WARN_CM) {                               // pulses: 120 ms .. 620 ms apart
    int period = 120 + (long)(d - NEAR_CM) * 500 / (WARN_CM - NEAR_CM);
    on = (now % period) < 60;
  }
  digitalWrite(VIB, on ? HIGH : LOW);
}