from machine import Pin
from time import sleep_ms

I2C_ADDR = 9
PIN_SERVO = 26

from RP2040_Slave import i2c_slave

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

COMMAND_DISPENSE_BATTER = "DISPENSE"

RESPONSE_UNKNOWN=0
RESPONSE_BATTER_DISPENSED=21

from servo import Servo
servo = Servo(PIN_SERVO)

dispense_time = 1500 * 4

def dispense_batter():
    print("dispense start")
    servo.write(90)
    sleep_ms(250)
    servo.off()

    sleep_ms(dispense_time)

    servo.write(0)
    sleep_ms(250)
    servo.off()

    sleep_ms(10000)
    print("dispense end")

def handle_command(command):
    print("command", command)
    if command == COMMAND_DISPENSE_BATTER:
        dispense_batter()
        return RESPONSE_BATTER_DISPENSED

    return RESPONSE_UNKNOWN

try:
    command = ''
    response = RESPONSE_UNKNOWN

    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()