library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
use IEEE.std_logic_arith.all;
library work;
use work.usefuls.ALL;

entity ServoObserver is
  generic (
      bus_width : integer := 8;
      resolution : integer := 8;
      reset_value : integer := 0;
      threshold : integer := 0);
  port (
      clk : in std_logic;
      reset : in std_logic;
      
      -- original
      pulse_in : in std_logic;
      tick : in std_logic;
      updated : out std_logic;
      over_threshold : out std_logic;
      
      -- data IF
      data_in : in std_logic_vector(bus_width-1 downto 0);
      data_out : out std_logic_vector(bus_width-1 downto 0);
      dir : in std_logic;    -- write => '1', read => '0'
      cs : in std_logic;
      addr : in std_logic_vector(min_bits((resolution - 1) / bus_width) - 1 downto 0);
      done : out std_logic);  -- complete write / valid output
end ServoObserver;

architecture Behavioral of ServoObserver is
  signal pulse_width : std_logic_vector(resolution-1 downto 0);
  signal counter : std_logic_vector(pulse_width'range);
  signal tick_buf : std_logic;
  signal pulse_in_buf : std_logic;
  signal cs_buf : std_logic;
begin

  process(clk) begin
    if clk'event and clk = '1' then
      tick_buf <= tick;
      if tick_buf = '0' and tick = '1' then
        pulse_in_buf <= pulse_in;
      else
        pulse_in_buf <= pulse_in_buf;
      end if;
      cs_buf <= cs;
    end if;
  end process;

  process(clk, reset) 
    variable pulse_width_in : std_logic_vector(pulse_width'range);
  begin
    if reset = '1' then -- async reset
      pulse_width <= conv_std_logic_vector(reset_value, pulse_width'length);
    elsif clk'event and clk = '1' then
      if (tick_buf = '0' and tick = '1') 
          and (pulse_in_buf = '1' and pulse_in = '0') then
        pulse_width <= counter;
      elsif cs_buf = '0' and cs = '1' and dir = '1' then
        pulse_width_in := pulse_width;
        for i in pulse_width'range loop
          if addr = conv_std_logic_vector(i / data_in'length, addr'length) then
            pulse_width_in(i) := data_in(i mod data_in'length);
          end if;
        end loop;
        pulse_width <= pulse_width_in;
      else 
        pulse_width <= pulse_width;
      end if;
    end if;
  end process;
  
  process(clk)
    variable data_out_res : std_logic_vector(data_out'high downto 0); 
  begin
    if clk'event and clk = '1' then
      if cs = '1' then
        data_out_res := (others => '0');
        for i in resolution - 1 downto 0 loop
          if addr = conv_std_logic_vector(i / bus_width, addr'length) then
            data_out_res(i mod bus_width) := pulse_width(i);
          end if;
        end loop;
        data_out <= data_out_res;
        done <= '1';
      else
        data_out <= (others => 'Z');
        done <= 'Z';
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if counter >= conv_std_logic_vector(threshold, pulse_width'length) then
        over_threshold <= '1';
      else
        over_threshold <= '0';
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if (tick_buf = '0' and tick = '1') and pulse_in = '1' then
        if pulse_in_buf = '0' then
          counter <= conv_std_logic_vector(0, resolution);
        else
          counter <= counter + 1;
        end if;
      else
        counter <= counter;
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if (tick_buf = '0' and tick = '1') 
          and (pulse_in_buf = '1' and pulse_in = '0') then
        updated <= '1';
      else 
        updated <= '0';
      end if;
    end if;
  end process;
  
end Behavioral;