import cocotb
from cocotb.triggers import Timer, RisingEdge, FallingEdge, ClockCycles, Join, First
from cocotb.clock import Clock

async def sof_handler(dut):
    dut.SOF_I.value = 0
    # fires a start of frame (SOF) every microframe
    # which is 125us
    while True:
        await Timer(125,'us')
        await RisingEdge(dut.CLK_I) # ensure synchronous with clock
        dut.SOF_I.value = 1
        await RisingEdge(dut.CLK_I)
        dut.SOF_I.value = 0

async def fifo_handler(dut):
    wrbytes = 0
    while True:
        await RisingEdge(dut.CLK_I)
        # handle writes and the almost full, empty flags
        if dut.DVAL_O.value == 1:
            wrbytes = wrbytes + 1
        if wrbytes == 0:
            dut.FIFO_EMPTY_I.value = 1
            dut.FIFO_AFULL_I.value = 0
        elif wrbytes >= 3072:
            dut.FIFO_EMPTY_I.value = 0
            dut.FIFO_AFULL_I.value = 1
        else:
            dut.FIFO_EMPTY_I.value = 0
            dut.FIFO_AFULL_I.value = 0
        # handle outputs to USB
        # assume that every SOF means we can dump 1024/2048/3072 bytes
        if dut.SOF_I.value == 1:
            if wrbytes >= 3072:
                wrbytes -= 3072
            else:
                wrbytes = 0

@cocotb.test()
async def test_i2c_write(dut):
    # assure safe start values
    dut.SOF_I.value = 0

    dut.FIFO_EMPTY_I.value = 0
    dut.FIFO_AFULL_I.value = 0
    
    # start a clock
    cocotb.start_soon(Clock(dut.CLK_I, 20.0, units="ns").start()) # 50MHz clock input
    # cocotb.start_soon(cam_write_handler(dut))
    # assert a reset for a bit
    dut.RST_I.value = 1
    await ClockCycles(dut.CLK_I, 20)
    dut.RST_I.value = 0

    # start the other processes
    cocotb.start_soon(fifo_handler(dut))
    cocotb.start_soon(sof_handler(dut))



    await Timer(5000,'us')