library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

use work.wishbone_pkg.all;
use work.trigunit_regs_consts.all;
use work.vtudiag_regs_consts.all;
use work.sim_wishbone.all;
use work.sim_wishbone16.all;

entity tb_vtu is
end tb_vtu;

architecture arch of tb_vtu is
    signal clk_sys : std_logic := '0';
    signal rst_sys : std_logic;
    signal rst_rf_n : std_logic;

    signal clk_rf : std_logic := '1';
    signal clk_vtu : std_logic := '1';

    signal sync : std_logic := '0';
    signal trig_p : std_logic := '0';
    signal start, stop : std_logic := '0';

    signal wb_in : t_wishbone_slave_in;
    signal wb_out : t_wishbone_slave_out;

    function to_string(v : std_ulogic) return character is
    begin
      case v is
        when 'U' => return 'U';
        when '0' => return '0';
        when '1' => return '1';
        when 'X' => return 'X';
        when 'Z' => return 'Z';
        when 'L' => return 'L';
        when 'H' => return 'H';
        when 'W' => return 'W';
        when '-' => return '-';
      end case;
    end to_string;

    function to_string(v : std_logic_vector) return string is
      alias av : std_logic_vector(1 to v'length) is v;
      variable res : string(av'range);
    begin
      for i in av'range loop
        res (i) := to_string(av (i));
      end loop;
      return res;
    end to_string;

    --  Command to be sent to the VTU observer: number of expected pulses.
    type observer_type is record
      --  Number of expected pulses.
      count : natural;

      --  True if can go beyond the number of pulses (infinite window).
      infinite : boolean;

      --  If True, expect square waves.  Check half-period.
      square : boolean;
    end record;

    signal observer_cmd : observer_type;

    --  VTU observer state.
    type obs_state_type is (IDLE, WORKING, OBS_DONE, OBS_ERR);
    signal observer_state : obs_state_type := IDLE;
    signal observer_count : natural;
    signal observer_period : time;
    signal observer_delay : natural;  --  Delay between sync and first pulse,
    type obs_sync_state_type is (WAIT_SYNC, WAIT_PULSE, DONE);
    signal sync_state : obs_sync_state_type := DONE;
begin
    --  System clock, 62.5 Mhz
    process
    begin
      clk_sys <= '0';
      wait for 8 ns;
      clk_sys <= '1';
      wait for 8 ns;
    end process;
    rst_sys <= '1', '0' after 48 ns;
    rst_rf_n <= '0', '1' after 80 ns;

    --  RF clock, ~200Mhz (5ns)
    --  VTU clock, ~25Mhz (40ns)
    process
      variable cnt : natural := 1;
    begin
      clk_rf <= not clk_rf;
      if cnt = 8 then
        clk_vtu <= not clk_vtu;
        cnt := 0;
      end if;
      cnt := cnt + 1;
      wait for 2.5 ns;
    end process;

    my_inst: entity work.vtu_blk
      port map (
        clk_sys_i => clk_sys,
        rst_sys_i => rst_sys,
        rst_rf_n_i => rst_rf_n,
        wb_i => wb_in,
        wb_o => wb_out,
        clk_vtu_i => clk_vtu,
        clk_rf_io_i => clk_rf,
        clk_rf_i => clk_rf,
        
        sync_i => sync,
        sync_x8_fb_i => (others => '0'),
        sync_x8_fb_o => open,
        
        trig_o => trig_p,

        trig_rst_i => '0',
        trig_rst_n_o => open,
        trig_rst_p_o => open,
        
        start_i => start,
        stop_i => stop
      );

    --  VTU observer.
    process (clk_rf, observer_cmd'transaction)
      variable sync_ts : time;
    begin
      if observer_cmd'active then
        sync_state <= WAIT_SYNC;
      elsif rising_edge(clk_rf) then
        case sync_state is
          when WAIT_SYNC =>
            if sync = '1' then
              report "observer: sync";
              sync_ts := now;
              sync_state <= WAIT_PULSE;
              observer_delay <= 0;
            end if;
          when WAIT_PULSE =>
            observer_delay <= observer_delay + 1;
            if trig_p = '1' then
              sync_state <= DONE;
            end if;
          when DONE =>
            null;
        end case;
      end if;
    end process;

    process (trig_p, stop, observer_cmd'transaction)
      variable last_ts : time;
      variable this_period : time;
      variable stopped : boolean;
    begin
      if observer_cmd'active then
        --  New command
        case observer_state is
          when IDLE | OBS_DONE | OBS_ERR =>
            report "observer: new command";
            observer_state <= WORKING;
            observer_count <= 1;
            last_ts := now;
            stopped := false;
          when WORKING =>
            report "VTU observer command overrides the previous one" severity error;
        end case;
      end if;

      if rising_edge (stop) then
        stopped := True;
      end if;

      if trig_p'event and (observer_cmd.square or trig_p = '1') then
        report "Trigger:" & Natural'image(observer_count);
        case observer_state is
          when IDLE =>
            report "VTU observer: trigger before command" severity error;
          when WORKING | OBS_DONE =>
            this_period := now - last_ts;
            if observer_count = 1 then
              --  First pulse.
              assert sync_state = WAIT_PULSE report "VTU observer: trigger before sync";
            elsif observer_count = 2 then
              observer_period <= this_period;
            elsif observer_count > 2 then
               if not stopped and this_period /= observer_period then
                 report "VTU observer: irregular period" severity error;
               end if;
            end if;
            last_ts := now;
            observer_count <= observer_count + 1;
            if observer_count = observer_cmd.count then
              observer_state <= OBS_DONE;
            elsif observer_count > observer_cmd.count and not observer_cmd.infinite then
              report "VTU observer: too many pulses" severity error;
            end if;
          when OBS_ERR =>
            null;
        end case;
      end if;
    end process;

    --  Test process.
    process
        --  Send a pulse on the start signal and wait until it is propagated to the VTU.
        procedure start_pulse is
        begin
          --  Start pulse.
          start <= '1';
          wait for 40 ns;
          start <= '0';

          --  Wait until the VTU get the start pulse
          for i in 1 to 7 loop
            wait until rising_edge(clk_vtu);
          end loop;
        end start_pulse;

        procedure sync_pulse is
        begin
          sync <= '1';
          wait for 10 ns;
          sync <= '0';

          --  Wait until the VTU get the sync pulse.
          for i in 1 to 3 loop
            wait until rising_edge(clk_vtu);
          end loop;
        end sync_pulse;

        procedure read_status (s : inout std_logic_vector(15 downto 0)) is
        begin
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_STATUS, s);
          report "status = " & to_string(s)
            & ", missvalid=" & to_string (s (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET))
            & ", missready=" & to_string (s (TRIGUNIT_REGS_STATUS_MISSREADY_OFFSET))
            & ", startready=" & to_string (s (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET))
            & ", running=" & to_string (s (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET));
          -- assert s (TRIGUNIT_REGS_STATUS_WRONGBVALUE_OFFSET) = '0' severity error;
          -- assert s (TRIGUNIT_REGS_STATUS_WRONGHTVALUE_OFFSET) = '0' severity error;
          -- assert s (TRIGUNIT_REGS_STATUS_WRONGWVALUE_OFFSET) = '0' severity error;
        end read_status;

        procedure read_diag
        is
          variable freq : natural;
          variable val, val2, gen : std_logic_vector(15 downto 0);
        begin
          report "show vtu";
          wait until rising_edge (clk_sys);
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_GENERATION, gen);
          report "Vtu: generation: " & to_string(gen);
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_FREQ, val);
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_FREQ + 2, val2);
          freq := to_integer(unsigned(val) & unsigned(val2));
          report "Vtu: freq: " & natural'image(freq);
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_COUNTER, val);
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_COUNTER + 2, val2);
          report "Vtu: counter: " & natural'image(to_integer(unsigned(val) & unsigned(val2)));
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_GENERATION, val);
          report "Vtu: generation: " & to_string(val);
          assert gen = val report "mismatching generation, values are not valid" severity error;
        end read_diag;

        procedure show_diag is
        begin
          report "show vtu";
          read_diag;
        end show_diag;

        procedure test_window (bval : natural; hval : natural; wval : natural) 
        is
          variable val : std_logic_vector(15 downto 0);
        begin
          --  Program the vtu
          --  Delay between start and the first pulse.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_BVALUEOFFLINE,
                        std_logic_vector(to_unsigned(bval, 64)));
          --  Delay between the pulses.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_HTVALUEOFFLINE,
                        std_logic_vector(to_unsigned(hval, 64)));
          --  Number of pulses.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_WVALUEOFFLINE,
                        std_logic_vector(to_unsigned(wval, 64)));

          --  Windowed mode, enable.
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONFIGOFFLINE, x"0021");

          --  Remove reset.
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONTROL, x"0000");

          --  Check status.
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_MISSREADY_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '0' severity error;
          --  Set to one after reset.
          assert val (TRIGUNIT_REGS_STATUS_WRONGBVALUE_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_WRONGHTVALUE_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_WRONGWVALUE_OFFSET) = '1' severity error;

          --  Check online config is 0.
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONFIGONLINE, val);
          report "config online = " & to_string(val);
          assert val = x"0000" severity error;

          --  Expect 64 syncs.
          observer_cmd <= (count => wval, infinite => False, square => False);

          --  Start pulse.
          start_pulse;

          --  Check status.
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_MISSREADY_OFFSET) = '0' severity error;
          --  Should be 0 if close enough to the start pulse.
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '0' severity note;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_WRONGBVALUE_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_WRONGHTVALUE_OFFSET) = '0' severity error;
          assert val (TRIGUNIT_REGS_STATUS_WRONGWVALUE_OFFSET) = '0' severity error;

          --  Sync pulse.
          sync_pulse;

          --  Check ...
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '1' severity error;

          --  Check online config.
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONFIGONLINE, val);
          report "config online = " & to_string(val);
          assert val = x"0020" severity error;

          --  Check online registers
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_WVALUEONLINE + 6, val);
          assert val = std_logic_vector(to_unsigned(wval, 16)) severity error;
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_HTVALUEONLINE + 6, val);
          assert val = std_logic_vector(to_unsigned(hval, 16)) severity error;
          read16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_BVALUEONLINE + 6, val);
          assert val = std_logic_vector(to_unsigned(bval, 16)) severity error;

          --  Wait until end of generation.
          wait on observer_state;
          --  Check observer status.
          assert observer_state = OBS_DONE severity error;
          assert observer_delay = (8 + bval + 8 + 8)
            report "window test: bad delay:" & natural'image(observer_delay)
            severity error;
          assert observer_period = hval * 5 ns;
          --  Check VTU status.
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '0' severity error;
        end test_window;

        procedure test_lowfreq (bval : natural; htval : natural)
         is
          variable val : std_logic_vector(15 downto 0);
          variable cnt : natural;
          variable dly_exp : natural;
        begin
          report "test lowfreq - setup vtu";
          --  Program the vtu
          wait until rising_edge(clk_sys);
          --  Delay between start and the first pulse.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_BVALUEOFFLINE,
                        std_logic_vector(to_unsigned(bval, 64)));
          --  Delay between the pulses.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_HTVALUEOFFLINE,
                        std_logic_vector(to_unsigned(htval, 64)));
          --  Number of pulses (unused).
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_WVALUEOFFLINE, x"0000_0000_0000_0014");

          --  Lowfreq generation, enable.
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONFIGOFFLINE, x"0041");

          --  Set expected results.
          observer_cmd <= (count => 10, infinite => true, square => true);

          report "test lowfreq - start pulse";
          start_pulse;

          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET) = '0' severity error;

          --  Sync pulse.
          sync_pulse;

          --  Check ...
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '1' severity error;

          --  Wait until end of generation.
          wait on observer_state;
          --  Check observer status.
          assert observer_state = OBS_DONE severity error;
          assert observer_period = htval * 5 ns;
          report "lowfreq delay:" & natural'image(observer_delay);
          dly_exp := (8 + bval + 8 + 8 + 1);
          assert observer_delay = dly_exp
            report "lowfreq test: bad delay:" & natural'image(observer_delay) & ", expect:" & natural'image (dly_exp)
            severity error;

          --  Stop counter.  Synchronize to
          wait until rising_edge(clk_vtu);
          stop <= '1';
          wait for 40 ns;
          stop <= '0';

          --  Wait a little bit (2 full periods)
          wait for 2 * 2 * observer_period;

          cnt := observer_count;

          --  Wait  for 2 full periods.
          wait for 2 * 2 * observer_period;

          --  We should have stopped after 20 periods.  There are some extra periods because of synchronizers.
          assert observer_count = cnt severity error;
          report "test lowfreq: done";
        end test_lowfreq;

        procedure test_highfreq (bval : natural; htval : natural)
         is
          variable val : std_logic_vector(15 downto 0);
          variable cnt : natural;
          variable dly_exp : natural;
          variable freq : natural;
        begin
          report "test highfreq - setup vtu ht:" & natural'image(htval);

          wait until rising_edge(clk_sys);

          --  Reset the diag unit
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_CONTROL, x"0000");

          --  Program the vtu
          --  Delay between start and the first pulse.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_BVALUEOFFLINE,
                        std_logic_vector(to_unsigned(bval, 64)));
          --  Delay between the pulses.
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_HTVALUEOFFLINE,
                        std_logic_vector(to_unsigned(htval, 64)));
          --  Number of pulses (unused).
          write64be_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_WVALUEOFFLINE, x"0000_0000_0000_0014");

          --  Lowfreq generation, enable.
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_CONFIGOFFLINE, x"0061");

          --  Set expected results.
          observer_cmd <= (count => 24, infinite => true, square => False);

          report "test highfreq - start pulse";
          start_pulse;

          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET) = '0' severity error;

          --  Sync pulse.
          sync_pulse;

          --  Check ...
          read_status(val);
          assert val (TRIGUNIT_REGS_STATUS_STARTREADY_OFFSET) = '1' severity error;
          assert val (TRIGUNIT_REGS_STATUS_RUNNING_OFFSET) = '1' severity error;

          --  Wait until the first pulse.
          wait until rising_edge(trig_p);
          wait until rising_edge(clk_sys);
          wait until rising_edge(clk_sys);

          --  Enable the diag unit (5us)
          write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_CONTROL, x"0010");

          --  Wait for a full diag unit cycle.
          wait for 5 us;
          --  Wait a little bit more to be after the generation.
          wait for 200 ns;

          --  Check observer status.
          assert observer_state = OBS_DONE severity error;
          assert observer_period = htval * 5 ns;
          report "highfreq delay:" & natural'image(observer_delay);
          dly_exp := (8 + bval + 8 + 8 + 16 + 1);
          assert observer_delay = dly_exp
            report "highfreq test: bad delay:" & natural'image(observer_delay) & ", expect:" & natural'image (dly_exp)
            severity error;

          --  Stop counter.  Synchronize to
          wait until rising_edge(clk_vtu);
          stop <= '1';
          wait for 40 ns;
          stop <= '0';

          read_diag;
          report "expected freq: " & natural'image(200_000_000 / 200_000 / htval);

          --  Wait a little bit (2 full periods)
          wait for 2 * 2 * observer_period;

          cnt := observer_count;

          --  Wait  for 2 full periods.
          wait for 2 * 2 * observer_period;

          report "test highfreq: done";
        end test_highfreq;

        variable val : std_logic_vector(15 downto 0);
    begin
        sync <= '0';
        start <= '0';
        wb_in.stb <= '0';
        wb_in.cyc <= '0';

        report "wait until reset";
        
        wait until rst_sys = '0' and rst_rf_n = '1';

        report "starting...";
        
        --  Program the diag (5us) + enable.
        write16_pl (clk_sys, wb_in, wb_out, ADDR_TRIGUNIT_REGS_TRIGDIAG + ADDR_VTUDIAG_REGS_CONTROL, x"0010");

        test_window (25, 13, 7);

        show_diag;

        -------------------------------------------------------------------------------------------------

        --  Test 2: check that missing valid parameters is reported.
        --  (A cycle has been finished, the start signal has to be ignored because
        --   parameters aren't set.)
        report "test 2 - start";
        start_pulse;

        read_status(val);
        assert val (TRIGUNIT_REGS_STATUS_MISSVALID_OFFSET) = '1' severity error;

        -------------------------------------------------------------------------------------------------

        --  Test 3: Low-freq
        test_lowfreq (29, 17);

        test_lowfreq (12, 20);

        --  Test 4: high-freq
        for i in 2 to 15 loop
          test_highfreq(16, i);
        end loop;

        report "end of tests";
        wait;
    end process;
end arch;
