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

architecture Behavioral of ServoDriver is
  signal pulse_width : std_logic_vector(resolution-1 downto 0);
  signal counter : std_logic_vector(pulse_width'range)
      := (others => '0');
  signal drive_buf : std_logic;
  signal tick_buf : std_logic;
  signal cs_buf : std_logic;
begin

  process(clk) begin
    if clk'event and clk = '1' then
      tick_buf <= tick;
      drive_buf <= drive;
      cs_buf <= cs;
    end if;
  end process;
  
  process(clk)
    variable pulse_width_in : std_logic_vector(pulse_width'range);
  begin
    if clk'event and clk = '1' then
	  if reset = '1' then -- sync reset
	    pulse_width <= conv_std_logic_vector(reset_value, pulse_width'length);
      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, reset, drive, pulse_width) begin
    if reset = '1' then
      counter <= (others => '0');
    elsif clk'event and clk = '1' then
      if drive_buf = '0' and drive = '1' then
        counter <= pulse_width;
      elsif tick_buf = '0' and tick = '1'
          and counter > conv_std_logic_vector(0, resolution) then
        counter <= counter - 1;
      else
        counter <= counter;
      end if;
    end if;
  end process;
  
  pulse_out <= '1' when (counter > conv_std_logic_vector(0, resolution))
      else '0';
end Behavioral;