// =====================================================
//  Ultrasound Motion Lab
//
//  Hardware:
//    ESP32D + 3.3 V compatible HC-SR04 / SR04
//    ultrasonic sensor
//
//  Function:
//    Measures distance and calculates velocity
//    and acceleration.
//
//  Author:
//  Stefan Bracher
//  Coded with AI Assistance (ChatGPT GPT-5.5)
//	Version: 	2026-06-11
//
//   Original Arduino prototype:
// 	Mobile Distance Sensor
//	Sophia Caruntu and Arman Ibne Kamal
//	203-SNC-VA Mechatronics and Computational Physics, Vanier College, Winter 2026
//
//    ESP implementation and v-t / a-t additions:
//    Stefan Bracher
//
//  Website:
//    https://stefan.bracher.info/ultrasound_motion_lab.php
//
//  Wiring:
//    SR04 VCC  -> 3.3 V
//    SR04 GND  -> GND
//    SR04 TRIG -> GPIO 25
//    SR04 ECHO -> GPIO 26
//
//  Serial Output Format:
//    time_ms,distance_m,velocity_m_s,acceleration_m_s2
//
// =====================================================
// ------------------ Variables -----------------
const int TRIG_PIN = 25;
const int ECHO_PIN = 26;

// ------------------ Timing -----------------
const unsigned long MEASUREMENT_INTERVAL_MS = 20;

// Timeout for pulseIn.
// 30000 us corresponds to about 5 m round-trip range.
const unsigned long ECHO_TIMEOUT_US = 30000;

unsigned long lastMeasurement = 0;


// ------------------ Filtering -----------------
// Higher alpha = faster response, less smoothing.
// Lower alpha = slower response, more smoothing.
const float ALPHA_DISTANCE = 0.60;
const float ALPHA_VELOCITY = 0.20;
const float ALPHA_ACCELERATION = 0.20;

float filteredDistance_m = 0.0;
float filteredVelocity_m_s = 0.0;
float filteredAcceleration_m_s2 = 0.0;

bool hasFilteredDistance = false;
bool hasFilteredVelocity = false;
bool hasFilteredAcceleration = false;


// ------------------ Previous Values -----------------
bool hasPreviousDistance = false;
bool hasPreviousVelocity = false;

unsigned long previousTime_ms = 0;

float previousDistance_m = 0.0;
float previousVelocity_m_s = 0.0;


// =====================================================
//  SETUP
// =====================================================
void setup() {
  Serial.begin(115200);

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

  digitalWrite(TRIG_PIN, LOW);

  delay(500);

  // CSV header for WebSerial / Serial Plotter / Python
  Serial.println("time_ms,distance_m,velocity_m_s,acceleration_m_s2");
}


// =====================================================
//  MAIN LOOP
// =====================================================
void loop() {
  unsigned long now = millis();

  if (now - lastMeasurement >= MEASUREMENT_INTERVAL_MS) {
    lastMeasurement = now;

    float rawDistance_m = measureDistanceM();

    if (rawDistance_m < 0) {
      rawDistance_m = 0.0;   // no valid echo
    }

    // ------------------ Filter distance ------------------
    if (!hasFilteredDistance) {
      filteredDistance_m = rawDistance_m;
      hasFilteredDistance = true;
    } else {
      filteredDistance_m =
        ALPHA_DISTANCE * rawDistance_m +
        (1.0 - ALPHA_DISTANCE) * filteredDistance_m;
    }

    float dt_s = (now - previousTime_ms) / 1000.0;

    // ------------------ Calculate and filter velocity ------------------
    if (hasPreviousDistance && dt_s > 0) {
      float rawVelocity_m_s =
        (filteredDistance_m - previousDistance_m) / dt_s;

      if (!hasFilteredVelocity) {
        filteredVelocity_m_s = rawVelocity_m_s;
        hasFilteredVelocity = true;
      } else {
        filteredVelocity_m_s =
          ALPHA_VELOCITY * rawVelocity_m_s +
          (1.0 - ALPHA_VELOCITY) * filteredVelocity_m_s;
      }

      hasPreviousVelocity = true;
    }

    // ------------------ Calculate and filter acceleration ------------------
    if (hasPreviousVelocity && dt_s > 0) {
      float rawAcceleration_m_s2 =
        (filteredVelocity_m_s - previousVelocity_m_s) / dt_s;

      if (!hasFilteredAcceleration) {
        filteredAcceleration_m_s2 = rawAcceleration_m_s2;
        hasFilteredAcceleration = true;
      } else {
        filteredAcceleration_m_s2 =
          ALPHA_ACCELERATION * rawAcceleration_m_s2 +
          (1.0 - ALPHA_ACCELERATION) * filteredAcceleration_m_s2;
      }
    }

    // ------------------ Store current values for next cycle ------------------
    previousTime_ms = now;
    previousDistance_m = filteredDistance_m;
    previousVelocity_m_s = filteredVelocity_m_s;

    hasPreviousDistance = true;

    // ------------------ Serial CSV output ------------------
    Serial.print(now);
    Serial.print(",");
    Serial.print(filteredDistance_m, 4);
    Serial.print(",");
    Serial.print(filteredVelocity_m_s, 4);
    Serial.print(",");
    Serial.println(filteredAcceleration_m_s2, 4);
  }
}


// =====================================================
//  MEASURE DISTANCE
// =====================================================
float measureDistanceM() {
  // Send 10 us trigger pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Measure echo pulse duration
  unsigned long duration_us = pulseIn(ECHO_PIN, HIGH, ECHO_TIMEOUT_US);

  if (duration_us == 0) {
    return -1;  // timeout / no echo
  }

  // Speed of sound approx. 343 m/s.
  // Sound travels to the object and back, so divide by 2.
  float distance_m = duration_us * 0.000343 / 2.0;

  return distance_m;
}