library ieee;
use ieee.std_logic_1164.all;

package control_package is
	-- component for the entire control module
	component  control is
		port( slow_clk, reset, enable, stop, obj_ahead : in std_logic;
			light_dir		: in std_logic_vector(2 downto 0);
			wall_contact	: in std_logic_vector(2 downto 0);
			
			fwd_back		: out std_logic_vector(3 downto 0);
			left_right		: out std_logic_vector(3 downto 0)
		);
	end component;
	
	-- component for the light following module
	component light_control is
		port(reset, enable : in std_logic;
			light_dir: in std_logic_vector(2 downto 0);
			obj_ahead: in std_logic;
	
			fwd_back, left_right: out std_logic_vector(3 downto 0)
		);
	end component light_control;

	-- component for following the walls
	component wall_control is
		generic (
		-- number of 80 kHz clock cycles to back up for
			backup_time : positive := 160000
		);
		port (
	    -- clk is rising edge sensitive
	    -- reset and enable are active high
		    clk, reset : in std_logic;
			wall_contact : in std_logic_vector(2 downto 0);
		-- done is active high
			done : out std_logic;
			fwd_back, left_right : out std_logic_vector(3 downto 0)
		);
	end component wall_control;

end control_package;

library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_arith.all;
use ieee.std_logic_unsigned.all;
library work;
use work.control_package.all;

entity control is
	port(	slow_clk, reset, enable, obj_ahead, stop	: in std_logic;
			light_dir		: in std_logic_vector(2 downto 0);
			wall_contact	: in std_logic_vector(2 downto 0);

			fwd_back		: out std_logic_vector(3 downto 0);
			left_right		: out std_logic_vector(3 downto 0)
	);
end control;

architecture mixed of control is
signal fwd_back_wall, fwd_back_light : std_logic_vector(3 downto 0);
signal left_right_wall, left_right_light : std_logic_vector(3 downto 0);
signal done : std_logic;
signal count : std_logic_vector(15 downto 0);

begin
		
	-- instatiate the light control
	-- light_dir is
	-- 000 front
	-- 001 front right
	-- 010 right
	-- 011 back right
	-- 100 back
	-- 101 back left
	-- 110 left
	-- 111 front left
	light_control_1 : component light_control 
		port map(
			-- inputs
			reset => reset,
			enable => enable,
			light_dir => light_dir,
			obj_ahead => obj_ahead,
	
			-- outputs
			fwd_back => fwd_back_light,
			left_right => left_right_light
	);

	-- instatiate the wall control
	wall_control_1 : component wall_control
		port map(
			-- inputs
			clk => slow_clk,
			reset => reset, 
			wall_contact => wall_contact,
		
			--outputs
			done => done,
			fwd_back => fwd_back_wall,
			left_right => left_right_wall
	);

	--determine which module to use based on whether
	--hirra should follow walls or light
	control_selection: process(slow_clk, done, wall_contact)
	begin
		if reset = '0' then
			fwd_back <= "0111";
			left_right <= "0111";
			count <= (others => '0');
		elsif slow_clk'event and slow_clk = '1' then
			if stop = '1' then
				fwd_back <= "0111";
				left_right <= "0111";
			else
				if count >= 20000 then
					count <= (others => '0');					
					if done = '1' and wall_contact = "000" then
						fwd_back <= fwd_back_light;
						left_right <= left_right_light;
					else 
						fwd_back <= fwd_back_wall;
						left_right <= left_right_wall;
					end if;
				else
					count <= count + 1;
				end if;
			end if;
		end if;
	end process control_selection;

end mixed;