from I2Sduplex import I2Sduplex
from rp2 import DMA
import time, uctypes, array
from machine import Pin

led = Pin('LED', Pin.OUT)

# TODO:
# 1. Use uPython RP2 DMA module. DONE
# 2. Make PIO SM choosable.

print("Initialising audio interface...")
i2s = I2Sduplex()
#DMA.abort_all()

PIO0_BASE      = 0x50200000
PIO0_BASE_TXF0 = PIO0_BASE + 0x10
PIO0_BASE_RXF0 = PIO0_BASE + 0x20
DREQ_PIO0_TX0  = 0x00
DREQ_PIO0_RX0  = 0x04
BUFFERSIZE = 64     # Size in 32-bit words

txBuf0 = bytearray(array.array('l', [x for x in range(BUFFERSIZE)]))
txBuf1 = bytearray(array.array('l', [x-BUFFERSIZE for x in range(BUFFERSIZE)]))
rxBuf0 = bytearray(BUFFERSIZE << 2)
rxBuf1 = bytearray(BUFFERSIZE << 2)

dma = [rp2.DMA() for i in range(4)]
ctrl = (dma[0].pack_ctrl(inc_write = False, chain_to = dma[1].channel, treq_sel = DREQ_PIO0_TX0),
        dma[1].pack_ctrl(inc_write = False, chain_to = dma[0].channel, treq_sel = DREQ_PIO0_TX0),
        dma[2].pack_ctrl(inc_read = False, chain_to = dma[3].channel, treq_sel = DREQ_PIO0_RX0),
        dma[3].pack_ctrl(inc_read = False, chain_to = dma[2].channel, treq_sel = DREQ_PIO0_RX0))

dma[0].config(read = txBuf0, write = PIO0_BASE_TXF0, count = BUFFERSIZE, ctrl = ctrl[0])
dma[1].config(read = txBuf1, write = PIO0_BASE_TXF0, count = BUFFERSIZE, ctrl = ctrl[1])
dma[2].config(read = PIO0_BASE_RXF0, write = rxBuf0, count = BUFFERSIZE, ctrl = ctrl[2])
dma[3].config(read = PIO0_BASE_RXF0, write = rxBuf1, count = BUFFERSIZE, ctrl = ctrl[3])

i2s.sm.active(1)

dma[0].active(1)
dma[2].active(1)

@micropython.viper
def processBuffer(rx : ptr32, tx : ptr32) -> bool:
    n = 0
    ret = True
    while n < 64:
        ret = ret and tx[n]==rx[n]
        tx[n] = tx[n] + 128
        n += 1
    return ret

nextBuffer = 1
buffsDone = 0
while(True):
    if(nextBuffer == 0 and not dma[0].active() and not dma[2].active()):
        #gp.high()
        dma[0].config(read = txBuf0)
        dma[2].config(write = rxBuf0)
        led.value(processBuffer(rxBuf0, txBuf0))
        nextBuffer = 1
        buffsDone += 1
        #gp.low()            
    if(nextBuffer == 1 and not dma[1].active() and not dma[3].active()):
        #gp.high()
        dma[1].config(read = txBuf1)
        dma[3].config(write = rxBuf1)
        led.value(processBuffer(rxBuf1, txBuf1))
        nextBuffer = 0
        buffsDone += 1
        #gp.low()



#DMA.abort_all()
#i2s.sm.active(0)