"""
Complete Sensor Diagnostic Test
Tests PIR sensor, Ultrasonic sensor, and White LED headlights
"""

from machine import Pin
import utime
import time

print("=" * 60)
print("🔧 AURORA SENSOR DIAGNOSTIC TEST")
print("=" * 60)

# Pin configuration
PIR_PIN = 16
ULTRASONIC_TRIG_PIN = 14
ULTRASONIC_ECHO_PIN = 15
WHITE_LED_1_PIN = 17
WHITE_LED_2_PIN = 19

# Initialize pins
print("\n📋 Initializing hardware...")
pir = Pin(PIR_PIN, Pin.IN, Pin.PULL_DOWN)
trig = Pin(ULTRASONIC_TRIG_PIN, Pin.OUT)
echo = Pin(ULTRASONIC_ECHO_PIN, Pin.IN)
led1 = Pin(WHITE_LED_1_PIN, Pin.OUT)
led2 = Pin(WHITE_LED_2_PIN, Pin.OUT)
onboard = Pin("LED", Pin.OUT)

# Turn everything off initially
trig.value(0)
led1.value(0)
led2.value(0)
onboard.value(1)  # Onboard ON = test running

print("✅ Hardware initialized")

# ============================================================================
# TEST 1: White LED Headlights
# ============================================================================
print("\n" + "=" * 60)
print("TEST 1: White LED Headlights")
print("=" * 60)
print("\n👀 Watch the white LEDs...")

print("\n1️⃣ Testing LED 1 (GP17)...")
for i in range(3):
    led1.value(1)
    print(f"   LED 1 ON  (blink {i+1}/3)")
    time.sleep(0.5)
    led1.value(0)
    print(f"   LED 1 OFF")
    time.sleep(0.5)

print("\n2️⃣ Testing LED 2 (GP19)...")
for i in range(3):
    led2.value(1)
    print(f"   LED 2 ON  (blink {i+1}/3)")
    time.sleep(0.5)
    led2.value(0)
    print(f"   LED 2 OFF")
    time.sleep(0.5)

print("\n3️⃣ Testing BOTH LEDs together...")
for i in range(3):
    led1.value(1)
    led2.value(1)
    print(f"   BOTH ON  (blink {i+1}/3)")
    time.sleep(0.5)
    led1.value(0)
    led2.value(0)
    print(f"   BOTH OFF")
    time.sleep(0.5)

input_result = input("\n❓ Did you see the white LEDs blink? (y/n): ")
if input_result.lower() != 'y':
    print("\n⚠️  LED PROBLEM DETECTED!")
    print("   Check wiring:")
    print("   - GP17 → Resistor → LED 1 (+) → LED 1 (-) → GND")
    print("   - GP19 → Resistor → LED 2 (+) → LED 2 (-) → GND")
    print("   - Verify resistor values (220Ω - 1KΩ)")
    print("   - Check LED polarity (long leg = +)")
else:
    print("✅ LEDs working correctly!")

# ============================================================================
# TEST 2: HC-SR04 Ultrasonic Sensor
# ============================================================================
print("\n" + "=" * 60)
print("TEST 2: HC-SR04 Ultrasonic Sensor")
print("=" * 60)
print("\n📏 Testing distance sensor...")
print("   Place your hand 10-50cm in front of sensor")
time.sleep(2)

def measure_distance():
    """Measure distance in cm"""
    try:
        trig.value(0)
        utime.sleep_us(2)
        trig.value(1)
        utime.sleep_us(10)
        trig.value(0)
        
        timeout = 30000
        start = utime.ticks_us()
        
        while echo.value() == 0:
            if utime.ticks_diff(utime.ticks_us(), start) > timeout:
                return -1, "Timeout waiting for echo start"
        
        pulse_start = utime.ticks_us()
        
        while echo.value() == 1:
            if utime.ticks_diff(utime.ticks_us(), pulse_start) > timeout:
                return -1, "Timeout waiting for echo end"
        
        pulse_end = utime.ticks_us()
        pulse_duration = utime.ticks_diff(pulse_end, pulse_start)
        distance = (pulse_duration * 0.0343) / 2
        
        if distance > 400:
            return -1, "Out of range (>400cm)"
        
        return distance, "OK"
    except Exception as e:
        return -1, str(e)

print("\n🔊 Taking 5 test readings...\n")
successful = 0
for i in range(5):
    dist, status = measure_distance()
    if dist > 0:
        successful += 1
        print(f"   Reading {i+1}: {dist:6.1f} cm  ✅ {status}")
    else:
        print(f"   Reading {i+1}: FAILED     ❌ {status}")
    time.sleep(0.2)

if successful == 0:
    print("\n❌ ULTRASONIC SENSOR NOT WORKING!")
    print("\n   Troubleshooting steps:")
    print("   1. Check wiring:")
    print("      - GP14 (TRIG) → HC-SR04 TRIG pin")
    print("      - GP15 (ECHO) → HC-SR04 ECHO pin")
    print("      - 5V (VBUS) → HC-SR04 VCC pin")
    print("      - GND → HC-SR04 GND pin")
    print("\n   2. Verify HC-SR04 has power (some have LED indicator)")
    print("\n   3. Some HC-SR04 modules need 5V ECHO signal converted to 3.3V")
    print("      If your module outputs 5V on ECHO:")
    print("      - Add voltage divider: ECHO → 2KΩ → GP15 → 1KΩ → GND")
    print("      - Or use a level shifter module")
    print("\n   4. Test module with multimeter:")
    print("      - VCC should have 5V")
    print("      - ECHO should pulse when triggered")
elif successful < 3:
    print(f"\n⚠️  Unreliable readings ({successful}/5 successful)")
    print("   - Check wiring connections are secure")
    print("   - Sensor should face clear open area (not metal/carpet)")
    print("   - Try adjusting sensor angle")
else:
    print(f"\n✅ Ultrasonic sensor working! ({successful}/5 readings successful)")

# ============================================================================
# TEST 3: PIR Motion Sensor
# ============================================================================
print("\n" + "=" * 60)
print("TEST 3: HC-SR501 PIR Motion Sensor")
print("=" * 60)
print("\n🚶 Testing motion detection...")
print("   ⏰ PIR sensors need 30-60 seconds warm-up time")
print("   ⏳ Waiting 30 seconds...")

for i in range(30, 0, -5):
    print(f"   {i} seconds remaining...")
    time.sleep(5)

print("\n✅ Warm-up complete!")
print("\n   Now wave your hand in front of the PIR sensor")
print("   When motion detected: White LEDs will turn ON")
print("   Press Ctrl+C to stop test\n")

motion_count = 0
no_motion_time = 0

try:
    while True:
        pir_value = pir.value()
        
        if pir_value == 1:
            motion_count += 1
            no_motion_time = 0
            led1.value(1)
            led2.value(1)
            print(f"🚨 MOTION DETECTED! White LEDs ON (Detection #{motion_count})")
            time.sleep(0.5)  # Debounce
        else:
            led1.value(0)
            led2.value(0)
            no_motion_time += 1
            
            if no_motion_time % 10 == 0:  # Every 5 seconds
                print(f"   Waiting for motion... ({no_motion_time//2}s elapsed)")
            time.sleep(0.5)
        
        # Heartbeat on onboard LED
        onboard.value(not onboard.value())

except KeyboardInterrupt:
    print("\n\n🛑 Test stopped")
    if motion_count == 0:
        print("\n❌ PIR SENSOR NOT DETECTING!")
        print("\n   Troubleshooting steps:")
        print("   1. Check wiring:")
        print("      - GP16 → PIR OUT pin")
        print("      - 5V (VBUS) → PIR VCC pin")
        print("      - GND → PIR GND pin")
        print("\n   2. Verify PIR jumper settings:")
        print("      - Set to 'H' (repeatable trigger mode)")
        print("      - NOT 'L' (single trigger)")
        print("\n   3. Adjust PIR potentiometers:")
        print("      - Sx (Sensitivity): Turn CLOCKWISE for more sensitivity")
        print("      - Tx (Time delay): Turn COUNTER-CLOCKWISE for shorter delay")
        print("\n   4. PIR sensor MUST have warm-up time (30-60 seconds)")
        print("\n   5. Test PIR voltage:")
        print("      - VCC should show ~5V")
        print("      - OUT pin should go HIGH (3.3V) when motion detected")
    else:
        print(f"\n✅ PIR sensor working! ({motion_count} detections)")
    
    # Turn off all LEDs
    led1.value(0)
    led2.value(0)
    onboard.value(0)

print("\n" + "=" * 60)
print("✅ DIAGNOSTIC TEST COMPLETE")
print("=" * 60)
n