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 ServoDriver is
  generic (
      resolution : integer := 8;
      reset_value : integer := 0);
  port (
      clk : in std_logic;
      
      -- original
      pulse_out : out std_logic;
      tick : in std_logic;
      drive : in 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 ServoDriver;

architecture Behavioral of ServoDriver 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)
      := (others => '0');
  signal tick_buf : std_logic;
  signal drive_buf : std_logic;
  
  signal cs_internal : std_logic;
  signal dir_internal : std_logic;
  signal done_internal : std_logic;
  
  signal cs_buf : std_logic;
  
  signal do_reload : boolean;
  signal reloading : boolean;
begin

  reg : SmallRegister 
      generic map(resolution, reset_value)
      port map(
          clk => clk, 
          data_in => pulse_width_in, 
          data_out => pulse_width, 
          dir => dir_internal,
          cs => cs_internal,
          done => done_internal,
          reset => reset);

  process(clk) begin
    if clk'event and clk = '1' then
      tick_buf <= tick;
      drive_buf <= drive;
    end if;
  end process;
  
  process(clk, reset) begin
    if reset = '1' then
      do_reload <= false;
    elsif clk'event and clk = '1' then
      if drive_buf = '0' and drive = '1' then
        do_reload <= true;
      elsif reloading and done_internal = '1' then
        do_reload <= false;
      else
        do_reload <= do_reload;
      end if;
    end if;
  end process;
  
  process(clk, reset) begin
    if reset = '1' then
      reloading <= false;
    elsif clk'event and clk = '1' then
      if do_reload and cs = '0' then
        reloading <= true;
      elsif reloading and done_internal = '1' then
        reloading <= false;
      else
        reloading <= reloading;
      end if;
    end if;
  end process;
  
  process(clk) begin
    if clk'event and clk = '1' then
      if do_reload and cs = '0' then
        cs_internal <= '1';
        dir_internal <= '0';
      elsif reloading then
        if done_internal = '1' then
          cs_internal <= '0';
          dir_internal <= dir;
        else
          cs_internal <= cs_internal;
          dir_internal <= dir_internal;
        end if;
      else
        cs_internal <= cs;
        dir_internal <= dir;
      end if;
    end if;
  end process;
  
  pulse_width_out <= pulse_width when cs = '1' 
      else (others => 'Z');
  
  done <= '0' when cs = '1' and (reloading or cs_internal = '0')
      else done_internal when cs = '1'
      else 'Z';
  
  process(clk, reset) begin
    if reset = '1' then
      counter <= (others => '0');
    elsif clk'event and clk = '1' then
      if reloading and done_internal = '1' then
        counter <= pulse_width;
      elsif tick_buf = '0' and tick = '1' then
        if counter > conv_std_logic_vector(0, resolution) then
          counter <= counter - 1;
        else
          counter <= counter;
        end if;
      else
        counter <= counter;
      end if;
    end if;
  end process;
  
  pulse_out <= '1' when (counter > conv_std_logic_vector(0, resolution)) and (reset /= '1')
      else '0';
end Behavioral;