import struct
import cocotb
import queue
from cocotb.triggers import RisingEdge


def find_handle(root, path):
    """ Find a hierachy node given a list of node names
        super hacky around cocotb's broken generate/block support
        ampl_lut = find_handle(dut.dut, ['chans(0)', 'chan', 'netan_gen', 'ampl_lut'])
    """
    root._discover_all()
    if not path:
        return root
    return find_handle(root._sub_handles[path.pop(0)], path)


async def _fifo_write(data, clk, en, din, full, overflow=None,
                      prog_full=None, prog_full_threshold=0):
    while True:
        await RisingEdge(clk)
        if en.value:
            data.put_nowait(din.value)

        full.value = int(data.full())  # TODO the delay thing
        if overflow is not None:
            if en.value.is_resolvable:
                overflow.value = int(en.value and data.full())

        if prog_full is not None:
            prog_full.value = int(data.qsize() > prog_full_threshold)


async def _fifo_read(data, clk, en, dout, empty, valid=None, length=1024,
                     prog_empty=None, prog_empty_threshold=0):
    rdq = queue.Queue(length)

    async def _dly(val):
        await RisingEdge(clk)
        await RisingEdge(clk)
        rdq.put_nowait(val)

    while True:
        await RisingEdge(clk)
        empty.value = int(rdq.empty())

        if prog_empty is not None:
            prog_empty.value = int(rdq.qsize() < prog_empty_threshold)

        if not data.empty() and not rdq.full():
            cocotb.start_soon(_dly(data.get_nowait()))

        if en.value:
            if rdq.empty():
                dout.binstr = 'zzzzzzzz'
            else:
                dout.value = rdq.get_nowait()
                if valid is not None:
                    valid.value = 1
        else:
            if valid is not None:
                valid.value = 0


def sim_fifo(dut, length=1024):
    # rst : in std_logic;
    # wr_clk : in std_logic;
    # rd_clk : in std_logic;
    # din : in std_logic_vector(47 downto 0);
    # wr_en : in std_logic;
    # rd_en : in std_logic;
    # dout : out std_logic_vector(11 downto 0);
    # full : out std_logic;
    # empty : out std_logic;
    # valid : out std_logic
    data = queue.Queue(length)
    cocotb.start_soon(_fifo_write(data, dut.wr_clk, dut.wr_en, dut.din, dut.full))
    cocotb.start_soon(_fifo_read(data, dut.rd_clk, dut.rd_en, dut.dout, dut.empty, length=length))



def cbuf_fifo(handle, length=1024):
    # rst : in std_logic;
    # wr_clk : in std_logic;
    # rd_clk : in std_logic;
    # din : in std_logic_vector(107 downto 0);
    # wr_en : in std_logic;
    # rd_en : in std_logic;
    # dout : out std_logic_vector(107 downto 0);
    # full : out std_logic;
    # overflow : out std_logic;
    # empty : out std_logic;
    # prog_full : out std_logic;
    # prog_empty : out std_logic;
    # wr_rst_busy : out std_logic;
    # rd_rst_busy : out std_logic
    data = queue.Queue(length)  # TODO reset
    cocotb.start_soon(_fifo_write(data, handle.wr_clk, handle.wr_en, handle.din, handle.full,
                      handle.overflow, handle.prog_full, prog_full_threshold=length // 2))
    cocotb.start_soon(_fifo_read(data, handle.rd_clk, handle.rd_en, handle.dout, handle.empty, length=length,
                      prog_empty=handle.prog_empty, prog_empty_threshold=16))
    handle.wr_rst_busy.value = 0
    handle.rd_rst_busy.value = 0


def sim_fft(dut):
    # aclk : in std_logic;
    # aresetn : in std_logic;
    # s_axis_config_tdata : in std_logic_vector(7 downto 0);
    # s_axis_config_tvalid : in std_logic;
    # s_axis_config_tready : out std_logic;
    # s_axis_data_tdata : in std_logic_vector(31 downto 0);
    # s_axis_data_tvalid : in std_logic;
    # s_axis_data_tready : out std_logic;
    # s_axis_data_tlast : in std_logic;
    # m_axis_data_tdata : out std_logic_vector(63 downto 0);
    # m_axis_data_tuser : out std_logic_vector(15 downto 0);
    # m_axis_data_tvalid : out std_logic;
    # m_axis_data_tready : in std_logic;
    # m_axis_data_tlast : out std_logic;
    # event_frame_started : out std_logic;
    # event_tlast_unexpected : out std_logic;
    # event_tlast_missing : out std_logic;
    # event_status_channel_halt : out std_logic;
    # event_data_in_channel_halt : out std_logic;
    # event_data_out_channel_halt : out std_logic

    dut.s_axis_config_tready.value = 0
    dut.s_axis_data_tready.value = 0
    dut.m_axis_data_tdata.value = 0
    dut.m_axis_data_tuser.value = 0
    dut.m_axis_data_tvalid.value = 0
    dut.m_axis_data_tlast.value = 0
    dut.event_frame_started.value = 0
    dut.event_tlast_unexpected.value = 0
    dut.event_tlast_missing.value = 0
    dut.event_status_channel_halt.value = 0
    dut.event_data_in_channel_halt.value = 0
    dut.event_data_out_channel_halt.value = 0


async def _ram(data, clk, addr, dout=None, en=None, we=None, din=None):
    async def _dly(a, val):
        await RisingEdge(clk)
        data[a] = val

    while True:
        try:
            await RisingEdge(clk)
            if en is None or en.value:
                a = addr.value
                if dout:
                    dout.value = data[a]
                if we is not None and we.value:
                    cocotb.start_soon(_dly(a, din.value))
        except ValueError:
            pass


def arbwave_buffer(handle):
    # clka : in std_logic;
    # ena : in std_logic;
    # wea : in std_logic_vector(0 to 0);
    # addra : in std_logic_vector(12 downto 0);
    # dina : in std_logic_vector(17 downto 0);
    # douta : out std_logic_vector(17 downto 0);
    # clkb : in std_logic;
    # enb : in std_logic;
    # web : in std_logic_vector(0 to 0);
    # addrb : in std_logic_vector(12 downto 0);
    # dinb : in std_logic_vector(17 downto 0);
    # doutb : out std_logic_vector(17 downto 0)

    data = [0] * 0x1000
    cocotb.start_soon(_ram(data, handle.clka, handle.addra, handle.douta, handle.ena, handle.wea, handle.dina, "READ_FIRST",
                                 handle.clkb, handle.addrb, handle.doutb, handle.enb, handle.web, handle.dinb, "READ_FIRST"))


def sin_lut(handle):
    # clka : in std_logic;
    # clkb : in std_logic;
    # addra : in std_logic_vector(9 downto 0);
    # douta : out std_logic_vector(17 downto 0);
    # addrb : in std_logic_vector(9 downto 0);
    # doutb : out std_logic_vector(17 downto 0)
    import sin_lut
    data = sin_lut.gen_lut(length=2**10, bitdepth=18)
    cocotb.start_soon(_ram(data, handle.clka, handle.addra, handle.douta, handle.ena, None, None, "WRITE_FIRST",
                                 handle.clkb, handle.addrb, handle.doutb, handle.enb, None, None, "WRITE_FIRST"))


def window_lut(handle):
    # clka : in std_logic;
    # ena : in std_logic;
    # wea : in std_logic_vector(0 to 0);
    # addra : in std_logic_vector(12 downto 0);
    # dina : in std_logic_vector(17 downto 0);
    # douta : out std_logic_vector(17 downto 0);
    # clkb : in std_logic;
    # enb : in std_logic;
    # web : in std_logic_vector(0 to 0);
    # addrb : in std_logic_vector(12 downto 0);
    # dinb : in std_logic_vector(17 downto 0);
    # doutb : out std_logic_vector(17 downto 0)
    import window_functions
    data = window_functions.bhwindow()
    cocotb.start_soon(_ram(data, handle.clka, handle.addra, handle.douta, handle.ena, handle.wea, handle.dina, "WRITE_FIRST",
                                 handle.clkb, handle.addrb, handle.doutb, handle.enb, handle.web, handle.dinb, "READ_FIRST"))


def accum_buffer(handle):
    # clka : in std_logic;
    # wea : in std_logic_vector(0 downto 0);
    # addra : in std_logic_vector(11 downto 0);
    # dina : in std_logic_vector(53 downto 0);
    # clkb : in std_logic;
    # addrb : in std_logic_vector(11 downto 0);
    # doutb : out std_logic_vector(53 downto 0)

    data = [0] * 0x1000
    cocotb.start_soon(_ram(data,
        handle.clka, handle.addra, None, None, handle.wea, handle.dina, "READ_FIRST",
        handle.clkb, handle.addrb, handle.doutb, None, None, None, "READ_FIRST"))
