#include <TinyGPS++.h>
#include <SoftwareSerial.h>

// Pin Definitions
const int lidarLeftPin = A0;
const int lidarRightPin = A1;

// Thresholds (tune according to your LIDAR's voltage-distance mapping)
const int thresholdLeft = 300;   // Example: corresponds to ~30 cm
const int thresholdRight = 300;

// GPS setup (use actual GPS RX/TX pins)
SoftwareSerial gpsSerial(4, 3);  // RX, TX
TinyGPSPlus gps;

void setup() {
  Serial.begin(9600);
  gpsSerial.begin(9600);
}

void loop() {
  int analogLeft = analogRead(lidarLeftPin);
  int analogRight = analogRead(lidarRightPin);

  // Debugging
  Serial.print("Left: "); Serial.print(analogLeft);
  Serial.print(" | Right: "); Serial.println(analogRight);

  if (analogLeft <= thresholdLeft || analogRight <= thresholdRight) {
    // Read GPS data
    while (gpsSerial.available()) {
      gps.encode(gpsSerial.read());
    }

    if (gps.location.isUpdated()) {
      Serial.print("GPS Triggered → Lat: ");
      Serial.print(gps.location.lat(), 6);
      Serial.print(" | Lon: ");
      Serial.println(gps.location.lng(), 6);
      // Optional: save to SD card or transmit
    }
  }

  delay(100);  // Adjust polling interval
}