library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;

entity Encoder is
    Port ( CLK      : in  STD_LOGIC;
           RESETn	: in  std_logic;
           A        : in  STD_LOGIC;
           B        : in  STD_LOGIC;
           I        : in  STD_LOGIC;
           ENA_I    : in  STD_LOGIC;
           EQEP_ERR : out STD_LOGIC;
           ENC_POS  : out STD_LOGIC_VECTOR (31 downto 0));
end Encoder;
--------------------------------------------------------------------------------
architecture Behavioral of Encoder is
--------------------------------------------------------------------------------
-- FSM declaration
--------------------------------------------------------------------------------
type state is (st00, st01, st11, st10);
signal curr_st : state ;
--------------------------------------------------------------------------------
-- Signal declaration
--------------------------------------------------------------------------------
signal err          : std_logic;
signal pos_cnt      : integer;
signal temp         : std_logic_vector(1 downto 0);
signal enc_in       : std_logic_vector(1 downto 0);
signal dr_I         : std_logic_vector(1 downto 0) := (others=>'0');	    
--------------------------------------------------------------------------------
-- Attributes
--------------------------------------------------------------------------------
attribute syn_preserve : boolean;
attribute syn_preserve of Behavioral : architecture is true; 
attribute syn_noprune : boolean;
attribute syn_noprune of Behavioral : architecture is true;

begin  
--------------------------------------------------------------------------------
-- concurrent signal routing 
--------------------------------------------------------------------------------
ENC_POS     <= std_logic_vector(to_signed(pos_cnt,32));
dr_I        <= dr_I(0) & I when rising_edge(CLK); -- double register input pin
EQEP_ERR    <= err;  

--------------------------------------------------------------------------------
-- Async Input handling, double registering
--------------------------------------------------------------------------------
process (CLK, RESETn)
begin 
    if RESETn = '0' then
        temp <=(others=>'0');
        enc_in <=(others=>'0');
	elsif rising_edge(CLK) then
        temp <= A & B;  
        enc_in <= temp;
    end if;
end process;
--------------------------------------------------------------------------------
-- FSM using a position counter
--------------------------------------------------------------------------------
process (CLK, RESETn)    
variable cu, cd : std_logic := '0';
begin

    if RESETn = '0' then
        curr_st <= st00; 
        pos_cnt <= 0;
        cu := '0'; 
        cd := '0';
        err <= '0';

	elsif rising_edge(CLK) then

        -- Position Counter
        if cu='1' then 
            pos_cnt <= pos_cnt + 1;
        elsif cd='1' then 
            pos_cnt <= pos_cnt - 1;
        end if;

        -- index clear count + quadrature error flag
        if ENA_I then
            if dr_I = "01" then
                pos_cnt <= 0;
		--		err <= '0';
        --       if pos_cnt>3 or pos_cnt<-3 then
        --            err <= '1';
        --        end if;
            end if;
            if dr_I = "10" then
                if pos_cnt>3 or pos_cnt<-3 then
                    err <= '1';
                else
                    err <= '0';
                end if;
            end if;
        end if;

        -- FSM
        cu := '0'; 
        cd := '0';
        case curr_st is
          when st00 => 
                if enc_in = "01" then 
                    curr_st <= st01; 
                    cu := '1';
                elsif enc_in = "10" then
                    curr_st <= st10; 
                    cd := '1';
                end if;

          when st01 => 
                if enc_in = "11" then 
                    curr_st <= st11; 
                    cu := '1';
                elsif enc_in = "00" then 
                    curr_st <= st00; 
                    cd := '1';
                end if;

          when st11 => 
                if enc_in = "10" then 
                    curr_st <= st10; 
                    cu := '1';
                elsif enc_in = "01" then 
                    curr_st <= st01; 
                    cd := '1';
                end if;

          when st10 => 
                if enc_in = "00" then 
                    curr_st <= st00; 
                    cu := '1';
                elsif enc_in = "11" then 
                    curr_st <= st11; 
                    cd := '1';
                end if;
-----------------------------------
			when others =>
				curr_st <= st00;
        end case;

    end if;
  end process;
end Behavioral;