`timescale 1ns/1ps

module tb_control;

    localparam int unsigned DAC_DATA_WIDTH = 12;

    
    // Clocks / reset
    logic eth_clk_in;
    logic dac_clk_in;
    logic adc_clk_in;
    logic rst_n;

    // axi stream (input)
    logic [7:0] s_axis_tdata;
    logic       s_axis_tvalid;
    logic       s_axis_tready;
    logic       s_axis_tlast;

    // ADC side input
    logic finish;

    // DUT outputs
    logic [31:0]                dac_pulse_width;
    logic [31:0]                dac_pulse_period;
    logic [DAC_DATA_WIDTH-1:0]  dac_pulse_height;
    logic [15:0]                dac_pulse_num;

    logic [31:0]                adc_pulse_period;
    logic [15:0]                adc_pulse_num;

    logic dac_start;
    logic adc_start;
    logic dac_rst;
    logic adc_rst;

    
    // DUT
    control #(
        .DAC_DATA_WIDTH(DAC_DATA_WIDTH)
    ) dut (
        .eth_clk_in        (eth_clk_in),
        .dac_clk_in        (dac_clk_in),
        .adc_clk_in        (adc_clk_in),
        .rst_n             (rst_n),

        .s_axis_tdata      (s_axis_tdata),
        .s_axis_tvalid     (s_axis_tvalid),
        .s_axis_tready     (s_axis_tready),
        .s_axis_tlast      (s_axis_tlast),

        .finish            (finish),

        .dac_pulse_width   (dac_pulse_width),
        .dac_pulse_period  (dac_pulse_period),
        .dac_pulse_height  (dac_pulse_height),
        .dac_pulse_num     (dac_pulse_num),

        .adc_pulse_period  (adc_pulse_period),
        .adc_pulse_num     (adc_pulse_num),

        .dac_start         (dac_start),
        .adc_start         (adc_start),
        .dac_rst           (dac_rst),
        .adc_rst           (adc_rst)
    );

    
    // Clock generation
    initial begin
        eth_clk_in = 1'b0;
        forever #(1 * 4.000) eth_clk_in = ~eth_clk_in;   // 125 MHz
    end

    initial begin
        dac_clk_in = 1'b0;
        forever #(1 * 3.846153846) dac_clk_in = ~dac_clk_in; // ~130 MHz
    end

    initial begin
        adc_clk_in = 1'b0;
        forever #(1 * 7.692307692) adc_clk_in = ~adc_clk_in; // ~65 MHz
    end

    
    // pulse counters and monitors for testing
    int dac_rst_count;
    int adc_rst_count;
    int dac_start_count;
    int adc_start_count;

    always_ff @(posedge dac_clk_in) begin
        if (!rst_n) begin
            dac_rst_count   <= 0;
            dac_start_count <= 0;
        end else begin
            if (dac_rst)   dac_rst_count   <= dac_rst_count + 1;
            if (dac_start) dac_start_count <= dac_start_count + 1;
        end
    end

    always_ff @(posedge adc_clk_in) begin
        if (!rst_n) begin
            adc_rst_count   <= 0;
            adc_start_count <= 0;
        end else begin
            if (adc_rst)   adc_rst_count   <= adc_rst_count + 1;
            if (adc_start) adc_start_count <= adc_start_count + 1;
        end
    end

    
    // some helpers for axi
    task automatic axis_send_byte(input logic [7:0] data, input logic last);
        begin
            @(negedge eth_clk_in);
            s_axis_tdata  <= data;
            s_axis_tvalid <= 1'b1;
            s_axis_tlast  <= last;

            @(posedge eth_clk_in);
            while (!s_axis_tready) begin
                @(posedge eth_clk_in);
            end

            s_axis_tvalid <= 1'b0;
            s_axis_tlast  <= 1'b0;
            s_axis_tdata  <= '0;
        end
    endtask

    task automatic send_cmd(input logic [7:0] cmd);
        begin
            axis_send_byte(cmd, 1'b1);
        end
    endtask

    task automatic send_set_data(
        input logic [31:0] pulse_width,
        input logic [31:0] pulse_period,
        input logic [15:0] pulse_num,
        input logic [15:0] pulse_height_raw
    );
        logic [95:0] payload;
        int i;
        begin
            // little-endian payload layout:
            // [31:0]   pulse_width
            // [63:32]  pulse_period
            // [79:64]  pulse_num
            // [95:80]  pulse_height_raw
            payload = {pulse_height_raw, pulse_num, pulse_period, pulse_width};

            axis_send_byte(8'h88, 1'b0); // CMD_SET_DATA

            for (i = 0; i < 12; i++) begin
                axis_send_byte(payload[i*8 +: 8], (i == 11));
            end
        end
    endtask

    task automatic pulse_finish;
        begin
            @(posedge adc_clk_in);
            finish <= 1'b1;
            @(posedge adc_clk_in);
            finish <= 1'b0;
        end
    endtask

    
    // waiters
    task automatic wait_dac_rst_count(input int expected, input int max_cycles = 100);
        int i;
        begin
            for (i = 0; i < max_cycles; i++) begin
                @(posedge dac_clk_in);
                if (dac_rst_count >= expected) return;
            end
            $fatal(1, "Timeout waiting for dac_rst_count >= %0d, current=%0d", expected, dac_rst_count);
        end
    endtask

    task automatic wait_adc_rst_count(input int expected, input int max_cycles = 100);
        int i;
        begin
            for (i = 0; i < max_cycles; i++) begin
                @(posedge adc_clk_in);
                if (adc_rst_count >= expected) return;
            end
            $fatal(1, "Timeout waiting for adc_rst_count >= %0d, current=%0d", expected, adc_rst_count);
        end
    endtask

    task automatic wait_dac_start_count(input int expected, input int max_cycles = 100);
        int i;
        begin
            for (i = 0; i < max_cycles; i++) begin
                @(posedge dac_clk_in);
                if (dac_start_count >= expected) return;
            end
            $fatal(1, "Timeout waiting for dac_start_count >= %0d, current=%0d", expected, dac_start_count);
        end
    endtask

    task automatic wait_adc_start_count(input int expected, input int max_cycles = 100);
        int i;
        begin
            for (i = 0; i < max_cycles; i++) begin
                @(posedge adc_clk_in);
                if (adc_start_count >= expected) return;
            end
            $fatal(1, "Timeout waiting for adc_start_count >= %0d, current=%0d", expected, adc_start_count);
        end
    endtask

    task automatic wait_cfg_applied(
        input logic [31:0] exp_pulse_width,
        input logic [31:0] exp_pulse_period,
        input logic [15:0] exp_pulse_num,
        input logic [15:0] exp_pulse_height_raw,
        input int max_cycles = 200
    );
        logic [DAC_DATA_WIDTH-1:0] exp_dac_height;
        int i;
        begin
            exp_dac_height = exp_pulse_height_raw[DAC_DATA_WIDTH-1:0];

            for (i = 0; i < max_cycles; i++) begin
                @(posedge eth_clk_in);
                if ((dac_pulse_width  === exp_pulse_width ) &&
                    (dac_pulse_period === exp_pulse_period) &&
                    (dac_pulse_num    === exp_pulse_num   ) &&
                    (dac_pulse_height === exp_dac_height  ) &&
                    (adc_pulse_period === exp_pulse_period) &&
                    (adc_pulse_num    === exp_pulse_num   )) begin
                    return;
                end
            end

            $fatal(1,
                "Timeout waiting config outputs. Got: dac_width=%h dac_period=%h dac_num=%h dac_height=%h adc_period=%h adc_num=%h",
                dac_pulse_width, dac_pulse_period, dac_pulse_num, dac_pulse_height,
                adc_pulse_period, adc_pulse_num
            );
        end
    endtask

    
    // Test sequence
    logic [31:0] test_pulse_width;
    logic [31:0] test_pulse_period;
    logic [15:0] test_pulse_num;
    logic [15:0] test_pulse_height_raw;

    initial begin
        // defaults
        rst_n         = 1'b0;
        s_axis_tdata  = '0;
        s_axis_tvalid = 1'b0;
        s_axis_tlast  = 1'b0;
        finish        = 1'b0;

        test_pulse_width      = 32'h11223344;
        test_pulse_period     = 32'h55667788;
        test_pulse_num        = 16'hA1B2;
        test_pulse_height_raw = 16'h0CDE; // for DAC_DATA_WIDTH=12 => 12'hCDE

        repeat (10) @(posedge eth_clk_in);
        rst_n = 1'b1;

        repeat (10) @(posedge eth_clk_in);

        $display("[%0t] TEST 1: soft_reset", $time);
        send_cmd(8'h0F);

        wait_dac_rst_count(1);
        wait_adc_rst_count(1);

        if (dac_rst_count != 1) begin
            $fatal(1, "Expected exactly one dac_rst pulse after first soft_reset, got %0d", dac_rst_count);
        end
        if (adc_rst_count != 1) begin
            $fatal(1, "Expected exactly one adc_rst pulse after first soft_reset, got %0d", adc_rst_count);
        end

        $display("[%0t] TEST 1 passed", $time);

        $display("[%0t] TEST 2: set_data", $time);
        send_set_data(
            test_pulse_width,
            test_pulse_period,
            test_pulse_num,
            test_pulse_height_raw
        );

        wait_cfg_applied(
            test_pulse_width,
            test_pulse_period,
            test_pulse_num,
            test_pulse_height_raw
        );

        if (dac_pulse_width !== 32'h11223344) begin
            $fatal(1, "dac_pulse_width mismatch: got %h expected %h", dac_pulse_width, 32'h11223344);
        end
        if (dac_pulse_period !== 32'h55667788) begin
            $fatal(1, "dac_pulse_period mismatch: got %h expected %h", dac_pulse_period, 32'h55667788);
        end
        if (dac_pulse_num !== 16'hA1B2) begin
            $fatal(1, "dac_pulse_num mismatch: got %h expected %h", dac_pulse_num, 16'hA1B2);
        end
        if (dac_pulse_height !== 12'hCDE) begin
            $fatal(1, "dac_pulse_height mismatch: got %h expected %h", dac_pulse_height, 12'hCDE);
        end
        if (adc_pulse_period !== 32'h55667788) begin
            $fatal(1, "adc_pulse_period mismatch: got %h expected %h", adc_pulse_period, 32'h55667788);
        end
        if (adc_pulse_num !== 16'hA1B2) begin
            $fatal(1, "adc_pulse_num mismatch: got %h expected %h", adc_pulse_num, 16'hA1B2);
        end

        $display("[%0t] TEST 2 passed", $time);
        
        repeat (20) @(posedge eth_clk_in);

        $display("[%0t] TEST 3: start", $time);
        send_cmd(8'hF0);

        wait_dac_start_count(1);
        wait_adc_start_count(1);

        if (dac_start_count != 1) begin
            $fatal(1, "Expected exactly one dac_start pulse after first start, got %0d", dac_start_count);
        end
        if (adc_start_count != 1) begin
            $fatal(1, "Expected exactly one adc_start pulse after first start, got %0d", adc_start_count);
        end

        $display("[%0t] TEST 3 start pulses passed", $time);

        // release busy by finish pulse from ADC domain
        $display("[%0t] Sending finish pulse", $time);
        pulse_finish();

        // a bit of wait for finish CDC back to ETH
        repeat (20) @(posedge eth_clk_in);

        // sanity check that commands are accepted again after finish
        $display("[%0t] TEST 4: soft_reset after finish", $time);
        send_cmd(8'h0F);

        wait_dac_rst_count(2);
        wait_adc_rst_count(2);

        if (dac_rst_count != 2) begin
            $fatal(1, "Expected exactly two dac_rst pulses total, got %0d", dac_rst_count);
        end
        if (adc_rst_count != 2) begin
            $fatal(1, "Expected exactly two adc_rst pulses total, got %0d", adc_rst_count);
        end

        $display("[%0t] TEST 4 passed", $time);

        $display("==============================================");
        $display("ALL BASIC TESTS PASSED");
        $display("dac_rst_count   = %0d", dac_rst_count);
        $display("adc_rst_count   = %0d", adc_rst_count);
        $display("dac_start_count = %0d", dac_start_count);
        $display("adc_start_count = %0d", adc_start_count);
        $display("==============================================");

        #100;
        $finish;
    end

endmodule