"""Tutorial 04 — Autonomous Mapping (Ultrasonic Scanning)

Hardware notes (IMPORTANT):
- HC-SR04 is a 5V device. The ECHO pin returns 5V and MUST be level-shifted to 3.3V for Raspberry Pi GPIO.
  Use a simple voltage divider (e.g., 1k + 2k) or a logic level shifter.
- Suggested GPIO pins (BCM):
  TRIG=23, ECHO=24, SERVO=18 (PWM)
"""

import time
import numpy as np
import RPi.GPIO as GPIO

TRIG = 23
ECHO = 24
SERVO = 18

GPIO.setmode(GPIO.BCM)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(SERVO, GPIO.OUT)

pwm = GPIO.PWM(SERVO, 50)  # 50Hz for SG90
pwm.start(7.5)

def get_distance_cm(timeout=0.02):
    """Return distance in cm (float)."""
    GPIO.output(TRIG, False)
    time.sleep(0.0002)

    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)

    start = time.time()
    while GPIO.input(ECHO) == 0:
        if time.time() - start > timeout:
            return None
    pulse_start = time.time()

    while GPIO.input(ECHO) == 1:
        if time.time() - pulse_start > timeout:
            return None
    pulse_end = time.time()

    duration = pulse_end - pulse_start
    distance_cm = duration * 17150  # speed of sound /2 in cm/s
    return round(distance_cm, 1)

def set_angle(angle_deg):
    """Set servo angle (approx)."""
    angle_deg = max(0, min(180, angle_deg))
    duty = 2 + (angle_deg / 18)  # rough SG90 mapping
    pwm.ChangeDutyCycle(duty)
    time.sleep(0.25)
    pwm.ChangeDutyCycle(0)

def scan_sweep(angles=(30, 60, 90, 120, 150)):
    """Return list of (angle, distance_cm)."""
    results = []
    for a in angles:
        set_angle(a)
        d = get_distance_cm()
        results.append((a, d))
    return results

def choose_direction(scan):
    """Pick direction based on max distance."""
    # Treat None as 0cm
    safe = [(a, d if d is not None else 0) for a, d in scan]
    best = max(safe, key=lambda x: x[1])
    return best[0], best[1]

# Simple 10x10 grid placeholder (expand in future tutorials)
grid = np.zeros((10, 10))

try:
    print("Starting sweep...")
    scan = scan_sweep()
    for a, d in scan:
        print(f"Angle {a:3d}°  Distance: {d} cm")
    best_angle, best_dist = choose_direction(scan)
    print(f"Best direction: {best_angle}° (distance {best_dist} cm)")

finally:
    pwm.stop()
    GPIO.cleanup()