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;

entity ServoObserver is
  generic (
      resolution : integer := 8;
      reset_value : integer := 0);
  port (
      clk : in std_logic;
      
      -- original
      pulse_in : in std_logic;
      tick : in std_logic;
      updated : out std_logic;
      
      -- data IF
      pulse_width_in : in std_logic_vector(resolution-1 downto 0);
      pulse_width_out : out std_logic_vector(resolution-1 downto 0);
      dir : in std_logic;    -- write => '1', read => '0'
      cs : in std_logic;     -- chip select
      done : out std_logic;  -- complete write / valid output
      reset : in std_logic);
end ServoObserver;

architecture Behavioral of ServoObserver is
  component SmallRegister is
    generic (
        bits : integer := 8;
        reset_value : integer := 0);
    port (
        clk : in std_logic;
        data_in : in std_logic_vector(bits-1 downto 0);
        data_out : out std_logic_vector(bits-1 downto 0);
        dir : in std_logic;    -- write register => '1', read register => '0'
        cs : in std_logic;     -- chip select
        done : out std_logic;  -- complete write / valid output
        reset : in std_logic);
  end component;

  signal pulse_width : std_logic_vector(resolution-1 downto 0);
  signal counter : std_logic_vector(resolution-1 downto 0);
  signal data_out_buf : std_logic_vector(resolution-1 downto 0);
  
  signal tick_buf : std_logic;
  signal pulse_in_buf, pulse_in_buf2 : std_logic;
  signal updating : boolean;
  
  signal dir_internal : std_logic;
  signal cs_internal : std_logic;
  signal done_internal, done_internal_buf : std_logic;
begin
  reg : SmallRegister 
      generic map(resolution, reset_value)
      port map(
          clk => clk, 
          data_in => pulse_width, 
          data_out => data_out_buf, 
          dir => dir_internal,
          cs => cs_internal,
          done => done_internal,
          reset => reset);
  
  process(clk) begin
    if clk'event and clk = '1' then
      tick_buf <= tick;
      pulse_in_buf <= pulse_in;
      pulse_in_buf2 <= pulse_in_buf;
    end if;
  end process;

  process(clk) begin
    if clk'event and clk = '1' then
      if pulse_in_buf2 = '0' and pulse_in_buf = '1' then
        counter <= conv_std_logic_vector(1, resolution);
      elsif (tick_buf = '0' and tick = '1') and pulse_in_buf = '1' then
        counter <= counter + 1;
      else null;
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if (pulse_in_buf2 = '1' and pulse_in_buf = '0' and cs = '0') 
          or updating then
        pulse_width <= counter;
      else 
        pulse_width <= pulse_width_in;
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if (pulse_in_buf2 = '1' and pulse_in_buf = '0' and cs = '0') 
          or (updating and done_internal_buf /= '1') then
        updating <= true;
      else
        updating <= false;
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if updating and done_internal = '1' then
        updated <= '1';
      else
        updated <= '0';
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if (pulse_in_buf2 = '1' and pulse_in_buf = '0' and cs = '0') then
        dir_internal <= '1';
        cs_internal <= '1';
      elsif updating then
        dir_internal <= '1';
        if done_internal_buf = '0' and done_internal = '1' then
          cs_internal <= '0';
        else null;
        end if;
      else
        dir_internal <= dir;
        cs_internal <= cs;
      end if;
      done_internal_buf <= done_internal;
    end if;
  end process;
  
  pulse_width_out <= data_out_buf when cs = '1' 
      else (others => 'Z');
  
  done <= '0' when cs = '1' and (updating or cs_internal = '0')
      else done_internal when cs = '1'
      else 'Z';
end Behavioral;