import array
import time
import board
import rp2pio
import adafruit_pioasm
from digitalio import DigitalInOut, Pull, DriveMode

#    set pins, 0   ; switch off counter sensor

# led = DigitalInOut(board.GP19)
# led.switch_to_output(value=False, drive_mode=DriveMode.PUSH_PULL)
# print(led.value)

# state = DigitalInOut(board.GP19)
# state.switch_to_input(pull=Pull.UP)
# state.switch_to_input(pull=None)


fb1 = """
.program fallblatt1
    mov x, x
"""

fb0 = """
.program zweites
    mov y, y
    nop
"""

fallblatt = """
.wrap_target

GetData:
    pull block              ; take count to osr
    mov x, osr              ; and store it in x
    pull block              ; take speed to osr

Start:
    set pins, 0b01          ; load pattern for Motor
    jmp pin, StepMotor1     ; check for Q status

; Q=0
StepMotor0:
    mov y, osr              ; in osr is the speed constant
LoopMotor0:
    jmp pin, Edge01         ; wenn Q = 1 goto Edge01
    jmp ContMotor0          ; Q noch 0, delay loop
ContMotor0:
    jmp y--, LoopMotor0     ; jump if y <> 0
    mov pins, ~pins         ; invert motor bits
    jmp StepMotor0          ;

Edge01:
    jmp x--, ContMotor1     ; jump if x <> 0
    jmp Stop

; Q=1
StepMotor1:
    mov y, osr              ; in osr is the speed constant
LoopMotor1:
    jmp pin, ContMotor1     ; solange Q = 1 warte auf timeout
    jmp Edge10              ; Q is 0 now, goto Edge10
ContMotor1:
    jmp y--, LoopMotor1     ; jump if y <> 0
    mov pins, ~pins         ; invert motor bits
    jmp StepMotor1          ;

Edge10:
    jmp x--, ContMotor0     ; jump if x <> 0
    ;jmp Stop

Stop:
    set pins, 0b00          ; load pattern for Motor

.wrap
"""

query = """
.program query
.wrap_target
    pull block
    out pins, 6
    in  pins, 6
    push block
.wrap
"""

pioprog0 = adafruit_pioasm.Program(query)

sm0 = rp2pio.StateMachine(
    pioprog0.assembled,
    frequency = 100000,
    first_in_pin = board.GP6,
    in_pin_count = 6,
    first_out_pin = board.GP0,
    out_pin_count = 6,
    initial_out_pin_state = 0,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog0.pio_kwargs
)
for key, value in pioprog0.pio_kwargs.items():
    print("sm0: %s == %s" % (key, value))
print("real frequency0: ", sm0.frequency)


pioprog1 = adafruit_pioasm.Program(fb1 + fallblatt)
sm1 = rp2pio.StateMachine(
    pioprog1.assembled,
    frequency = 2000,
    first_in_pin = board.GP12,
    in_pin_count = 2,
    first_set_pin = board.GP12,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP12,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP6,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog1.pio_kwargs
)
for key, value in pioprog1.pio_kwargs.items():
    print("sm1: %s == %s" % (key, value))
print("real frequency1: ", sm1.frequency)


# pioprog2 = adafruit_pioasm.Program(fb1 + fallblatt)
sm2 = rp2pio.StateMachine(
    program = pioprog1.assembled,
    frequency = 2000,
    first_in_pin = board.GP14,
    in_pin_count = 2,
    first_set_pin = board.GP14,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP14,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP7,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog1.pio_kwargs
)
for key, value in pioprog1.pio_kwargs.items():
    print("sm2: %s == %s" % (key, value))
print("real frequency2: ", sm2.frequency)


pioprog3 = adafruit_pioasm.Program(fb0 + fallblatt)
sm3 = rp2pio.StateMachine(
    program = pioprog3.assembled,
    frequency = 2000,
    first_in_pin = board.GP16,
    in_pin_count = 2,
    first_set_pin = board.GP16,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP16,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP8,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog3.pio_kwargs
)
for key, value in pioprog3.pio_kwargs.items():
    print("sm3: %s == %s" % (key, value))
print("real frequency3: ", sm3.frequency)


# pioprog4 = adafruit_pioasm.Program(fb0 + fallblatt)
sm4 = rp2pio.StateMachine(
    pioprog3.assembled,
    frequency = 2000,
    pio = 0,
    first_in_pin = board.GP18,
    in_pin_count = 2,
    first_set_pin = board.GP18,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP18,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP9,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog3.pio_kwargs
)
for key, value in pioprog3.pio_kwargs.items():
    print("sm4: %s == %s" % (key, value))
print("real frequency4: ", sm4.frequency)


# pioprog5 = adafruit_pioasm.Program(fb0 + fallblatt)
sm5 = rp2pio.StateMachine(
    pioprog3.assembled,
    frequency = 2000,
    first_in_pin = board.GP20,
    in_pin_count = 2,
    first_set_pin = board.GP20,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP20,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP10,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog3.pio_kwargs
)
for key, value in pioprog3.pio_kwargs.items():
    print("sm5: %s == %s" % (key, value))
print("real frequency5: ", sm5.frequency)


# pioprog6 = adafruit_pioasm.Program(fb0 + fallblatt)
sm6 = rp2pio.StateMachine(
    pioprog3.assembled,
    frequency = 2000,
    first_in_pin = board.GP26,
    in_pin_count = 2,
    first_set_pin = board.GP26,
    set_pin_count = 2,
    initial_set_pin_state = 0,
    first_out_pin = board.GP26,
    out_pin_count = 2,
    initial_out_pin_state = 0,
    jmp_pin = board.GP11,
    exclusive_pin_use = False,
    wait_for_txstall = False,
    **pioprog3.pio_kwargs
)
for key, value in pioprog3.pio_kwargs.items():
    print("sm6: %s == %s" % (key, value))
print("real frequency6: ", sm6.frequency)


time.sleep(5)


data = array.array("I", [0]) # count
sm1.write(data)
data = array.array("I", [400]) # speed
sm1.write(data)

result = array.array("B", [0])

cnt = 1
while True:
    time.sleep(0.5)
    data = array.array("B", [0b00001101]) # count
    sm0.write(data)
    while sm0.in_waiting == 0:
        pass
    sm0.readinto(result)
    print("result: ", cnt, bin(result[0]))
    cnt += 1

#