from drv8825 import DRV8825
from machine import Pin
from time import sleep_ms

I2C_ADDR = 16

PIN_STEP = 26
PIN_DIR  = 27
PIN_SW   = 28

stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR)
home_sw = Pin(PIN_SW, Pin.IN, Pin.PULL_UP)

MAX_STEPS = 200 * 4
stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR)

def handle_switch_release(stepper):
    global stepper_position
    if home_sw.value() == 1:
        print("up switch released")
        stepper.stop()
        stepper_position = 0

def handle_switch_press(stepper):
    if home_sw.value() == 0:
        print("up switch hit")
        stepper.stop()
        stepper.set_timer_callback(handle_switch_release)
        stepper.freerun(stepfreq=-200)

def up():
    print("going up")
    stepper.set_timer_callback(handle_switch_press)
    stepper.freerun(stepfreq=200)

def down():
    print("going down")
    t = -MAX_STEPS
    stepper.set_timer_callback(lambda s: s.get_progress() == t and s.stop())
    stepper.steps(t)

def wait_for_stepper():
    while stepper.is_running():
        sleep_ms(500)

up()
wait_for_stepper()

from RP2040_Slave import i2c_slave

i2c = i2c_slave(i2cID=1, sda=6, scl=7, slaveAddress=I2C_ADDR)

COMMAND_OPEN_LID = "OPEN-LID"
COMMAND_CLOSE_LID = "CLOSE-LID"

RESPONSE_UNKNOWN = 0
RESPONSE_LID_OPEN = 30
RESPONSE_LID_CLOSED = 32


def handle_command(command):
    print("command", command)
    if command == COMMAND_OPEN_LID:
        up()
        wait_for_stepper()
        return RESPONSE_LID_OPEN
    if command == COMMAND_CLOSE_LID:
        down()
        wait_for_stepper()
        return RESPONSE_LID_CLOSED

    return RESPONSE_UNKNOWN

try:
    command = ''
    response = 0

    while True:
        state = i2c.handle_event()

        if state == i2c.I2CStateMachine.I2C_START:
            pass

        if state == i2c.I2CStateMachine.I2C_RECEIVE:
            while (i2c.Available()):
                command += chr(i2c.Read_Data_Received())

        if state == i2c.I2CStateMachine.I2C_REQUEST:
            while (i2c.is_Master_Req_Read()):
                i2c.Slave_Write_Data(response)

        if state == i2c.I2CStateMachine.I2C_FINISH:
            response = handle_command(command)
            command = '' # clear command for next receive

finally:
    # need this to release pins when script is stopped
    i2c.deinit()