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

entity 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 wall_control;

architecture behavioral of wall_control is
-- these constants allow changing the speed/turning radius
-- by changing just one value
constant forward : std_logic_vector(3 downto 0) := "0011";
constant right : std_logic_vector(3 downto 0) := "1111";
constant left : std_logic_vector(3 downto 0) := "0000";
constant back : std_logic_vector(3 downto 0) := "1010";
constant zero : std_logic_vector(3 downto 0) := "0111";
type state_type is (normal, backup);
signal state : state_type;
signal backup_timer : std_logic_vector(18 downto 0);

begin
   process (clk, reset)
   begin
      if reset = '0' then
         done <= '1';
         fwd_back <= zero;
         left_right <= zero;
         state <= normal;
         backup_timer <= (others => '0');
      elsif clk'event and clk = '1' then
         case state is

            -- a simple wait state which allows the wall contact
            -- module to remain in control even though there is
            -- no wall contact
            when backup => if backup_timer >= backup_time then
                              state <= normal;
                           else
                              backup_timer <= backup_timer + 1;
                           end if;

            -- determines which direction to move the car depending
            -- upon what whiskers are in contact with an object
            when normal => -- no wall contact
                           if wall_contact = "000" then
                              left_right <= zero;
                              fwd_back <= zero;
                              done <= '1';
                           -- wall right or left
                           elsif (wall_contact = "010") or
                                 (wall_contact = "011") then
                              left_right <= zero;
                              fwd_back <= forward;
                              done <= '0';
                           -- too close right
                           elsif wall_contact = "100" then
                              left_right <= left;
                              fwd_back <= forward;
                              done <= '0';
                           -- too close left
                           elsif wall_contact = "101" then
                              left_right <= right;
                              fwd_back <= forward;
                              done <= '0';
                           -- collision right
                           elsif wall_contact = "110" then
                              left_right <= right;
                              fwd_back <= back;
                              state <= backup;
                              backup_timer <= (others => '0');
                              done <= '0';
                           -- collision left or general collision
                           else
                              left_right <= left;
                              fwd_back <= back;
                              state <= backup;
                              backup_timer <= (others => '0');
                              done <= '0';
                           end if;
            when others => state <= normal;
                           backup_timer <= (others => '0');
         end case;
      end if;
   end process;
end behavioral;