library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

use work.CamControl;
use work.EdgeDet;
use work.MemPack;

entity Kamera is
	Port ( 
		-- fpga clock
		clk : in std_logic;
			
		-- serial port
		rs232RX : in  std_logic;
		rs232TX : out  std_logic;
	
		-- camera pins
		XCLK : out std_logic; -- system clock
		RSTB : out std_logic := '0'; -- reset (active low)
		PWDN : out std_logic; -- power down (power down = high)
			
		SCL : out std_logic; -- I2C clock
		SDA : inout std_ulogic; -- I2C i/o
			
		PCLK : in std_logic; -- pixel clock
		HREF : in std_logic;
		VSYNC : in std_logic;
		D : in std_logic_vector(9 downto 0); -- pic data
		
		-- monitor pins
		r : out std_logic_vector(7 downto 0);
		g : out std_logic_vector(7 downto 0);
		b : out std_logic_vector(7 downto 0);
		m1 : out std_logic;
		m2 : out std_logic;
		mclk : out std_logic;
		syncT : out std_logic;
		nsync : out std_logic;
		nblank : out std_logic;
		mVSYNC : out std_logic;
		mHSYNC : out std_logic;
		
		-- sram
		sr1ce : out std_logic;
		sr1ub : out std_logic;
		sr1lb : out std_logic;
		sr2ce : out std_logic;
		sr2ub : out std_logic;
		sr2lb : out std_logic;
		sr12we : out std_logic := '1';
		sr12oe : out std_logic;
		sraddr : out std_logic_vector(17 downto 0);
		srio : inout std_logic_vector(31 downto 0);
		
		-- debug
		led : out std_logic_vector(7 downto 0);
		sw : in std_logic_vector(7 downto 0);
		btn : in std_logic_vector(3 downto 0);
		
		sigled : out std_logic_vector(3 downto 0);
		
		seg7 : out std_logic_vector(7 downto 0);
		seg7de : out std_logic_vector(3 downto 0)
	);
end Kamera;


architecture Behavioral of Kamera is

	-- VGA output registers
	signal curPixelPosX: std_logic_vector(15 downto 0) := x"0000";
	signal curPixelPosY: std_logic_vector(15 downto 0) := x"0000";
	
	signal h_Sync: std_logic;
	signal v_Sync: std_logic;
	
	signal hCounter: integer range 0 to 799:=0;
	signal vCounter: integer range 0 to 520:=0;
	signal hBlack: std_logic:='1';
	signal vBlack: std_logic:='1';

	type tVGAState is (VGA_Output, Capture1, Capture2, Capture3);
	signal VGAState : tVGAState := VGA_Output;

	-- clock
	signal dcmLocked : std_logic;
	signal clk05x : std_logic;
	signal clk1x : std_logic;
	signal clk2x : std_logic;

	-- SRAM address pointers
	constant SramInpDir : std_logic_vector(31 downto 0) := "ZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZZ";

	signal rPos : std_logic_vector(17 downto 0) := "000000000000000000";
	signal rPosTmp : std_logic_vector(17 downto 0) := "000000000000000000";
	
	signal wPos : std_logic_vector(17 downto 0) := "000000000000000000";
	signal wDiffPos : std_logic_vector(17 downto 0) := "000000000000000000";
	
	-- camera input sampling
	signal dinBuf : std_logic_vector(7 downto 0);
	signal dinBufPrev : std_logic_vector(7 downto 0);
	signal hrefBuf : EdgeDet.tBuffer := EdgeDet.tBuffer_InitLow;
	signal vsyncBuf : EdgeDet.tBuffer := EdgeDet.tBuffer_InitHigh;
	signal pclkBuf : EdgeDet.tBuffer := EdgeDet.tBuffer_InitHigh;
	
	signal isFrame : boolean := False;
	
	-- debug
	signal btnBuf : EdgeDet.tBuffer := EdgeDet.tBuffer_InitLow;
	signal ledBuf : std_logic_vector(7 downto 0);
	signal sigledBuf : std_logic_vector(3 downto 0);
	
	signal captureReset : std_logic;
	
	-- SRAM <-> Camera sample synchronization
	signal packPos : integer range 0 to 3 := 0;
	signal writeRequest : boolean := false;
	signal writeDiffRequest : boolean := false;
	signal writeData : std_logic_vector(31 downto 0);
	signal writeDiffData : std_logic_vector(31 downto 0);
	
	signal writeDataLastFrame : std_logic_vector(31 downto 0);
	
	signal readRequest : boolean := false;
	signal readLastRequest : boolean := false;
	
	-- diff image helper function
	subtype Byte is std_logic_vector(7 downto 0);
	
	function twoscompToAbs ( val : Byte ) return Byte is
	begin
		if val(7)='0' then
			return val;
		else
			return x"ff" - val;
		end if;
	end twoscompToAbs;
	
	-- blinking led phase config
	signal pictCnt : std_logic_vector(31 downto 0);
	signal pictRef : std_logic_vector(31 downto 0) := x"00000000";
	signal btnSampleClk : std_logic;
	signal seg7data : std_logic_vector(15 downto 0);
	
	-- camera point detection
	signal curCamPixelPosX: std_logic_vector(15 downto 0) := x"0000";
	signal curCamPixelPosY: std_logic_vector(15 downto 0) := x"0000";

	signal curCamDetPPX: std_logic_vector(15 downto 0) := x"0000";
	signal curCamDetPPY: std_logic_vector(15 downto 0) := x"0000";
	signal curCamDetStrength : std_logic_vector(7 downto 0) := x"00";
	signal curIsSig : boolean := False;
	
	signal pipeRequest : boolean := False;
	signal pipePos : integer range 0 to 4 := 0;
	
	type tPipeData is record
		req : boolean;
		inpVal : std_logic_vector(7 downto 0);
	end record;
	
	constant tPipeData_Init : tPipeData := (
		req => false,
		inpVal => x"00"
	);
	
	signal pipeInp : tPipeData := tPipeData_Init;
	
begin

	-- fixed outputs

	led <= ledBuf;
	
	sr1ce <= '0'; sr2ce <= '0';
	sr1ub <= '0'; sr2ub <= '0';
	sr1lb <= '0'; sr2lb <= '0';
	sr12oe <= '0';
	
	sigled <= sigledBuf;

	-- clocks

	clk2x_dcm_inst : entity clk2x_dcm(BEHAVIORAL) port map (
		CLKIN_IN => clk,
		RST_IN => '0',
		CLKDV_OUT => clk05x,
		CLKIN_IBUFG_OUT => open,
		CLK0_OUT => clk1x,
		CLK2X_OUT => clk2x,
		LOCKED_OUT => dcmLocked
	);
	
	-- seg7 display
	seg7_inst : entity Seg7Display(Behavioral) port map (
		Clk => clk1x,
		Seg7 => seg7,
		Seg7DE => seg7de,
		Data => seg7data
	);
	
	--seg7data <= curCamDetPPX(7 downto 0) & curCamDetPPY(7 downto 0); -- prev: pictRef(23 downto 8)
	
	process(clk1x)
		variable Q : std_logic_vector(31 downto 0) := x"00000000";
	begin
		if clk1x'event and clk1x='1' then
			btnSampleClk <= Q(16);
			Q := Q + 1;
		end if;
	end process;
	
	process(btnSampleClk)
	begin
		if btnSampleClk'event and btnSampleClk='1' then
			if btn(0)='1' then
				pictRef <= pictRef + x"100";
			elsif btn(1)='1' then
				pictRef <= pictRef - x"100";
			end if;
		end if;
	end process;

	-- ###########################################
	-- ### monitor output
	-- ###########################################

	-- VGA DAC control
	m1 <= '0';
	m2 <= '0';
	nblank <= '1';
	nsync <= '1';
	syncT <= '0';
	mVSYNC <= v_Sync;
	mHSYNC <= h_Sync;
	mclk <= not clk05x; -- TODO: synchronize


	process(clk2x)
			variable newline : boolean;
			
			variable tmpByte : std_logic_vector(7 downto 0);
			
			variable isLine : boolean := False;
			variable writeDataTmp : std_logic_vector(31 downto 0);
			variable readDataTmp : std_logic_vector(31 downto 0);

			variable detected : boolean := False;
	begin
		if clk2x'event and clk2x='1' and dcmLocked='1' then

			-- ### VGA output ###
			
			case VGAState is
			
				when VGA_Output =>

					-- horizontal control
					hCounter<=hCounter +1;
					newline:=False;
					case hCounter is
						when  95 => h_Sync<='0';  -- Zeilen-Sync aus
										curPixelPosX <= x"0000";
						when 143 => hBlack<='0'; -- schwarzer Rand aus
						when 783 => hBlack<='1'; -- schwarzer Rand ein
						when 799 => hCounter<=0; -- newline
								 newline:=True;
								 h_Sync<='1';  -- Zeilen-Sync ein
								 rPos <= curPixelPosY * conv_std_logic_vector(160, 18);
						when others => null;
					end case;
				
					-- vertical control
					if newline then
						vCounter<=vCounter+1;
						curPixelPosY <= curPixelPosY + x"0001";
						case	vCounter is
							when   0 => v_Sync<='1';
							when   2 => v_Sync<='0';
							when  33 => vBlack<='0'; -- was: 30 (!!!)
											curPixelPosY <= x"0000";
											rPos <= "000000000000000000";
							when 512 => vBlack<='1'; -- was: 511 (!!!)
							when 520 => vCounter<=0;
											ledBuf(5) <= not ledBuf(5);
							when others => null;
						end case;
					end if;

					-- read new data
					if readRequest then
						readDataTmp := srio;
						readRequest <= false;
					elsif readLastRequest then
						writeDataLastFrame <= srio;
						readLastRequest <= false;
					end if;

					-- draw contents
					r <= x"00";
					g <= x"00";
					b <= x"00";

					if hBlack='0' and vBlack='0' then

						curPixelPosX <= curPixelPosX + x"0001";

						case curPixelPosX(1 downto 0) is
							when "00" =>
								tmpByte := readDataTmp(7 downto 0);
								if curPixelPosY(0) = '0' then -- even line
									g <= '0' & tmpByte(7 downto 1);
								else -- odd line
									b <= tmpByte;
								end if;
							when "01" =>
								tmpByte := readDataTmp(15 downto 8);
								if curPixelPosY(0) = '0' then
									r <= tmpByte;
								else
									g <= '0' & tmpByte(7 downto 1);
								end if;
							when "10" =>
								tmpByte := readDataTmp(23 downto 16);
								if curPixelPosY(0) = '0' then
									g <= '0' & tmpByte(7 downto 1);
								else
									b <= tmpByte;
								end if;
							when "11" =>
								tmpByte := readDataTmp(31 downto 24);
								if curPixelPosY(0) = '0' then
									r <= tmpByte;
								else
									g <= '0' & tmpByte(7 downto 1);
								end if;
								rPos <= rPos + 1;
								readRequest <= true;
							when others => null;
						end case;
						
						-- draw a small red rectangle in the upper left corner
						if (curPixelPosX < x"000a" and curPixelPosY < x"000a") or detected then
							r <= x"ff";
							g <= x"00";
							b <= x"00";
						end if;
						
						if (curPixelPosX = (curCamDetPPX - 10)) and (curPixelPosY = (curCamDetPPY - 10)) then
							detected := True;
						else
							detected := False;
						end if;
						
					end if;
					
					-- set address to write
					if writeRequest then
						sraddr <= wPos;
					elsif writeDiffRequest then
						sraddr <= wDiffPos;
					end if;
					
					VGAState <= Capture1;
				
				when Capture1 =>
					
					EdgeDet.setNext(btnBuf, sw(0));

					ledBuf(6) <= '0'; -- 1 when waiting for line
					ledBuf(7) <= '0'; -- 1 when waiting for frame
					
					-- write to SRAM ?
					if writeRequest then
						srio <= writeData; -- set data
						sr12we <= '0'; -- enable write
						wPos <= wPos + 1; -- increment write pos for next write
						wDiffPos <= wPos + conv_std_logic_vector(640*480/4+1, 18);
						writeRequest <= false;
						readLastRequest <= true;
						writeDiffRequest <= true;
					elsif writeDiffRequest then
						srio <= writeDiffData;
						sr12we <= '0';
						writeDiffRequest <= false;
					end if;
	
					
					if sw(0)='0' then
						rPosTmp <= rPos;
					else
						rPosTmp <= rPos + conv_std_logic_vector(640*480/4+1, 18);
					end if;
	
					
					VGAState <= Capture2;
				
				when Capture2 =>
					-- disable write
					sr12we <= '1';

					if readRequest then
						-- output address to read for VGA output:
						-- 10 bit for column, 8 bit for line
						sraddr <= rPosTmp;
						-- i/o input direction
						srio <= SramInpDir;
					elsif readLastRequest then
						sraddr <= wPos;
						srio <= SramInpDir;
					end if;
					
					VGAState <= Capture3;
				
				when Capture3 =>
				
					VGAState <= VGA_Output;
					
			end case;
			
			-- ************************************************************************************+
			
			pictCnt <= pictCnt + 1;
			
			-- frame start/end ?
			if EdgeDet.risingEdge(vsyncBuf) then
				isFrame <= False;
				wPos <= "000000000000000000";
			elsif EdgeDet.fallingEdge(vsyncBuf) then
				isFrame <= True;
				curIsSig <= False;
				curCamPixelPosY <= x"0000";
				
				pictCnt <= x"00000000";
			end if;
			
			if isFrame then
				
				-- line start/end ?
				if EdgeDet.risingEdge(hrefBuf) then
					isLine := True;
				elsif EdgeDet.fallingEdge(hrefBuf) then
					isLine := False;
					curCamPixelPosY <= curCamPixelPosY + 1;
					curCamPixelPosX <= x"0000";
				end if;
				
				-- store data
				if isLine and EdgeDet.risingEdge(pclkBuf) then
				
					case packPos is
						when 0 =>
							writeDataTmp(7 downto 0) := dinBufPrev;
						when 1 =>
							writeDataTmp(15 downto 8) := dinBufPrev;
						when 2 =>
							writeDataTmp(23 downto 16) := dinBufPrev;
						when 3 =>
							writeDataTmp(31 downto 24) := dinBufPrev;
							
							writeData <= writeDataTmp;
							writeDiffData <= 
								twoscompToAbs(writeDataTmp(31 downto 24) - writeDataLastFrame(31 downto 24)) &
								twoscompToAbs(writeDataTmp(23 downto 16) - writeDataLastFrame(23 downto 16)) &
								twoscompToAbs(writeDataTmp(15 downto 8) - writeDataLastFrame(15 downto 8)) &
								twoscompToAbs(writeDataTmp(7 downto 0) - writeDataLastFrame(7 downto 0));
							
							writeRequest <= True;
							
						when others => null;
					end case;
					packPos <= packPos + 1;
					
					-- detect bright point
					if dinBufPrev > x"80" then
						pipeInp.req <= true;
						pipeInp.inpVal <= dinBufPrev;
						pipeInp.posX <= curCamPixelPosX;
						pipeInp.posY <= curCamPixelPosY;
					
						if not curIsSig then
							curCamDetPPX <= curCamPixelPosX;
							curCamDetPPY <= curCamPixelPosY;
							curIsSig <= True;
							curCamDetStrength <= x"01";
						else
							if ((curCamPixelPosX - curCamDetPPX) < x"0010") and ((curCamPixelPosY - curCamDetPPY) < x"0010") then
								curCamDetStrength <= curCamDetStrength + 1;
							elsif curCamDetStrength < x"03" then
								curCamDetPPX <= curCamPixelPosX;
								curCamDetPPY <= curCamPixelPosY;
								curCamDetStrength <= x"01";
							end if;
						end if;
					else
						--seg7data <= x"00" & curCamDetPPX(7 downto 0); -- & curCamDetPPY(7 downto 0);
					end if;
					
					curCamPixelPosX <= curCamPixelPosX + 1;
					
				end if;
			end if;
			
			-- buffer signals
			dinBuf <= D(9 downto 2);
			dinBufPrev <= dinBuf;
			EdgeDet.setNext(pclkBuf, PCLK);
			EdgeDet.setNext(hrefBuf, HREF);
			EdgeDet.setNext(vsyncBuf, VSYNC);
			
			
			-- ************************************************************************************+

			if pipeInt2.req then
				if curCamDetStrength < x"03" then
					curCamDetPPX <= pipeInt2.posX;
					curCamDetPPY <= pipeInt2.posY;
					curCamDetStrength <= x"01";
				end if;
			
				pipeInt2.req <= false;
			end if;

			if pipeInt1.req then
				if ((pipeInt1.posX - curCamDetPPX) < x"0010") and ((curCamPixelPosY - curCamDetPPY) < x"0010") then
					curCamDetStrength <= curCamDetStrength + 1;
				else
					pipeInt2 <= pipeInt1;
				end if;
			
				pipeInt1.req <= false;
			end if;

			if pipeInp.req then
				if not curIsSig then
					curCamDetPPX <= pipeInp.posX;
					curCamDetPPY <= pipeInp.posY;
					curIsSig <= true;
					curCamDetStrength <= x"01";
				else
					pipeInt1 <= pipeInp; -- next pipeline step
				end if;
			
				pipeInp.req <= false;
			end if;
			
			
			
			
			-- ************************************************************************************+
			
			if pictCnt = pictRef then
				sigledBuf <= not sigledBuf;
			end if;
			
			
		end if;
	end process;



	-- ###########################################
	-- ### SCCB bus / camera control
	-- ###########################################
	
	CamControl_Inst : entity CamControl(arch) port map (
		clk => clk1x,
		rs232RX => rs232RX,
		rs232TX => rs232TX,
		RSTB => RSTB,
		PWDN => PWDN,
		SCL => SCL,
		SDA => SDA,
		reset => captureReset
	);
	
	XCLK <= clk05x;
	

	
end Behavioral;