
LIBRARY IEEE;
USE IEEE.std_logic_1164.all;
use IEEE.numeric_std.all;

entity TD_Projection is
  port(
    CLK : in std_logic;
    nRST : in std_logic;
    
-- From Matrix_Math   
    DATA : in std_logic_vector(95 downto 0);
    
-- To Line_Math    
    RESULT : out std_logic_vector(23 downto 0);
-- Controller interface   
    OPCODE : in std_logic_vector(2 downto 0);
    ENABLE : in std_logic;
    BUSY : out std_logic);
end TD_Projection;


architecture TDP_arch of TD_Projection is

component signed_divide
  port (
    X: in  std_logic_vector(31 downto 0);
    Y: in  std_logic_vector(31 downto 0);
    Z: out std_logic_vector(31 downto 0));
end component;

  signal xMultLarge : std_logic_vector(63 downto 0);
  signal yMultLarge : std_logic_vector(63 downto 0);
  
  signal xMultLargeSigned: std_logic_vector(31 downto 0);
  signal yMultLargeSigned: std_logic_vector(31 downto 0);

  signal FOVx: std_logic_vector(31 downto 0);  -- latched FOVx
  signal FOVy: std_logic_vector(31 downto 0);  -- latched FOVy

  signal xUV: std_logic_vector(31 downto 0);
  signal yUV: std_logic_vector(31 downto 0);

  signal xs: std_logic_vector(31 downto 0);
  signal ys: std_logic_vector(31 downto 0);

  signal xvis, yvis, vis: std_logic;
  signal x: std_logic_vector(7 downto 0);
  signal y: std_logic_vector(7 downto 0);

--  signal signx, signy : std_logic;
--  signal xCoord : std_logic_vector(31 downto 0);  -- 4 bytes
--  signal yCoord : std_logic_vector(31 downto 0);  -- 4 bytes
--  signal zCoord : std_logic_vector(31 downto 0);  -- 1 byte stored
--  signal Xs, Ys : std_logic_vector(15 downto 0); -- interpolated point
  
-- Bubble Place Periscope
--      rhymes with
-- Hubble Space Telescope

  constant ZERO32: std_logic_vector(31 downto 0) := x"00000000";  
  
begin  -- TDP_arch

    xMultLarge <= std_logic_vector(signed(DATA(31 downto 0)) * signed(FOVx));
    yMultLarge <= std_logic_vector(signed(DATA(31 downto 0)) * signed(FOVy));
    
    xMultLargeSigned <= xMultLarge(47 downto 16) when (xMultLarge(63) = '0') else STD_LOGIC_VECTOR(SIGNED(ZERO32) - SIGNED(xMultLarge(47 downto 16)));
    yMultLargeSigned <= yMultLarge(47 downto 16) when (yMultLarge(63) = '0') else STD_LOGIC_VECTOR(SIGNED(ZERO32) - SIGNED(yMultLarge(47 downto 16)));

    DIV0: signed_divide port map (
      DATA(95 downto 64), xMultLargeSigned, xUV);
    DIV1: signed_divide port map (
      DATA(63 downto 32), yMultLargeSigned, yUV);

    xs <= std_logic_vector(SIGNED(xUV) + x"00010000");
    ys <= std_logic_vector(SIGNED(yUV) + x"00010000");

    xvis <= '1' when ((SIGNED(xs) < x"00020000") and (SIGNED(xs) > x"00000000")) else '0';
    yvis <= '1' when ((SIGNED(ys) < x"00020000") and (SIGNED(ys) > x"00000000")) else '0';
    vis <= '1' when ((xvis = '1') and (yvis = '1')) else '0';

    x <= xs(16 downto 9) when (vis = '1') else "00000000";
    y <= ys(16 downto 9) when (vis = '1') else "00000000";

  ClkProcess: process (CLK, nRST)
  begin  -- process ClkProcess
    if nRST = '0' then                  -- asynchronous reset (active low)
      FOVx <= x"00010000";              -- '1' in fixed point
      FOVy <= x"00010000";              -- '1' in fixed point
    elsif CLK'event and CLK = '1' then  -- rising clock edge
      
      if (ENABLE = '1') then
        if (OPCODE = "011") then        -- Set FOVx (first byte of DATA)
          FOVx <= DATA(31 downto 0);     -- to_ufixed(zCoord, 23, -8);
        elsif (OPCODE = "100") then     -- Set FOVy (first byte of DATA)
          FOVy <= DATA(31 downto 0);     --to_ufixed(zCoord, 23, -8);
        end if;
      end if;
      
    end if;
  end process ClkProcess;

  BUSY <= '0'; 
   
  Control: process (CLK, nRST, ENABLE, OPCODE, x, y, DATA)
  begin  -- process Control
    if(nRST = '0') then --lol kk thx ^___^
      RESULT <= x"000000";
    elsif (CLK'event and CLK = '1') then
      if ((ENABLE = '1') and (OPCODE = "001")) then
        RESULT(23 downto 16) <= x;
        RESULT(15 downto 8) <= y;
        RESULT(7 downto 0)   <= DATA(31 downto 24);
      end if;
    end if;
  end process Control;
  
end TDP_arch;
