from machine import Pin,PWM,freq
import rp2
import micropython
from time import sleep

micropython.alloc_emergency_exception_buf(100)
freq(200000000)
waitPin = Pin(12, Pin.IN)
intPin = Pin(13, Pin.OUT)
FDCWritePin = Pin(14, Pin.IN)  #combination of Z80 IORQ and WR pins both low
FDCReadPin = Pin(15, Pin.IN)   #combination of Z80 IORQ and RD pins both low
a7Pin = Pin(8, Pin.IN)

button_pressed_count = 0

@rp2.asm_pio(autopull=True,pull_thresh=8,autopush=True,push_thresh=32,out_shiftdir=1,out_init=(rp2.PIO.IN_HIGH, rp2.PIO.IN_HIGH,rp2.PIO.IN_HIGH, rp2.PIO.IN_HIGH,rp2.PIO.IN_HIGH, rp2.PIO.IN_HIGH,rp2.PIO.IN_HIGH, rp2.PIO.IN_HIGH))
def readFDC():
  #pull()
  #mov(y,osr)
  wrap_target()
  label('waitForIrq')
  wait(0, gpio, 15)     #io read pin
  jmp(pin,'waitForIrq') #wait for a7 to go low
  mov(osr,invert(x))
  out(pindirs,8)
  mov(osr,y)
  #push()
  #pull()
  out(pins,8)
  wait(1, gpio, 15)     #io read pin
  mov(osr,x)
  out(pindirs,8)
  wrap()


@rp2.asm_pio(autopush=True,push_thresh=8,set_init=rp2.PIO.IN_HIGH)
def writeFDC():
  
  set(pindirs,0)    #set wait pin to input
  wait(1, gpio,14)  #io write pin  
  label('begin')
  wait(0, gpio,14)  #io write pin
  jmp(pin,'begin')  #wait for a7 to go low
  in_(pins,8)      
  set(pindirs,1)    #set wait pin to output
  set(pins,0)       #set wait pin to low
  irq(block, rel(0))
  wrap_target()
  wrap()

@micropython.native
def writeFDCHandler(writeFDCSM):
  global button_pressed_count
  button_pressed_count += 1
  print('interrupt')
  led = Pin("LED", Pin.OUT)
  led.toggle()
  data = writeFDCSM.get()
  print(data)
  led.toggle()
  writeFDCSM.restart()
  return
  

writeFDCSM = rp2.StateMachine(1, writeFDC, freq=200000000,jmp_pin=Pin(8),in_base=Pin(0),set_base=Pin(12))
writeFDCSM.irq(writeFDCHandler,hard=False)
writeFDCSM.active(1)

readFDCSM = rp2.StateMachine(2, readFDC, freq=200000000,in_base=Pin(8), out_base=Pin(0), jmp_pin=Pin(8))
readFDCSM.put(0x80)
readFDCSM.exec('pull()')
readFDCSM.exec('mov(y,osr)')
readFDCSM.active(1)

print("active")


# button_pressed_count_old = 0
# while True:
#     if button_pressed_count_old != button_pressed_count:
#        print('Button 1 value:', button_pressed_count)
#        button_pressed_count_old = button_pressed_count