library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Std_Logic_Arith.all;

entity EncoderMonitorTB is
end EncoderMonitorTB;

architecture HTWTestBench of EncoderMonitorTB is

component EncoderMonitor
    port (-- Local bus signals
        pnReset          : in  std_ulogic;
        pLCLK            : in  std_ulogic;
        pEncoderReset    : in  std_logic_vector(5 downto 0);
        pEncoderSelect   : in  std_logic_vector(2 downto 0);
        pClicksPerBScan  : in  std_logic_vector(15 downto 0);
        pFractClicks     : in  std_logic_vector(7 downto 0);
        pBScanActive     : in  std_ulogic;
        pEncoder0In      : in  std_logic_vector(1 downto 0);
        pEncoder1In      : in  std_logic_vector(1 downto 0);
        pEncoder2In      : in  std_logic_vector(1 downto 0);
        pEncoder3In      : in  std_logic_vector(1 downto 0);
        pEncoder4In      : in  std_logic_vector(1 downto 0);
        pEncoder5In      : in  std_logic_vector(1 downto 0);
        pSyncIn          : in  std_ulogic;
        pAckIn           : in  std_ulogic;
        pSyncAckEn       : in  std_ulogic;
        pEncoder0Pos     : out std_logic_vector(31 downto 0);
        pEncoder1Pos     : out std_logic_vector(31 downto 0);
        pEncoder2Pos     : out std_logic_vector(31 downto 0);
        pEncoder3Pos     : out std_logic_vector(31 downto 0);
        pEncoder4Pos     : out std_logic_vector(31 downto 0);
        pEncoder5Pos     : out std_logic_vector(31 downto 0);
        pSerialOut       : out std_ulogic;
        pSerialOutEn     : out std_ulogic;
        pBScanTriggerOut : out std_ulogic;
        pBScanTriggerIn  : in  std_ulogic);
end component;

        -- Local bus signals
        signal pnReset          : std_ulogic;
        signal pLCLK            : std_ulogic := '0';
        signal pEncoderReset    : std_logic_vector(5 downto 0);
        signal pEncoderSelect   : std_logic_vector(2 downto 0);
        signal pClicksPerBScan  : std_logic_vector(15 downto 0);
        signal pFractClicks     : std_logic_vector(7 downto 0);
        signal pBScanActive     : std_ulogic;
        signal pEncoder0In      : std_logic_vector(1 downto 0);
        signal pEncoder1In      : std_logic_vector(1 downto 0);
        signal pEncoder2In      : std_logic_vector(1 downto 0);
        signal pEncoder3In      : std_logic_vector(1 downto 0);
        signal pEncoder4In      : std_logic_vector(1 downto 0);
        signal pEncoder5In      : std_logic_vector(1 downto 0);
        signal pSyncIn          : std_ulogic;
        signal pAckIn           : std_ulogic;
        signal pSyncAckEn       : std_ulogic;
        signal pEncoder0Pos     : std_logic_vector(31 downto 0);
        signal pEncoder1Pos     : std_logic_vector(31 downto 0);
        signal pEncoder2Pos     : std_logic_vector(31 downto 0);
        signal pEncoder3Pos     : std_logic_vector(31 downto 0);
        signal pEncoder4Pos     : std_logic_vector(31 downto 0);
        signal pEncoder5Pos     : std_logic_vector(31 downto 0);
        signal pSerialOut       : std_ulogic;
        signal pSerialOutEn     : std_ulogic;
        signal pBScanTriggerOut : std_ulogic;
        signal pBScanTriggerIn  : std_ulogic;
        signal Encoder0Fwd      : std_ulogic;
        signal Encoder0Back     : std_ulogic;
        signal Encoder1Fwd      : std_ulogic;
        signal Encoder1Back     : std_ulogic;

begin
  
  pLCLK <= not pLCLK after 7500 ps; -- 66MHz

  U1 : EncoderMonitor
	port map (pnReset => pnReset,
		        pLCLK => pLCLK,
		        pEncoderReset => pEncoderReset,
		        pEncoderSelect => pEncoderSelect,
		        pClicksPerBScan => pClicksPerBScan,
		        pFractClicks => pFractClicks,
		        pBScanActive => pBScanActive,
		        pEncoder0In => pEncoder0In,
		        pEncoder1In => pEncoder1In,
		        pEncoder2In => pEncoder2In,
		        pEncoder3In => pEncoder3In,
		        pEncoder4In => pEncoder4In,
		        pEncoder5In => pEncoder5In,
		        pSyncIn => pSyncIn,
		        pAckIn => pAckIn,
		        pSyncAckEn => pSyncAckEn,
		        pEncoder0Pos => pEncoder0Pos,
		        pEncoder1Pos => pEncoder1Pos,
		        pEncoder2Pos => pEncoder2Pos,
		        pEncoder3Pos => pEncoder3Pos,
		        pEncoder4Pos => pEncoder4Pos,
		        pEncoder5Pos => pEncoder5Pos,
		        pSerialOut => pSerialOut,
		        pSerialOutEn => pSerialOutEn,
		        pBScanTriggerOut => pBScanTriggerOut,
		        pBScanTriggerIn => pBScanTriggerIn);

  Test: process
  begin
    pnReset <= '0';
    wait for 10 ns;
    pnReset <= '1';
    Encoder0Fwd <= '0';
    Encoder0Back <= '0';
    Encoder1Fwd <= '0';
    Encoder1Back <= '0';


    pEncoderSelect <= "110"; -- dual encoder mode
    pClicksPerBScan <= X"0004";
    pFractClicks <= X"80";
    pBScanActive <= '0';
    pBScanTriggerIn <= '0';

    -- reset encoder counters etc
    wait until rising_edge(pLCLK);
    pEncoderReset <= (0 => '1', others => '0');
    wait until rising_edge(pLCLK);
    pEncoderReset <= (others => '0');

    wait for 2 ms;

    -- encoder 0 only
    for i in 1 to 40 loop
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 1 ms;
    end loop;

    -- both encoders
    for i in 1 to 40 loop
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    wait for 200 us;

    -- both encoders - 1 first
    for i in 1 to 40 loop
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    wait for 200 us;

    -- both encoders -- back to 0 first
    for i in 1 to 40 loop
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    wait for 200 us;

    -- both encoders at the same time
    for i in 1 to 40 loop
      Encoder0Fwd <= '1', '0' after 1 ns;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 1 ms;
    end loop;

    -- both encoders -- alternating pairs
    for i in 1 to 20 loop
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder0Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    -- encoder 1 only
    for i in 1 to 40 loop
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 1 ms;
    end loop;

    -- both encoders one going slower
    for i in 1 to 40 loop
      if (i mod 3) = 0 then
        Encoder0Fwd <= '1', '0' after 1 ns;
      end if;
      wait for 600 us;
      Encoder1Fwd <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    wait for 600 us;

    -- encoder 1 reverse
    for i in 1 to 40 loop
      Encoder1Back <= '1', '0' after 1 ns;
      wait for 1 ms;
    end loop;

    -- both encoders back
    for i in 1 to 40 loop
      Encoder1Back <= '1', '0' after 1 ns;
      wait for 600 us;
      Encoder0Back <= '1', '0' after 1 ns;
      wait for 400 us;
    end loop;

    wait for 600 us;

    -- encoder 0 back
    for i in 1 to 40 loop
      Encoder0Back <= '1', '0' after 1 ns;
      wait for 1 ms;
    end loop;

    wait for 2 ms;
    wait until rising_edge(pLCLK);
    pBScanTriggerIn <= '1';
    wait until rising_edge(pLCLK);
    pBScanTriggerIn <= '0';

    wait for 4 us;

    report "Encoder Testbench: Simulation completed" severity failure;
    wait;

  end process Test;

  -- encoder 0
  Encoder0: process
  begin
    pEncoder0In <= "00";
    loop
      wait on Encoder0Fwd, Encoder0Back;
      if rising_edge(Encoder0Fwd) then
        case pEncoder0In is
          when "00" => pEncoder0In <= "01";
          when "01" => pEncoder0In <= "11";
          when "11" => pEncoder0In <= "10";
          when "10" => pEncoder0In <= "00";
          when others => null;
        end case;
      end if;
      if rising_edge(Encoder0Back) then
        case pEncoder0In is
          when "00" => pEncoder0In <= "10";
          when "01" => pEncoder0In <= "00";
          when "11" => pEncoder0In <= "01";
          when "10" => pEncoder0In <= "11";
          when others => null;
        end case;
      end if;
    end loop;
  end process Encoder0;

  -- encoder 1
  Encoder1: process
  begin
    pEncoder1In <= "00";
    loop
      wait on Encoder1Fwd, Encoder1Back;
      if rising_edge(Encoder1Fwd) then
        case pEncoder1In is
          when "00" => pEncoder1In <= "01";
          when "01" => pEncoder1In <= "11";
          when "11" => pEncoder1In <= "10";
          when "10" => pEncoder1In <= "00";
          when others => null;
        end case;
      end if;
      if rising_edge(Encoder1Back) then
        case pEncoder1In is
          when "00" => pEncoder1In <= "10";
          when "01" => pEncoder1In <= "00";
          when "11" => pEncoder1In <= "01";
          when "10" => pEncoder1In <= "11";
          when others => null;
        end case;
      end if;
    end loop;
  end process Encoder1;

end HTWTestBench;
