#include 

// Front ultrasonic sensor
#define TRIG1 4
#define ECHO1 3

// Back ultrasonic sensor
#define TRIG2 11
#define ECHO2 10

// Servo
#define SERVO_PIN 9
Servo myServo;

// ---------------------- Tunable parameters ----------------------
// Distance between two sensors (cm). Change to your real spacing.
const float SENSOR_GAP_CM = 10.0;

// Trigger threshold (cm). Object is considered "detected" when distance < this.
const float DETECT_THRESHOLD_CM = 20.0;

// Speed range for gauge mapping (cm/s). 0..MAX_SPEED maps to -90..+90 degrees.
const float MAX_SPEED_CM_S = 60.0;

// Anti-repeat trigger window (ms)
const unsigned long COOLDOWN_MS = 150;

// Timeout for pulseIn (microseconds). 30000us ~ 5m max, safe for HC-SR04.
const unsigned long PULSE_TIMEOUT_US = 30000;

// How long to show the gauge result (ms)
const unsigned long HOLD_MS = 1000;
// ---------------------------------------------------------------

// State
bool waitingForSecond = false;
unsigned long t1_ms = 0;
unsigned long lastTrigger_ms = 0;

// A small helper to read distance (cm) from an HC-SR04-like sensor.
// Returns: distance in cm; returns -1 if timeout/no reading.
float readDistanceCm(uint8_t trigPin, uint8_t echoPin) {
  // Trigger pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read echo pulse
  unsigned long duration = pulseIn(echoPin, HIGH, PULSE_TIMEOUT_US);
  if (duration == 0) return -1.0; // timeout

  // Sound speed ~ 0.034 cm/us. Divide by 2 for round trip.
  float distance = (duration * 0.034f) / 2.0f;
  return distance;
}

// Map speed (0..MAX_SPEED_CM_S) to a gauge angle (-90..+90),
// then convert to servo position (0..180).
int speedToServoPos(float speed_cm_s) {
  // Clamp speed
  if (speed_cm_s < 0) speed_cm_s = 0;
  if (speed_cm_s > MAX_SPEED_CM_S) speed_cm_s = MAX_SPEED_CM_S;

  // Linear map to angle -90..+90
  float angle = (speed_cm_s / MAX_SPEED_CM_S) * 180.0f - 90.0f; // -90..+90

  // Convert "gauge angle" to servo write value.
  // Keep same style as original project: servoPos = 90 - angle
  int servoPos = (int)(90.0f - angle);

  // Clamp servo bounds
  if (servoPos < 0) servoPos = 0;
  if (servoPos > 180) servoPos = 180;
  return servoPos;
}

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

  pinMode(TRIG1, OUTPUT);
  pinMode(ECHO1, INPUT);

  pinMode(TRIG2, OUTPUT);
  pinMode(ECHO2, INPUT);

  myServo.attach(SERVO_PIN);

  // Initialize gauge pointer to left side (like original: 180 corresponds to -90° style)
  myServo.write(180);
  delay(300);

  Serial.println("=== Speed Detection 2.0 (Dual Ultrasonic) ===");
  Serial.print("SENSOR_GAP_CM = "); Serial.println(SENSOR_GAP_CM);
  Serial.print("DETECT_THRESHOLD_CM = "); Serial.println(DETECT_THRESHOLD_CM);
  Serial.print("MAX_SPEED_CM_S = "); Serial.println(MAX_SPEED_CM_S);
}

void loop() {
  unsigned long now = millis();

  // Read both distances
  float dFront = readDistanceCm(TRIG1, ECHO1);
  float dBack  = readDistanceCm(TRIG2, ECHO2);

  // Optional debug (comment out if too noisy)
  // Serial.print("Front: "); Serial.print(dFront); Serial.print(" cm, ");
  // Serial.print("Back: ");  Serial.print(dBack);  Serial.println(" cm");

  // Basic validity
  bool frontDetected = (dFront > 0 && dFront < DETECT_THRESHOLD_CM);
  bool backDetected  = (dBack  > 0 && dBack  < DETECT_THRESHOLD_CM);

  // Cooldown to prevent repeated triggers while object stays in front
  bool cooldownOK = (now - lastTrigger_ms) > COOLDOWN_MS;

  // 1) Front sensor triggers start timing
  if (!waitingForSecond && cooldownOK && frontDetected) {
    t1_ms = now;
    waitingForSecond = true;
    lastTrigger_ms = now;

    Serial.print("[Front] Detected at t1 = ");
    Serial.print(t1_ms);
    Serial.println(" ms");
  }

  // 2) Back sensor triggers stop timing and compute speed
  if (waitingForSecond && cooldownOK && backDetected) {
    unsigned long t2_ms = now;
    waitingForSecond = false;
    lastTrigger_ms = now;

    unsigned long dt_ms = (t2_ms >= t1_ms) ? (t2_ms - t1_ms) : 0;

    Serial.print("[Back ] Detected at t2 = ");
    Serial.print(t2_ms);
    Serial.print(" ms, dt = ");
    Serial.print(dt_ms);
    Serial.println(" ms");

    if (dt_ms > 0) {
      float time_s = dt_ms / 1000.0f;
      float speed_cm_s = SENSOR_GAP_CM / time_s;

      Serial.print("Speed = ");
      Serial.print(speed_cm_s, 2);
      Serial.println(" cm/s");

      int servoPos = speedToServoPos(speed_cm_s);
      Serial.print("ServoPos = ");
      Serial.println(servoPos);

      // Show result on gauge
      myServo.write(servoPos);
      delay(HOLD_MS);

      // Return to start position
      myServo.write(180);
      delay(200);
    } else {
      Serial.println("dt_ms == 0, ignored.");
    }
  }

  // Safety: if front triggered but back never arrives, reset after some time
  if (waitingForSecond && (now - t1_ms) > 5000) {
    waitingForSecond = false;
    Serial.println("Timeout waiting for back sensor. Reset.");
    myServo.write(180);
  }

  delay(10); // small loop delay
}