`timescale 1ns / 1ps

`include "interfaces.svh"
        // start push

`define DEBUG

`define MEASURE_CLK(clk, period) \
begin \
    realtime t1, t2; \
    @(posedge clk); \
    t1 = $realtime; \
    @(posedge clk); \
    t2 = $realtime; \
    period = t2 - t1; \
end

module reflectometer_tb;
    //------------------------------------------------------------
    // Параметры
    //------------------------------------------------------------
    localparam int unsigned DAC_DATA_WIDTH = 14;
    localparam int unsigned ADC_DATA_WIDTH = 12;
    localparam LOGIC_ZERO_LEVEL     = 0; // DAC -5V for logic zero
    localparam VOLTAGE_ZERO_LEVEL   = 2**(DAC_DATA_WIDTH-1); // DAC 0V for logic zero
    localparam PACK_FACTOR = 1;     // not used in TB
    localparam PROCESS_MODE = 0;    // 0 - uint, 1 - int. Current accumulator don't support signed sum
    localparam ACCUM_WIDTH = 32;    // accumulator number bit witdth
    localparam N_MAX = 4096;        // max value of windows to average by experiments
    localparam PACKET_SIZE = 1024;  // bytes per UDP packet
    
    localparam int REQUEST_TIMEOUT = 3 * PACKET_SIZE; // timeout for packet receiving from accumulator

    localparam ZERO_LEVEL           = LOGIC_ZERO_LEVEL; // "logic" VS "voltage"

    localparam CLK_ETH_PHY_PERIOD           = 8.000; // 125 MHz
    localparam CLK_REF_PERIOD               = 5.000; // 200 MHz
    
    //------------------------------------------------------------
    // Глобальные перменные
    //------------------------------------------------------------
    realtime CLK_ADC_PERIOD;
    realtime CLK_DAC_PERIOD;

    //------------------------------------------------------------
    // Тактовые сигналы и сброс
    //------------------------------------------------------------
    logic clk_ref       = 1'b0; // 200 MHz
    logic clk_eth_phy   = 1'b0; // common for RX & TX
    logic rst_n         = 1'b0;
    
    //------------------------------------------------------------
    // Управление и конфиг DUT
    //------------------------------------------------------------
    logic [31:0] window_size_port;
    // AXI-S интерфейс для управления
    axis_if axis_control_if (
        .clk(clk_eth_phy),
        .rst_n(rst_n)
    );
    
    //------------------------------------------------------------
    // Входы DUT
    //------------------------------------------------------------
    // ADC интерфейс
    wire                       clk_adc;
    wire                       adc_otr;
    wire [ADC_DATA_WIDTH-1:0]  adc_data;
    
    //------------------------------------------------------------
    // Выходы
    //------------------------------------------------------------
    // Статусы
    wire mmcm_locked;
    wire workflow_done;
    wire processing_done;
    // DAC интерфейс
    wire                       clk_dac;
    wire                       dac_wrt;
    wire [DAC_DATA_WIDTH-1:0]  dac_data;
    // AXI-S интерфейс для данных
    axis_if axis_accumulator_if (
        .clk(clk_eth_phy),
        .rst_n(rst_n)
    );
    //------------------------------------------------------------
    // Внутренние сигналы тестбенча
    //------------------------------------------------------------
    // Интерфейс хендшейка с MAC-PHY
    wire  send_request;
    logic request_ready;
    // Сигнал между ЦАП и АЦП
    real signal_voltage;
    
    //------------------------------------------------------------
    // Virtual DAC
    //------------------------------------------------------------
    virtual_dac_model #( // default voltage range is +/- 5V
        .DAC_DATA_WIDTH(DAC_DATA_WIDTH)
        // ,.VOLTAGE_GAIN(2)
    ) virtual_dac (
        .clk_i(clk_dac),
        .wrt_i(dac_wrt),
        .data_i(dac_data),
        .voltage_o(signal_voltage)
    );

    //------------------------------------------------------------
    // Virtual ADC
    //------------------------------------------------------------
    virtual_adc_model #( // default voltage range is +/- 5V
        .ADC_DATA_WIDTH(ADC_DATA_WIDTH)
    ) virtual_adc (
        .clk_i(clk_adc),
        .voltage_i(signal_voltage),
        .otr_o(adc_otr),
        .data_o(adc_data)
    );

    //------------------------------------------------------------
    // Statistics processing
    //------------------------------------------------------------

    //------------------------------------------------------------
    // Config handler
    //------------------------------------------------------------

    //------------------------------------------------------------
    // DUT
    //------------------------------------------------------------
    reflectometer_top #(
        .DAC_DATA_WIDTH(DAC_DATA_WIDTH),
        .ADC_DATA_WIDTH(ADC_DATA_WIDTH),
        .PACK_FACTOR(PACK_FACTOR),
        .PROCESS_MODE(PROCESS_MODE),
        .ZERO_LEVEL(ZERO_LEVEL),
        .ACCUM_WIDTH(ACCUM_WIDTH),
        .N_MAX(N_MAX),
        .PACKET_SIZE(PACKET_SIZE)
    ) DUT (
        .clk_in(clk_ref),
        .rst_n(rst_n),

        // Status
        .locked(mmcm_locked),
        .workflow_done(workflow_done),
        .processing_done(processing_done),

        // Accumulator AXI-S bus
        .clk_axis_accumulator(clk_eth_phy), //  GMII PHY RX clock    
        .axis_accumulator(axis_accumulator_if.master),

        // Control AXI-S bus
        .clk_axis_control(clk_eth_phy), //  GMII PHY TX clock  
        .axis_control(axis_control_if.slave),
        .window_size(window_size_port),  // direct signal crutch (old controller)
        
        // RTL-MAC handshake
        .request_ready(request_ready),
        .send_request(send_request),

        // DAC
        .dac_clk_o(clk_dac),
        .dac_data(dac_data),
        .dac_wrt(dac_wrt),

        // ADC
        .adc_clk_o(clk_adc),
        .adc_data(adc_data),
        .adc_otr(adc_otr)
    );

    //------------------------------------------------------------
    // Тактовые сигналы
    //------------------------------------------------------------
    initial begin
        forever #(CLK_REF_PERIOD/2) clk_ref = ~clk_ref;
    end
    initial begin
        forever #(CLK_ETH_PHY_PERIOD/2) clk_eth_phy = ~clk_eth_phy;
    end

    //------------------------------------------------------------
    // Таски для тестирования
    //------------------------------------------------------------
    // Таски работы с AXI-Stream
    task automatic dut_soft_reset(virtual axis_if#(8).tb vif);
        logic [7:0] tx_packet[];
        tx_packet = '{8'h0f};
        vif.master_send(tx_packet);
    endtask

    task automatic dut_start(virtual axis_if#(8).tb vif);
        logic [7:0] tx_packet[];
        tx_packet = '{8'hf0};
        vif.master_send(tx_packet);
    endtask
    
    task automatic dut_send_system_config(
        virtual axis_if#(8).tb vif,
        input logic [31:0] pulse_width,
        input logic [31:0] pulse_period,
        input logic [15:0] pulse_num,
        input logic [13:0] pulse_height, // achtung! p_height strictly must have 14 bits of width
        input logic [31:0] pulse_period_adc,
        input logic [31:0] window_size
    );
        // Создаем временный фиксированный массив и упаковываем всё одной строкой
        logic [7:0] tx_packet[];

        // Ахтунг, 14-битный ЦАП захардкожен
        if (DAC_DATA_WIDTH != 14)
            $warning("[WARNING] -dut_send_system_config- Default pulse height (DAC bitwidth) is equal to 14. Be aware, controller packet structure is coded for 14 bits");

        tx_packet = '{
            8'h88, // Команда
            pulse_width[7:0],  pulse_width[15:8],  pulse_width[23:16],  pulse_width[31:24],
            pulse_period[7:0], pulse_period[15:8], pulse_period[23:16], pulse_period[31:24],
            pulse_num[7:0],    pulse_num[15:8], pulse_height[7:0], 8'({2'b00, pulse_height[13:8]}),
            pulse_period_adc[7:0], pulse_period_adc[15:8], pulse_period_adc[23:16], pulse_period_adc[31:24]
        };
        
        vif.master_send(tx_packet);

        // TODO remove for new controller
        window_size_port = window_size;
    endtask

    // Таски сбора статистики
    task automatic dut_read_output(
        virtual axis_if#(8).tb vif,
        input   int     sample_num,
        input   int     window_size,
        input   bit     randomize_recv_delays,
        output  int     output_data[]
    );
        logic [7:0]             rx_packet[];
        logic [ACCUM_WIDTH-1:0] data_packet[];
        int                     numbers_per_packet = PACKET_SIZE/(ACCUM_WIDTH/8);
        int                     packet_num = $ceil(real'(sample_num / window_size) / real'(numbers_per_packet));
        int timeout_flag = 0;
        int packet_counter = 0;

        if (sample_num % window_size) begin
            $error("-dut_read_output- Sample_num must be multiple of window_size: %0d %% %0d = %0d", sample_num, window_size, sample_num % window_size);
            $finish;
        end

        data_packet = new[numbers_per_packet];
        output_data = new[numbers_per_packet * packet_num];
        
        // count send_request pulses (equal to number of packets)
        fork
            begin : packet_counter_proc
                forever begin
                    @(posedge clk_eth_phy);
                    if(send_request === 1)
                        packet_counter++;
                end
            end
        join_none

        // Wait until reflectometer done sampling and averaging
        wait(processing_done == 1);

        // recv loop
        // если число пакетов превышает заложенное предрассчитанное значение -- ошибка
        fork : recv_loop_proc
            begin
                // packet recv loop
                forever begin
                    if (packet_counter > packet_num) begin
                        $error("-dut_read_output- Packet overflow detected. Number of data packets exceeds expected amount of packets");
                        $finish;
                    end

                    if (randomize_recv_delays)
                        repeat($urandom_range(0, 500)) @(posedge clk_eth_phy);
                    
                    timeout_flag = 0;
                    fork : receive_packet_timeout
                        begin
                            request_ready = 1;
                            vif.slave_recv(rx_packet);
                            request_ready = 0;
                        end
                        begin
                            repeat(REQUEST_TIMEOUT) @(posedge clk_eth_phy);
                            timeout_flag = 1;
                        end
                    join_any

                    disable receive_packet_timeout;
                    if (timeout_flag) begin
                        $error("-dut_read_output- Timeout detected when receiving packet");
                        $finish;
                    end

                    if (rx_packet.size() != PACKET_SIZE) begin
                        $error("-dut_read_output- Wrong packet size received: %0d bytes received, %0d bytes expected", rx_packet.size(), PACKET_SIZE);
                        $finish;
                    end
                    
                    // unpack values
                    data_packet = {<< byte {rx_packet}};
                    data_packet = {<< ACCUM_WIDTH {data_packet}};

                    // copy and convert values
                    for (int j = 0; j < data_packet.size(); j++) begin
                        output_data[(packet_counter-1) * data_packet.size() + j] = int'(data_packet[j]);
                    end

                end
            end
            begin
                // IP workflow completion event
                wait(workflow_done == 1);
            end
        join_any

        disable recv_loop_proc;
        disable packet_counter_proc;

        if (packet_counter != packet_num) begin
            $error("-dut_read_output- Wrong number of packets received: %0d received, %0d expected", packet_counter, packet_num);
            $finish;
        end

    endtask

    //------------------------------------------------------------
    // Функции и таски для верификации сигналов
    //------------------------------------------------------------
    // Таска генерации идеального тестового сигнала
    task automatic reference_signal(
        input int pulse_width,
        input int pulse_height,
        input int pulse_period_adc,
        input int window_size,
        output real result[]
    );
        /*
            Globals:
            ADC and DAC clock periods,
            Virtual ADC and DAC voltage steps

            Task developed with assumption that first discrete values of DAC and ADC 
            are syncrhonized at t==0 and started simultaneously.

            Gains and biases of virtual ADC & DAC are default and ranges are [-5V;5V].
            Bitwidths may be altered.

            Returned result[] array is an array of sums of voltage potentials in discrete time points.
            Discrete samples summed over a time window.
            
            result[time] = (voltage)
        */

        int sample_num = pulse_period_adc / window_size; // total averaged output samples from accumulator
        
        real current_signal_sample, partial_sum;
        real ref_signal_active_voltage  = virtual_dac.code_to_voltage(pulse_height);
        real ref_signal_zero_voltage    = virtual_dac.code_to_voltage(ZERO_LEVEL);

        if (pulse_period_adc % window_size) begin
            $error("-reference_signal- pulse_period_adc must be multiple of window_size: %0d %% %0d = %0d", pulse_period_adc, window_size, pulse_period_adc % window_size);
            $finish;
        end

        result = new[sample_num];
        partial_sum = 0;

        for (int i = 0; i < pulse_period_adc; i++) begin
            // var i in ADC timespace
            // i == 0 is a t0 of pulse generation and sampling
            current_signal_sample = (i*CLK_ADC_PERIOD <= pulse_width*CLK_DAC_PERIOD) ? ref_signal_active_voltage : ref_signal_zero_voltage;
            partial_sum += current_signal_sample;
            if (i % window_size == (window_size-1)) begin
                result[i / window_size] = partial_sum;
                partial_sum = 0;
            end
        end
    endtask

    // Функция проверки размеров выборок
    function automatic void check_size(
        input real a[],
        input real b[]
    );
        if (a.size() != b.size())
            $fatal(1, "Array size mismatch: %0d != %0d",
                a.size(), b.size());
        
        if (a.size() == 0)
            $error(1, "Empty array");
    endfunction

    // Среднее по выборке
    function automatic real array_mean(
        input real a[]
    );
        real sum = 0.0;

        foreach (a[i])
            sum += a[i];
        
        return sum / a.size();
    endfunction

    // MSE двух выборок
    function automatic real calc_mse(
        input real a[],
        input real b[]
    );
        real sum = 0.0;

        check_size(a, b);

        foreach (a[i]) begin
            real err;
            err = a[i] - b[i];
            sum += err * err;
        end

        return sum / a.size();
    endfunction

    // NRMSE двух выборок (нормирование MSE)
    function automatic real calc_nrmse(
        input real a[],
        input real b[]
    );
        real mse;
        real min_val;
        real max_val;

        mse = calc_mse(a, b);

        min_val = a[0];
        max_val = a[0];

        foreach (a[i]) begin
            if (a[i] < min_val)
                min_val = a[i];

            if (a[i] > max_val)
                max_val = a[i];
        end

        if (max_val == min_val)
            return 0.0;

        return $sqrt(mse) / (max_val - min_val);
    endfunction

    // Функция модуля
    function automatic real abs_f(input real x);
        return (x < 0.0) ? -x : x;
    endfunction

    // Максимальная абсолютная ошибка
    function automatic real calc_max_error(
        input real a[],
        input real b[]
    );
        real max_err = 0.0;

        check_size(a, b);

        foreach (a[i]) begin
            real err;

            err = abs_f(a[i] - b[i]);

            if (err > max_err)
                max_err = err;
        end

        return max_err;
    endfunction

    // Коэффициент корреляции Пирсона
    function automatic real calc_pearson(
        input real a[],
        input real b[]
    );
        real mean_a;
        real mean_b;
        real numerator = 0.0;
        real denom_a   = 0.0;
        real denom_b   = 0.0;

        check_size(a, b);

        mean_a = array_mean(a);
        mean_b = array_mean(b);

        foreach (a[i]) begin
            real da;
            real db;

            da = a[i] - mean_a;
            db = b[i] - mean_b;

            numerator += da * db;
            denom_a += da * da;
            denom_b += db * db;
        end

        if ((denom_a == 0.0) || (denom_b == 0.0))
            return 0.0;

        return numerator / $sqrt(denom_a * denom_b);
    endfunction

    // Вспомогательная функция для вывода массива
    function automatic void display_array_f(input real a[]);
        $write("\t");
        foreach(a[i])
            $write("%f ", a[i]);
        $write("\n");
    endfunction

    // Основная таска типового теста
    task automatic run_test_case(
        virtual axis_if#(8).tb ctrl_vif,
        virtual axis_if#(8).tb accum_vif,
        input int pulse_width,
        input int pulse_period,
        input int pulse_num,
        input int pulse_height,
        input int pulse_period_adc,
        input int window_size,
        input bit rand_recv_delays,
        input bit use_reset,
        output bit result
    );
        int output_data[];              // raw accum values
        real output_signal_v[];         // accum values after voltage conversion
        real reference_signal_v[];      // reference signal voltage values
        real nrmse, pearson, max_err;   // error and correlation metrics
    
        if (use_reset) begin
            dut_soft_reset(ctrl_vif);
            #100;
        end
        
        dut_send_system_config(
            .vif(ctrl_vif),
            .pulse_width(pulse_width),
            .pulse_period(pulse_period),
            .pulse_num(pulse_num),
            .pulse_height(pulse_height),
            .pulse_period_adc(pulse_period_adc),
            .window_size(window_size)
        );
        #100;

        dut_start(ctrl_vif);

        dut_read_output(
            .vif(accum_vif),
            .sample_num(pulse_period_adc),
            .window_size(window_size),
            .randomize_recv_delays(rand_recv_delays),
            .output_data(output_data)
        );
        // actual size of payload is pulse_period_adc / window_size
        output_signal_v = new[pulse_period_adc / window_size];

        // voltage conversion
        begin
            // zero level for partial sum
            real zero_level_bias = window_size * virtual_adc.ZERO_CODE;
            // common voltage multiplier for step & amplifier
            real voltage_multiplier = virtual_adc.VOLTAGE_STEP / virtual_adc.VOLTAGE_GAIN;
            // array conversion
            foreach (output_signal_v[i]) begin
                real average_code_per_pulse = real'(output_data[i]) / pulse_num;
                output_signal_v[i] = (average_code_per_pulse - zero_level_bias) * voltage_multiplier;
            end
        end

        reference_signal(
            .pulse_width(pulse_width),
            .pulse_height(pulse_height),
            .pulse_period_adc(pulse_period_adc),
            .window_size(window_size),
            .result(reference_signal_v)
        );

        `ifdef DEBUG
            $display("[TB] Output signal");
            display_array_f(output_signal_v);
            $display("[TB] Reference signal");
            display_array_f(reference_signal_v);
        `endif

        nrmse = calc_nrmse(output_signal_v, reference_signal_v);
        pearson = calc_pearson(output_signal_v, reference_signal_v);
        max_err = calc_max_error(output_signal_v, reference_signal_v);
        
        `ifdef DEBUG
            $display("[TB] Metrics:\n\tNRMSE = %0.4f\t|\tPearson = %0.4f\t|\tMax error = %0.4f", nrmse, pearson, max_err);
        `endif

        // check metrics
        result = 1;
        if (pearson < 0.99)
            result = 0;
        if (nrmse > 0.1)
            result = 0;
        /* 
            Max error not used in evaluation because of fast pulse edge falling 
            resulting in plain difference between active signal level and zero level
            For ex.: zero_level = 0x00 = -5V. pulse_height = 2^14-1 = 0x3fff = 5V
            In some cases like jitter this may cause max error = 5 - (-5) = 10(V)
            This cases are hardly traceble, thus max error not used in eval.
        */
    endtask

    //------------------------------------------------------------
    // ОСНОВНОЙ ПРОЦЕСС ТЕСТИРОВАНИЯ
    //------------------------------------------------------------
    initial begin
        bit result_flag;
        int total_failed_tests = 0, total_tests = 0;
        
        automatic virtual axis_if.tb control_vif = axis_control_if.tb;
        automatic virtual axis_if.tb accumulator_vif = axis_accumulator_if.tb;
        
        $info("[TB] DUT initializaton");
        // Инициализация
        request_ready = 0;
        rst_n = 0;
        #100;
        rst_n = 1;
        wait(mmcm_locked === 1'b1);
        #150;
        $info("[TB] MMCM locked");

        // Meause periods because actual values hardcoded in IP
        fork
            `MEASURE_CLK(DUT.clk_sampler, CLK_ADC_PERIOD);
            `MEASURE_CLK(DUT.clk_generator, CLK_DAC_PERIOD);
        join
        $info("[TB] ADC & DAC clock periods measured: ADC_period = %0.3f, DAC_period = %0.3f", CLK_ADC_PERIOD, CLK_DAC_PERIOD);

        dut_soft_reset(control_vif);
        #100;

        // Тесты
        $info("[TB] Tests start");
        
        $info("[TB] Simple test run");
        
        run_test_case(
            .ctrl_vif(control_vif),
            .accum_vif(accumulator_vif),
            .pulse_width(400),
            .pulse_period(1000),
            .pulse_num(5),
            .pulse_height(12000),
            .pulse_period_adc(600),
            .window_size(10),
            .rand_recv_delays(0),
            .use_reset(0),
            .result(result_flag)
        );

        $info("[TB] Random test run");
        
        $info("[TB] Corner case test run");
        

        // if (!failed)
            $info("[TB] ALL PASSED");
        $finish;
    end
endmodule