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

from cocotbext.i2c import I2cMaster, I2cMemory

i2c_addr = 0x3C

# async def cam_write_handler(dut):
#     dut.CAM_REG_WRITE_IN_PROGRESS.value = 0
#     while True:
#         await RisingEdge(dut.CAM_REG_WRITE_START)
#         await ClockCycles(dut.CLK, 25)
#         dut.CAM_REG_WRITE_IN_PROGRESS.value = 1
#         await ClockCycles(dut.CLK, 200)
#         dut.CAM_REG_WRITE_IN_PROGRESS.value = 0
#         await ClockCycles(dut.CLK, 10)

@cocotb.test()
async def test_i2c_write(dut):
    # start a clock
    cocotb.start_soon(Clock(dut.CLK, 41.66, units="ns").start()) # 24MHz clock input
    # cocotb.start_soon(cam_write_handler(dut))
    # assert a reset for a bit
    dut.RST.value = 1
    await ClockCycles(dut.CLK, 20)
    dut.RST.value = 0

    # create an I2C controller
    i2cm = I2cMaster(sda=dut.SDA_O, sda_o=dut.SDA_I, scl=dut.SCL, scl_o=dut.SCL, speed=100e3)

    # await Timer(100, 'us')
    await RisingEdge(dut.u_i2c_cam.config_finished)

    await i2cm.write(i2c_addr, b'\x00\x01')
    await i2cm.send_stop()
    dut.log.info('Wrote 0x01 to register 0x00')

    await Timer(100, 'us')

    # gotta do the repeated start stuff
    await i2cm.write(i2c_addr, b'\x00') # ask for register 0x00
    data = await i2cm.read(0x3C, 1)
    await i2cm.send_stop()

    dut.log.info(f'Read 0x{data[0]:02X} from register 0x00')

    await Timer(10,'us')

    await i2cm.write(i2c_addr, b'\x00\x30\x8C\x1D')
    await i2cm.send_stop()
    dut.log.info('Wrote 0x30,0x8C,0x1D to register 0x00')

    await Timer(10,'us')

    await i2cm.write(i2c_addr, b'\x10\x05')
    await i2cm.send_stop()
    dut.log.info('Wrote 0x05 to register 0x10')
    
    await Timer(10,'us')
    await i2cm.write(i2c_addr, b'\x10') # ask for register 0x10
    data = await i2cm.read(0x3C, 1)
    await i2cm.send_stop()

    dut.log.info(f'Read 0x{data[0]:02X} from register 0x10')

    await Timer(10,'us')

    await i2cm.write(i2c_addr, b'\x10\x03\x48')
    await i2cm.send_stop()
    dut.log.info('Wrote 0x03,0x48 to register 0x10')
    # await i2cm.write(i2c_addr, b'\x00') # ask for register 0x00
    # data = await i2cm.read(0x3C, 0x14)
    # await i2cm.send_stop()

    # for i, dat in enumerate(data):
    #     dut.log.info(f'Read 0x{dat:02X} from register 0x{i:02X}')

    await Timer(1000,'us')