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 (35 downto 0);
           adder_enable : in STD_LOGIC;
           --fifo_wr_en : out STD_LOGIC;
           max_found : out STD_LOGIC;
	   val_out : out STD_LOGIC_VECTOR (35 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 cycle : integer := 400;--(2,4,6,0,6,4,2);--40
  signal valind : integer := 0;
  signal val0 : STD_LOGIC_VECTOR (35 downto 0); --change if decimating squared signal
  signal val1 : STD_LOGIC_VECTOR (35 downto 0); --change if decimating squared signal
  signal val2 : STD_LOGIC_VECTOR (35 downto 0); --change if decimating squared signal


begin
  process (clk) 
    variable i : integer := 0;
    variable j : integer := 0;
    variable k : integer := 0; 
    variable k_0 : integer := 1; 	
    variable k_1 : integer := 0; 
    variable k_2 : integer := 0;
    variable k_3 : integer := 0; 
	
  begin
    if (rising_edge(clk)) then
        case state is
          -- Waiting for start
          when IDLE =>

		max_found <= '0'; --reset
		if (start = '1') then
		   state <= STORE;	
		end if;
	  when STORE =>   
  		if (i=0) then
		   i:=1;
		   val0 <= val_in;
		end if;
		if (i='1') then
		   if (j<'7') then
			j:=j+1;
		   else
			i:=2;
		   	val1 <= val_in;
		   end if;
		end if;
		if (i='2') then
		   if (j<'15') then
			j:=j+1;
		   else
			state <= COMPARE;
		   	val2 <= val_in;
			j:=0;
			i:=0;
		   end if;
		end if;
	  when COMPARE =>
		if (val1>val0) and (val1>val2) then
				max_found <= '1'; --maximum found, go to next angle
				val_out<=val1;	--can do more detailed comparison to get better value but probably not necessary
				state <= IDLE;
		else
			state<=COMPARE;
		end if;        
		                                  
        end case;
    end if;
  end process;
end behavior;


