library ieee;
use ieee.std_logic_1164.all;
USE IEEE.numeric_std.ALL;

entity Encoder_core is
  generic(
    pulses : integer;
    pairs : integer;
    clk_freq : integer;
    inv_dir : std_logic
  );
	port (
    sens_a : in std_logic;
    sens_b : in std_logic;
    sens_index : in std_logic;
    zero_calib : in std_logic;
    clk : in std_logic;
    angle : out unsigned (15 downto 0);
    speed : out signed (15 downto 0);
    zero_calib_done : out std_logic    
	);
end Encoder_core;

architecture Behavioral of Encoder_core is

  constant MAX_SPEED_CNT : integer := (60000000/pulses) * clk_freq;
  
  signal angle_counter : integer := 0;
  signal speed_counter : integer := 0;
  signal zero_offset : integer := 0;
  
  signal speed_d1 : signed (15 downto 0) := (others => '0');
  signal speed_d2 : signed (15 downto 0) := (others => '0');
  signal speed_d3 : signed (15 downto 0) := (others => '0');
  signal speed_d4 : signed (15 downto 0) := (others => '0');
  signal speed_d5 : signed (15 downto 0) := (others => '0');
	
  signal sens_a_in : std_logic;
  signal sens_b_in : std_logic;
  signal sens_index_in : std_logic;
  signal zero_calib_in : std_logic;
  signal sens_in : std_logic_vector(1 downto 0);
  signal sens_q1 : std_logic;
  signal sens_q2 : std_logic;
  signal delay_sens_q1 : std_logic;
  signal delay_sens_q2 : std_logic;
  signal delay_index_in : std_logic;
  signal delay_zero_calib_in : std_logic;
  signal sens_event : std_logic;
  signal index_event : std_logic;
  signal zero_calib_event : std_logic;
  signal sens_direction : std_logic;
  
begin
  rotary_filter: process(clk)
  begin
    if clk'event and clk='1' then

      --Synchronise inputs to clock domain using flip-flops in input/output blocks.
      sens_a_in <= sens_a;
      sens_B_in <= sens_b;
      sens_index_in <= sens_index;
      zero_calib_in <= zero_calib;

      --concatinate rotary input signals to form vector for case construct.
      sens_in <= sens_b_in & sens_a_in;

      case sens_in is

        when "00" => sens_q1 <= '0';         
                     sens_q2 <= sens_q2;
 
        when "01" => sens_q1 <= sens_q1;
                     sens_q2 <= '0';

        when "10" => sens_q1 <= sens_q1;
                     sens_q2 <= '1';

        when "11" => sens_q1 <= '1';
                     sens_q2 <= sens_q2; 

        when others => sens_q1 <= sens_q1; 
                       sens_q2 <= sens_q2; 
      end case;
    end if;
  end process rotary_filter;
  
  sens_event_detector: process(clk)
  begin
    if clk'event and clk='1' then

      delay_sens_q1 <= sens_q1;
      delay_sens_q2 <= sens_q2;
      
      if sens_q1='1' and delay_sens_q1='0' then
        sens_event <= '1';
        sens_direction <= sens_q2 xor inv_dir;              
      else
        sens_event <= '0';
        sens_direction <= sens_direction;
      end if;
      
    end if;
  end process sens_event_detector;
  
  index_event_detector: process(clk)
    begin
      if clk'event and clk='1' then
  
        delay_index_in <= sens_index_in;
        
        if sens_index_in='1' and delay_index_in='0' then
          index_event <= '1';
        else
          index_event <= '0'; 
        end if;  
        
      end if;
    end process index_event_detector;
    
    calib_event_detector: process(clk)
        begin
          if clk'event and clk='1' then
      
            delay_zero_calib_in <= zero_calib_in;
            
            if zero_calib_in='1' and delay_zero_calib_in='0' then
              zero_calib_event <= '1';
            else
              zero_calib_event <= '0'; 
            end if;  
            
          end if;
        end process calib_event_detector;
  
      speed_cnt: PROCESS (clk)
        VARIABLE   cnt         : INTEGER RANGE 0 TO MAX_SPEED_CNT+1;
      BEGIN
        IF (clk'EVENT AND clk = '1') THEN
          IF sens_event='1' THEN
             cnt := 0;             
          ELSE
             IF cnt <= MAX_SPEED_CNT THEN
               cnt := cnt + 1;
             END IF;
          END IF;
        END IF; 
        speed_counter <= cnt;   
      END PROCESS speed_cnt;
     
     angle_cnt: PROCESS (clk)
        VARIABLE   cnt         : INTEGER RANGE 0 TO pulses;
        --VARIABLE   inc    : INTEGER;
     BEGIN
--        IF (sens_direction = '1') THEN
--            inc := 1;
--        ELSE
--            inc := -1;
--        END IF;
              
        IF (clk'EVENT AND clk = '1') THEN
            IF zero_calib_event='1' THEN
                cnt := 0;
            ELSE
                IF index_event='1' and zero_calib_in='0' THEN
                    cnt := zero_offset;
                ELSE
                    IF sens_event='1' THEN
                        if sens_direction = '1' then
                            if cnt<pulses then
                                cnt := cnt + 1;
                            else
                                cnt:=0;
                            end if;
                        else
                            if cnt>0 then
                                cnt := cnt - 1;
                            else
                                cnt := pulses-1;
                            end if;
                        end if;                       
                    END IF;
                END IF;
            END IF;    
        END IF;
        angle_counter <= cnt;
    END PROCESS angle_cnt;
  
  offset: process(clk)
  begin
    if clk'event and clk='1' then
      if index_event='1' and zero_calib_in='1' then
         zero_offset <= angle_counter;
         zero_calib_done <= '1';   
      else
         zero_calib_done <= '0';       
	  end if;
	  
    end if;
  end process offset;
  
  speed_calc: process (clk) begin
    IF (clk'EVENT AND clk = '1') THEN
        if (speed_counter > MAX_SPEED_CNT) then
            speed_d1 <= (others => '0');
        elsif sens_event='1' THEN
            if sens_direction = '1' then
                speed_d1 <= to_signed(MAX_SPEED_CNT / speed_counter,16);
            else
                speed_d1 <= to_signed(-MAX_SPEED_CNT / speed_counter,16); 
            end if;
            speed_d2 <= speed_d1;
            speed_d3 <= speed_d2;
            speed_d4 <= speed_d3;
            speed_d5 <= speed_d4; 
            speed <= (speed_d1 + speed_d2 + speed_d3 + speed_d4 + speed_d5) / 5;     
        end if;
        angle <= to_unsigned((angle_counter mod (pulses/pairs)) * 65536 * pairs / pulses,16);
    end if;        
  end process speed_calc;     
  
end Behavioral;
