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

entity energy_compare is
    Port ( clk : in STD_LOGIC;
           start : in STD_LOGIC;
          -- t_finish : in STD_LOGIC;
	       val_in : in STD_LOGIC_VECTOR(31 downto 0);
          -- adder_enable : in STD_LOGIC;
           --fifo_wr_en : out STD_LOGIC;
           max_found : out STD_LOGIC;
           start_fifo : out STD_LOGIC;
         --  rst : out STD_LOGIC;
	       val_out : out STD_LOGIC_VECTOR(31 downto 0));
end energy_compare;

architecture behavior of energy_compare is
  type STATES is (IDLE,STORE,COMPARE);
  type VALINDS is array (0 to 2) of integer;
  signal state : STATES := IDLE;
  signal valind : integer := 0;
  signal resig : std_logic;
  signal val0 : STD_LOGIC_VECTOR(31 downto 0); --change if decimating squared signal
  signal val1 : STD_LOGIC_VECTOR(31 downto 0); --change if decimating squared signal
  signal val2 : STD_LOGIC_VECTOR(31 downto 0); --change if decimating squared signal


begin
  process (clk) 
    variable fifo_reset : integer := 0;
    variable j : integer := 0;
    variable k : integer := 0; 
	
  begin
    if (rising_edge(clk)) then
        case state is
          -- Waiting for start
          when IDLE =>
          if (fifo_reset=0) and (resig='0') then
          resig <= '1';
          fifo_reset:=1;
          else
          resig <= '0';
          end if;
        j:=0;
		max_found <= '0'; --reset
		start_fifo <= '0';
		val_out <=  std_logic_vector(to_signed(0,32));
		if (start = '1') then
		   state <= STORE;	
		end if;
	  when STORE => 
  		if not(val2 = val_in) then
  		   val0<=val1;
  		   val1 <= val2; 
		   val2 <= val_in;
		   j:=j+1;
		   if (j>2) then
		   state<=COMPARE;
		   end if;
		   end if;
	  when COMPARE =>	    
	    if (start='0') then
	    state <=IDLE;
		start_fifo <= '1';
		val_out<= std_logic_vector(to_signed(0,32));
  		val0<= std_logic_vector(to_signed(0,32));
        val1 <= std_logic_vector(to_signed(0,32)); 
		val2 <=  std_logic_vector(to_signed(0,32));
		else
		if (signed(val1)>signed(val0)) and (signed(val1)>signed(val2)) then
				max_found <= '1'; --maximum found, go to next angle
	           	start_fifo <= '1';
				val_out<=std_logic_vector(to_signed(0,32));--std_logic_vector(signed(val1));	--can do more detailed comparison to get better value but probably not necessary
				state <= IDLE;
                val0<= std_logic_vector(to_signed(0,32));
                val1 <= std_logic_vector(to_signed(0,32)); 
                val2 <=  std_logic_vector(to_signed(0,32));
		else
			state<=STORE;
		end if;        
		end if;
		                                  
        end case;
    end if;
  end process;
end behavior;

