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

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)

stepper_position = None
stepper = DRV8825(PIN_STEP, direction_pin=PIN_DIR)

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

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

def home():
    print("going home")
    stepper.set_timer_callback(handle_switch_press)
    stepper.freerun(stepfreq=-1000)

TRACK_LENGTH_STEPS = 96880
TRACK_LENGTH_MM = 600
STEPS_PER_MM = int(TRACK_LENGTH_STEPS / TRACK_LENGTH_MM)

def handle_goto(stepper, target, new_position):
    global stepper_position
    if stepper.get_progress() == target:
        stepper.stop() # why do I need to do this?
        stepper_position = new_position

def goto(new_position):
    if new_position > TRACK_LENGTH_STEPS:
        new_position = TRACK_LENGTH_STEPS

    target = new_position - stepper_position
    stepper.set_timer_callback(lambda s, t=target, p=new_position: handle_goto(s, t, p))
    stepper.steps(target, stepfreq=1000)
    return new_position

# go home
home()
while stepper.is_running():
    sleep_ms(500)

from RP2040_Slave import i2c_slave

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

POSITION_PAN1_BATTER = 150 * STEPS_PER_MM
POSITION_PAN2_BATTER = 290 * STEPS_PER_MM
POSITION_PANS_IN_HEATER = 390 * STEPS_PER_MM
POSITION_PANS_UNDER_DISPENSER = TRACK_LENGTH_MM * STEPS_PER_MM
POSITION_PANS_SERVE = TRACK_LENGTH_MM * STEPS_PER_MM

COMMAND_READY = "READY"
COMMAND_PAN1_BATTER = "POSITION-PAN1"
COMMAND_PAN2_BATTER = "POSITION-PAN2"
COMMAND_PANS_IN_HEATER = "POSITION-PANS-IN-HEATER"
COMMAND_PANS_UNDER_DISPENSER = "POSITION-PANS-UNDER-DISPENSER"
COMMAND_PANS_SERVE = "POSITION-PANS-SERVE"

RESPONSE_UNKNOWN=0
RESPONSE_PAN1_BATTER=20
RESPONSE_PAN2_BATTER=22
RESPONSE_PANS_IN_HEATER=31
RESPONSE_PANS_UNDER_DISPENSER=41
RESPONSE_PANS_SERVE=44 # ??

COMMANDS = {
    "STATUS": (POSITION_PAN1_BATTER, RESPONSE_PAN1_BATTER),
    COMMAND_PAN1_BATTER: (POSITION_PAN1_BATTER, RESPONSE_PAN1_BATTER),
    COMMAND_PAN2_BATTER: (POSITION_PAN2_BATTER, RESPONSE_PAN2_BATTER),
    COMMAND_PANS_IN_HEATER: (POSITION_PANS_IN_HEATER, RESPONSE_PANS_IN_HEATER),
    COMMAND_PANS_UNDER_DISPENSER: (POSITION_PANS_UNDER_DISPENSER, RESPONSE_PANS_UNDER_DISPENSER),
    COMMAND_PANS_SERVE: (POSITION_PANS_SERVE, RESPONSE_PANS_SERVE)
}

current_command = None

def handle_command(command):
    global current_command

    pos, res = COMMANDS.get(command) or (None, None)
    if pos is not None and not stepper.is_running():
        print("new command", command, (pos, res))
        current_command = command
        goto(pos)

    pos, res = COMMANDS.get(current_command) or (None, None)
    if pos is not None and pos == stepper_position:
        print("response", res)
        current_command = None
        return res

    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 killed
    i2c.deinit()