import machine
import utime

# Define pins for ultrasonic sensor
trigger_pin = machine.Pin(3, machine.Pin.OUT)
echo_pin = machine.Pin(2, machine.Pin.IN)

# Define pin for LED indicator - using GP19 as in the diagram
led = machine.Pin(19, machine.Pin.OUT)

# Define pins for stepper motor control (for a simple robot)
# Left stepper motor pins (naming based on Wokwi diagram)
left_motor_a_plus = machine.Pin(7, machine.Pin.OUT)  # A+
left_motor_a_minus = machine.Pin(6, machine.Pin.OUT) # A-
left_motor_b_plus = machine.Pin(8, machine.Pin.OUT)  # B+
left_motor_b_minus = machine.Pin(9, machine.Pin.OUT) # B-

# Right stepper motor pins (naming based on Wokwi diagram)
right_motor_a_plus = machine.Pin(10, machine.Pin.OUT)  # A+
right_motor_a_minus = machine.Pin(11, machine.Pin.OUT) # A-
right_motor_b_plus = machine.Pin(12, machine.Pin.OUT)  # B+
right_motor_b_minus = machine.Pin(13, machine.Pin.OUT) # B-

# Define threshold distance for collision detection (in cm)
COLLISION_THRESHOLD = 15

# Stepper motor sequence (full step)
step_sequence = [
    [1, 0, 1, 0],  # Step 1: A+ on, B+ on
    [0, 1, 1, 0],  # Step 2: A- on, B+ on
    [0, 1, 0, 1],  # Step 3: A- on, B- on
    [1, 0, 0, 1]   # Step 4: A+ on, B- on
]

def get_distance():
    """ Measure distance using the HC-SR04 ultrasonic sensor """
    trigger_pin.low()
    utime.sleep_us(2)
    trigger_pin.high()
    utime.sleep_us(10)
    trigger_pin.low()
    
    # Wait for echo to go high
    pulse_start = utime.ticks_us()
    while echo_pin.value() == 0:
        pulse_start = utime.ticks_us()
    
    # Wait for echo to go low
    pulse_end = utime.ticks_us()
    while echo_pin.value() == 1:
        pulse_end = utime.ticks_us()
    
    pulse_duration = utime.ticks_diff(pulse_end, pulse_start)
    distance = (pulse_duration * 0.0343) / 2  # Speed of sound is 343 m/s
    return distance

def stop_motors():
    """ Stop all motor movement """
    # Turn off all motor pins
    left_motor_a_plus.value(0)
    left_motor_a_minus.value(0)
    left_motor_b_plus.value(0)
    left_motor_b_minus.value(0)
    
    right_motor_a_plus.value(0)
    right_motor_a_minus.value(0)
    right_motor_b_plus.value(0)
    right_motor_b_minus.value(0)

def set_motor_step(motor, step):
    """ Set the pins for a motor according to the step sequence """
    if motor == "left":
        left_motor_a_plus.value(step[0])
        left_motor_a_minus.value(step[1])
        left_motor_b_plus.value(step[2])
        left_motor_b_minus.value(step[3])
    elif motor == "right":
        right_motor_a_plus.value(step[0])
        right_motor_a_minus.value(step[1])
        right_motor_b_plus.value(step[2])
        right_motor_b_minus.value(step[3])

def move_forward(steps=20, delay=0.01):
    """ Move both stepper motors forward """
    for _ in range(steps):
        for step in step_sequence:
            set_motor_step("left", step)
            set_motor_step("right", step)
            utime.sleep(delay)

def turn_right(steps=50, delay=0.01):
    """ 
    Make a right turn by moving left motor forward 
    and right motor backward
    """
    for _ in range(5):  # Blink LED to indicate turning
        led.toggle()
        utime.sleep(0.2)
    
    for _ in range(steps):
        for i, step in enumerate(step_sequence):
            set_motor_step("left", step)  # Left motor moves forward
            # Right motor moves backward (use reverse sequence)
            reverse_idx = (len(step_sequence) - 1) - i
            set_motor_step("right", step_sequence[reverse_idx])
            utime.sleep(delay)

    led.value(1)  # Keep LED on after turn

# Setup initial state
led.value(0)
stop_motors()

print("Collision Detection System Ready")

try:
    while True:
        # Measure distance to obstacle
        distance = get_distance()
        print(f"Distance: {distance:.1f} cm")
        
        if distance < COLLISION_THRESHOLD:
            # Collision detected
            print("⚠️ Collision Alert! Stopping and turning... ⚠️")
            led.value(1)  # Turn on LED
            stop_motors()
            utime.sleep(1)  # Longer pause before turning
            turn_right()
            stop_motors()
            utime.sleep(1)  # Pause after turning
            move_forward(10)  # Move forward a bit after turning
        else:
            # No collision detected
            led.value(0)  # Turn off LED
            move_forward(5)  # Move forward fewer steps to check more frequently
        
        utime.sleep(0.2)  # Short delay before next measurement
        
except KeyboardInterrupt:
    stop_motors()
    led.value(0)
    print("Program stopped")