-- TestBench Template 

LIBRARY ieee;
USE ieee.std_logic_1164.ALL;
USE ieee.numeric_std.ALL;
USE std.textio.ALL;


ENTITY multiboot_tb IS
   END multiboot_tb;

ARCHITECTURE behavior OF multiboot_tb IS 

  -- Component Declaration
   component multiboot_top is
      port(
             wb_rst_i   : in   STD_LOGIC;
             wb_clk     : in   STD_LOGIC;
             wb_we_i    : in   STD_LOGIC;
             wb_stb_i   : in   STD_LOGIC;
             wb_cyc_i   : in   STD_LOGIC;
             wb_sel_i   : in   STD_LOGIC;
             wb_data_i  : in   STD_LOGIC_VECTOR (31 downto 0);
             wb_data_o  : out  STD_LOGIC_VECTOR (31 downto 0);
             wb_addr_i  : in   STD_LOGIC_VECTOR (3  downto 0);
             wb_ack_o   : out  STD_LOGIC;
             wb_rty_o   : out  STD_LOGIC;
             wb_err_o   : out  STD_LOGIC);
   end component;

   signal  wb_rst_i  : STD_LOGIC; 
   signal  wb_clk    : STD_LOGIC; 
   signal  wb_we_i   : STD_LOGIC; 
   signal  wb_stb_i  : STD_LOGIC; 
   signal  wb_cyc_i  : STD_LOGIC; 
   signal  wb_sel_i  : STD_LOGIC; 
   signal  wb_data_i : STD_LOGIC_VECTOR(31 downto 0); 
   signal  wb_data_o : STD_LOGIC_VECTOR(31 downto 0); 
   signal  wb_addr_i : STD_LOGIC_VECTOR(3  downto 0); 
   signal  wb_ack_o  : STD_LOGIC; 
   signal  wb_rty_o  : STD_LOGIC; 
   signal  wb_err_o  : STD_LOGIC; 

   constant wb_clk_period  : TIME := 50 ns; 

   constant OP_FULLMULTIBOOT : STD_LOGIC_VECTOR(3 downto 0) := X"0";
   constant OP_GENERAL1      : STD_LOGIC_VECTOR(3 downto 0) := X"1";
   constant OP_GENERAL2      : STD_LOGIC_VECTOR(3 downto 0) := X"2";
   constant OP_GENERAL3      : STD_LOGIC_VECTOR(3 downto 0) := X"3";
   constant OP_GENERAL4      : STD_LOGIC_VECTOR(3 downto 0) := X"4";
   constant OP_IPROG         : STD_LOGIC_VECTOR(3 downto 0) := X"7";
   constant OP_STAT          : STD_LOGIC_VECTOR(3 downto 0) := X"D";

   constant CTRL0_addr       : STD_LOGIC_VECTOR(3 downto 0) := X"0";
   constant CTRL1_addr       : STD_LOGIC_VECTOR(3 downto 0) := X"1";
   constant STAT_addr        : STD_LOGIC_VECTOR(3 downto 0) := X"2";
   constant GENERAL1_addr    : STD_LOGIC_VECTOR(3 downto 0) := X"4";
   constant GENERAL2_addr    : STD_LOGIC_VECTOR(3 downto 0) := X"5";
   constant GENERAL3_addr    : STD_LOGIC_VECTOR(3 downto 0) := X"6";
   constant GENERAL4_addr    : STD_LOGIC_VECTOR(3 downto 0) := X"7";

   signal CTRL_data          : STD_LOGIC_VECTOR(31 downto 0); 
   signal GENERAL1_data      : STD_LOGIC_VECTOR(31 downto 0);
   signal GENERAL2_data      : STD_LOGIC_VECTOR(31 downto 0);
   signal GENERAL3_data      : STD_LOGIC_VECTOR(31 downto 0);
   signal GENERAL4_data      : STD_LOGIC_VECTOR(31 downto 0);

   file file_handler    : TEXT;
   constant log_path    : STRING := "../test/log/multiboot_top_tb.log";
   constant sep         : CHARACTER := ht;

begin

   uut: multiboot_top
   port map(
              wb_rst_i  =>  wb_rst_i,  
              wb_clk    =>  wb_clk,    
              wb_we_i   =>  wb_we_i,   
              wb_stb_i  =>  wb_stb_i,  
              wb_cyc_i  =>  wb_cyc_i, 
              wb_sel_i  =>  wb_sel_i,  
              wb_data_i =>  wb_data_i, 
              wb_data_o =>  wb_data_o, 
              wb_addr_i =>  wb_addr_i, 
              wb_ack_o  =>  wb_ack_o,  
              wb_rty_o  =>  wb_rty_o,  
              wb_err_o  =>  wb_err_o 
           );

   wb_clk_process: process
   begin
      wb_clk <= '1';
      wait for wb_clk_period/2;
      wb_clk <= '0';
      wait for wb_clk_period/2;
   end process;


   tb : process

      procedure rst is
      begin
         wait for wb_clk_period;
         wb_rst_i  <= '1';
         wait for wb_clk_period;
         wb_rst_i  <= '0';
         wait for wb_clk_period*25;
      end rst;

      procedure read_wishbone(addr: STD_LOGIC_VECTOR(3 downto 0)) is
      begin
         wait until rising_edge(wb_clk);
         wb_we_i     <= '0';
         wb_stb_i    <= '1';
         wb_cyc_i    <= '1';
         wb_addr_i   <= addr;
         wait for wb_clk_period*3/2;
         wait until rising_edge(wb_clk);
         wb_we_i     <= '0';
         wb_stb_i    <= '0';
         wb_cyc_i    <= '0';
      end procedure;

      procedure write_wishbone(addr: STD_LOGIC_VECTOR(3  downto 0);
      data: STD_LOGIC_VECTOR(31 downto 0)) is
   begin
      wait until rising_edge(wb_clk);
      wb_we_i     <= '1';
      wb_stb_i    <= '1';
      wb_cyc_i    <= '1';
      wb_addr_i   <= addr;
      wb_data_i   <= data;
      wait for wb_clk_period*3/2;
      wait until rising_edge(wb_clk);
      wb_we_i     <= '0';
      wb_stb_i    <= '0';
      wb_cyc_i    <= '0';
   end procedure;


   procedure assert16_to_file(data:STD_LOGIC_VECTOR(15 downto 0);
   reference:STD_LOGIC_VECTOR(15 downto 0);
   test_id :string; msg :string) is

      variable line_buffer : LINE;
   begin
      if data = reference then
         write(line_buffer, string'(test_id&sep&"OK"&sep&msg)); 
      else
         write(line_buffer, string'(test_id&sep&"Fail"&sep&msg)); 
      end if;
      writeline(file_handler, line_buffer);
      assert data = reference  report "Bad"&msg; 
   end procedure;

   procedure assert32_to_file(data:STD_LOGIC_VECTOR(31 downto 0);
   reference:STD_LOGIC_VECTOR(31 downto 0);
   test_id :string; msg :string) is

      variable line_buffer : LINE;
   begin
      if data = reference then
         write(line_buffer, string'(test_id&sep&"OK"&sep&msg)); 
      else
         write(line_buffer, string'(test_id&sep&"Fail"&sep&msg)); 
      end if;
      writeline(file_handler, line_buffer);
      assert data = reference  report "Bad"&msg; 
   end procedure;


   variable header_line_buffer : LINE;

begin
   file_open(file_handler, log_path, WRITE_MODE);
   write(header_line_buffer, string'("Test ID"&sep&"Status"&sep&"Description"));
   writeline(file_handler, header_line_buffer);

   wait for 2*wb_clk_period;
   rst;
   GENERAL1_data <= (others => '0');    
   GENERAL2_data <= (others => '0');
   GENERAL3_data <= (others => '0');
   GENERAL4_data <= (others => '0');


   wait for 2*wb_clk_period;
   read_wishbone(CTRL_addr);
      assert32_to_file(wb_data_o, X"00000000","0x0","CTRL register initialization");
   read_wishbone(STAT_addr);
      assert32_to_file(wb_data_o, X"00000000","0x1","STAT register initialization");
   read_wishbone(GENERAL1_addr);
      assert32_to_file(wb_data_o, X"00000000","0x2","GENERAL1 register initialization");
   read_wishbone(GENERAL2_addr);
      assert32_to_file(wb_data_o, X"00000000","0x3","GENERAL2 register initialization");
   read_wishbone(GENERAL3_addr);
      assert32_to_file(wb_data_o, X"00000000","0x4","GENERAL3 register initialization");
   read_wishbone(GENERAL4_addr);
      assert32_to_file(wb_data_o, X"00000000","0x5","GENERAL4 register initialization");

   GENERAL1_data  <= X"09ABCDEF";
   GENERAL2_data  <= X"12345678";
   GENERAL3_data  <= X"FEDCBA90";
   GENERAL4_data  <= X"87654321";

   write_wishbone(GENERAL1_addr, GENERAL1_data);
   read_wishbone(GENERAL1_addr);
      assert16_to_file(wb_data_o(15 downto 0), GENERAL1_data(15 downto 0),"0x6","GENERAL1 readback");
   write_wishbone(GENERAL2_addr, GENERAL2_data);
   read_wishbone(GENERAL2_addr);
      assert16_to_file(wb_data_o(15 downto 0), GENERAL2_data(15 downto 0),"0x7","GENERAL2 readback");
   write_wishbone(GENERAL3_addr, GENERAL3_data);
   read_wishbone(GENERAL3_addr);
      assert16_to_file(wb_data_o(15 downto 0), GENERAL3_data(15 downto 0),"0x8","GENERAL3 readback");
   write_wishbone(GENERAL4_addr, GENERAL4_data);
   read_wishbone(GENERAL4_addr);
      assert16_to_file(wb_data_o(15 downto 0), GENERAL4_data(15 downto 0),"0x9","GENERAL4 readback");

  --TODO: we have to internally wire ICAP_SPARTAN6 so to readback and do an assert
   CTRL_data     <= X"0000000"&OP_FULLMULTIBOOT;
   write_wishbone(CTRL_addr, CTRL_data);
   --TODO: Here it is a problem inside
  -- Manually program a full multiboot operation
--   CTRL_data     <= X"0000000"&OP_GENERAL1;
--   write_wishbone(CTRL_addr, CTRL_data);
--   CTRL_data     <= X"0000000"&OP_GENERAL2;
--   write_wishbone(CTRL_addr, CTRL_data);
--   CTRL_data     <= X"0000000"&OP_GENERAL3;
--   write_wishbone(CTRL_addr, CTRL_data);
--   CTRL_data     <= X"0000000"&OP_GENERAL4;
--   write_wishbone(CTRL_addr, CTRL_data);
--   CTRL_data     <= X"0000000"&OP_IPROG;
--   write_wishbone(CTRL_addr, CTRL_data);

   assert false report "No error. Simulation ends." severity failure;
end process tb;

  end;
