"""
HC-SR04 Ultrasonic Sensor Test Script
Tests GP14 (TRIG), GP15 (ECHO)
"""

from machine import Pin
import utime
import time

# Pin configuration
TRIG_PIN = 14
ECHO_PIN = 15

print("🔊 HC-SR04 Ultrasonic Sensor Test")
print("=" * 50)

# Initialize pins
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)
onboard_led = Pin("LED", Pin.OUT)
onboard_led.value(1)

trig.value(0)
time.sleep(2)  # Let sensor stabilize

print("\n📏 Reading distance measurements...")
print("   Valid range: 2cm to 400cm")
print("   Press Ctrl+C to stop\n")

def measure_distance():
    """Measure distance in cm"""
    try:
        # Send trigger pulse
        trig.value(0)
        utime.sleep_us(2)
        trig.value(1)
        utime.sleep_us(10)
        trig.value(0)
        
        # Wait for echo pulse
        timeout = 30000  # 30ms timeout
        start = utime.ticks_us()
        
        # Wait for echo to go HIGH
        while echo.value() == 0:
            if utime.ticks_diff(utime.ticks_us(), start) > timeout:
                return -1  # Timeout
        
        pulse_start = utime.ticks_us()
        
        # Wait for echo to go LOW
        while echo.value() == 1:
            if utime.ticks_diff(utime.ticks_us(), pulse_start) > timeout:
                return -1  # Timeout
        
        pulse_end = utime.ticks_us()
        pulse_duration = utime.ticks_diff(pulse_end, pulse_start)
        
        # Calculate distance
        # Speed of sound = 343 m/s = 0.0343 cm/μs
        # Distance = (time * speed) / 2  (divided by 2 for round trip)
        distance = (pulse_duration * 0.0343) / 2
        
        if distance > 400:
            return -1  # Out of range
        
        return distance
        
    except Exception as e:
        print(f"❌ Error: {e}")
        return -1

# Collect statistics
readings = []
successful = 0
failed = 0

try:
    while True:
        distance = measure_distance()
        
        if distance > 0:
            successful += 1
            readings.append(distance)
            
            # Keep last 10 readings for average
            if len(readings) > 10:
                readings.pop(0)
            
            avg = sum(readings) / len(readings)
            
            # Visual indicator based on distance
            if distance < 10:
                indicator = "🔴 VERY CLOSE"
            elif distance < 30:
                indicator = "🟡 CLOSE"
            elif distance < 100:
                indicator = "🟢 MEDIUM"
            else:
                indicator = "🔵 FAR"
            
            print(f"{indicator}  Distance: {distance:6.1f} cm  |  Avg: {avg:6.1f} cm  |  ✅: {successful}  ❌: {failed}")
        else:
            failed += 1
            print(f"⚠️  No reading (timeout or out of range)  |  ✅: {successful}  ❌: {failed}")
        
        # Blink LED on each reading
        onboard_led.value(0)
        time.sleep(0.05)
        onboard_led.value(1)
        time.sleep(0.15)

except KeyboardInterrupt:
    print("\n\n🛑 Test stopped")
    print("\n📊 Statistics:")
    print(f"   Successful readings: {successful}")
    print(f"   Failed readings: {failed}")
    if successful > 0:
        success_rate = (successful / (successful + failed)) * 100
        print(f"   Success rate: {success_rate:.1f}%")
        if readings:
            print(f"   Min distance: {min(readings):.1f} cm")
            print(f"   Max distance: {max(readings):.1f} cm")
            print(f"   Avg distance: {sum(readings)/len(readings):.1f} cm")
    onboard_led.value(0)