HC-SR04 ultrasonic sensor VHDL

library ieee;
use ieee.std_logic_1164.all;

entity sensor_driver is

  generic(
    triger_pulse: integer:= 6; -- assume that every clock is 1ms
    max_pulse_echo: integer:= 500;
    wait_time: integer:= 20;
    -- time = (2*distance/speed)
    -- (2*0.01/343) = 58.3μ
    -- pulses = time/(clock_time)
    -- number of pulses = 58.3μ/0.02μ(50MHZ) = 2915 pulses
    number_of_pulses_per_cm : integer:= 2915
    );
  port( 
    ECHO, RST, SYS_CLK, TRIG_IN :in std_logic;
    EOC, TRIG_OUT: out std_logic;
    DURATION : out integer range 0 to max_pulse_echo);
end entity;

architecture behave of sensor_driver is
  signal cnt_echo : integer range 0 to max_pulse_echo + 1 :=0 ;
  signal cnt : integer range 0 to triger_pulse := 0;
  signal mode : integer range 0 to 4:=0;
  signal start_get_echo : integer range 0 to 1 :=0;
  signal wait_t : integer range 0 to wait_time :=0;
  signal data_in_cm : integer range 0 to max_pulse_echo :=0;
  signal counter_cm_cycles : integer range 0 to number_of_pulses_per_cm + 1 :=0;
  begin
    DURATION <= data_in_cm;
    process(SYS_CLK,RST)
    begin
      if RST = '1' then
       mode <= 0;
       wait_t <= 0;
       start_get_echo <= 0;
       cnt <= 0;
       TRIG_OUT <= '0';
       EOC <= '0';
       cnt_echo <= 0;
       data_in_cm<= 0 ;
       counter_cm_cycles<= 0;
       elsif rising_edge(SYS_CLK) then
         EOC <= '0';
         if TRIG_IN = '1' then
       if mode = 0 then -- sends trig
         if cnt = triger_pulse - 1 then
           TRIG_OUT <= '0';
           cnt <= 0;
           mode <= 1;
           else
           TRIG_OUT <= '1';
           cnt <= cnt + 1;
           cnt_echo <= 0;
           data_in_cm <= 0;
           counter_cm_cycles <= 0;
           start_get_echo <= 0;
         end if;
       elsif mode = 1 then -- waits for echo
        if start_get_echo = 0 then
          if ECHO = '1' then
            cnt_echo <= cnt_echo + 1;
            counter_cm_cycles <=counter_cm_cycles + 1;
            start_get_echo <= 1;
          end if;
          elsif start_get_echo = 1 then
            if ECHO = '1' then
              cnt_echo <= cnt_echo + 1;
              counter_cm_cycles <=counter_cm_cycles + 1;
              if counter_cm_cycles >= number_of_pulses_per_cm - 1 then
                counter_cm_cycles <= 0;
                data_in_cm <= data_in_cm + 1;
              end if;
              if cnt_echo = max_pulse_echo - 1 then
                mode <= 3;
              end if;
              else
              start_get_echo <= 0;
              mode <= 2;
            end if;
          end if;
       elsif mode = 2 then -- waits to send EOC after end of echo
         if wait_t = wait_time - 1 then
           wait_t <= 0;
            mode <= 4; 
           cnt <= 0;
           EOC <= '1';
           else
           wait_t <= wait_t + 1;
         end if;
         
       elsif mode = 3 then -- waits to send EOC after end of echo, when number is 999cm
         if ECHO = '1' then
           else
           if wait_t = wait_time - 1 then
            wait_t <= 0;
            mode <= 4; 
            cnt <= 0;
            EOC <= '1';
            else
            wait_t <= wait_t + 1;
          end if;
        end if;

       elsif mode = 4 then -- end of measurement.
        mode <= 0;
      end if;

    end if;
  end if;
end process;

end architecture;

TB:

library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_unsigned.all;

entity sensor_driver_tb is
end entity;

architecture behave of sensor_driver_tb is
  
component sensor_driver is
  generic(triger_pulse: integer:= 6; -- assume that every clock is 1ms
  max_pulse_echo: integer:= 500;
  wait_time: integer:= 20);
port( 
  ECHO, RST, SYS_CLK, TRIG_IN :in std_logic;
  EOC, TRIG_OUT: out std_logic;
  DURATION : out integer range 0 to max_pulse_echo);
end component;

-- Constants declaration
	constant C_CLK_PRD	: time := 20 ns; -- F_s = 50Mhz
	constant trig_p: integer := 600; -- what's needed for triger :
	-- 600*20 ns = 12000 ns = 12 us 
	constant max_p_v: integer := 1800000; -- 36 ms / 20 ns =
	-- 36 / 20 * 10^6 = 1800000
	constant w_t: integer :=  2500000; --  2500000*20ns = 50ms
	
-- signals
  
	signal clk		: std_logic := '0';
	signal rst		: std_logic := '0';
	signal echo : std_logic := '0';
	signal trig_in : std_logic := '0';
	signal trig_out : std_logic := '0';
	signal eoc : std_logic;
	signal dur : integer range 0 to max_p_v;
	
begin
-- ********************* clock & reset generation ********************
	clk <= not clk after C_CLK_PRD / 2;
	rst <= '1', '0' after 1 us;
	trig_in <= '0', '1' after 5 us;
	echo <= '0', '1'  after 20 us, '0' after 30 us;
-- *******************************************************************
U1: sensor_driver
  generic map(triger_pulse => trig_p, -- assume that every clock is 1ms
  max_pulse_echo => max_p_v,
  wait_time => w_t)
  port map( 
  ECHO => echo,
  RST => rst,
  SYS_CLK => clk, 
  TRIG_IN => trig_in,
  EOC => eoc,
  TRIG_OUT => trig_out,
  DURATION => dur
  );

end architecture;