from machine import Pin, UART
import rp2
import time

# Define PIO program for controlling stepper motor (Clockwise)
@rp2.asm_pio(set_init=(rp2.PIO.OUT_LOW,)*4)
def stepper_clockwise():
    label("start")
    
    # Step 1: Pin 21 (A-negative) = High, others = Low
    set(pins, 2)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay1")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay1")
    
    # Step 2: Pin 20 (A-positive) = High, others = Low
    set(pins, 8)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay2")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay2")
    
    # Step 3: Pin 19 (B-positive) = High, others = Low
    set(pins, 1)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay3")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay3")
    
    # Step 4: Pin 18 (B-negative) = High, others = Low
    set(pins, 4)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay4")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay4")

# Define PIO program for controlling stepper motor (Counterclockwise)
@rp2.asm_pio(set_init=(rp2.PIO.OUT_LOW,)*4)
def stepper_counterclockwise():
    label("start")
    
    # Step 1: Pin 21 (A-negative) = High, others = Low
    set(pins, 4)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay1")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay1")
    
    # Step 2: Pin 20 (A-positive) = High, others = Low
    set(pins, 1)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay2")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay2")
    
    # Step 3: Pin 19 (B-positive) = High, others = Low
    set(pins, 8)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay3")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay3")
    
    # Step 4: Pin 18 (B-negative) = High, others = Low
    set(pins, 2)       
    set(x, 5)          # Delay loop (5 * 32 cycles = 160 cycles)
    label("delay4")
    nop() [31]         # Delay loop
    jmp(x_dec, "delay4")

# Create the state machine for controlling stepper motor
def activate_stepper(sm_program):
    sm = rp2.StateMachine(0, sm_program, freq=2000, set_base=Pin(18))  # Pins 18-21 for stepper motor, 10Hz frequency
    sm.active(1)
    return sm

# Initialize the state machine for stepper (default to clockwise)
sm = activate_stepper(stepper_clockwise)
current_direction = 'clockwise'  # Default direction

# Main loop to simulate UART RX behavior for user input and control stepper motor direction
while True:
    user_input = input("\nEnter 'A' for clockwise\nEnter other 'character' for counterclockwise: ").strip()

    if user_input == 'A':  # If 'A' is entered, run clockwise
        if current_direction != 'clockwise':
            print("Clockwise")
            sm.active(0)  # Deactivate current state machine
            sm = activate_stepper(stepper_clockwise)  # Activate clockwise
            current_direction = 'clockwise'
    else:  # Otherwise, run counterclockwise
        if current_direction != 'counterclockwise':
            print("Counterclockwise")
            sm.active(0)  # Deactivate current state machine
            sm = activate_stepper(stepper_counterclockwise)  # Activate counterclockwise
            current_direction = 'counterclockwise'

    time.sleep(0.1)  # Small delay to prevent excessive CPU usage