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 Takt
		clk : in std_logic;
			
		-- Serielle Schnittstelle
		rs232RX : in  std_logic;
		rs232TX : out  std_logic;
	
		-- Kamera Pins
		XCLK : out std_logic; 						-- System Takt
		RSTB : out std_logic := '0'; 				-- Reset (active low)
		PWDN : out std_logic; 						-- Power Down (power down = high)
			
		SCL : out std_logic; 						-- I2C Takt
		SDA1 : inout std_ulogic; 					-- I2C i/o
		SDA2 : inout std_ulogic; 					-- I2C i/o
		
		PCLK1 : in std_logic; 						-- Pixel Takt Cam1
		PCLK2 : in std_logic; 						-- Pixel Takt Cam2
		HREF1 : in std_logic;						-- Horizontales Referenz Signal Cam1
		HREF2 : in std_logic;						-- Horizontales Referenz Signal Cam2
		camVSYNC1 : in std_logic;					-- Vertikales Synchronisationssignal Cam1
		camVSYNC2 : in std_logic;					-- Vertikales Synchronisationssignal Cam2
		D : in std_logic_vector (1 downto 0) := "00";
		D1 : in std_logic_vector(9 downto 0); 	-- Bilddaten Cam1
		D2 : in std_logic_vector(9 downto 0); 	-- Bilddaten Cam2
		
		-- IrLEDs
		irLed : out std_logic_vector (1 downto 0);
		
		-- monitor pins
		r : out std_logic_vector(7 downto 0);	-- Rot-Kanal
		g : out std_logic_vector(7 downto 0);	-- Gruen-Kanal
		b : out std_logic_vector(7 downto 0);	-- Blau-Kanal
		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;						-- Vertikale Synchronisation
		mHSYNC : out std_logic;						-- Horizontale Synchronisation
		
		-- sram
		sr1ce : out std_logic; 						-- chip enable
		sr1ub : out std_logic; 						-- upper byte
		sr1lb : out std_logic; 						-- lower byte
		sr2ce : out std_logic;						-- chip enable
		sr2ub : out std_logic;						-- upper byte
		sr2lb : out std_logic;						-- lower byte
		sr12we : out std_logic := '1';			-- write enable 32Bit Daten-Modus
		sr12oe : out std_logic;						-- output enable 32 Bit Daten-Modus
		sraddr : out std_logic_vector(17 downto 0); 	-- SRAM-Adresse
		srio : inout std_logic_vector(31 downto 0);	-- Daten I/O 32 Bit
		
		-- debug
		led : out std_logic_vector(7 downto 0);	-- achtstellige LED-Anzeige
		sw : in std_logic_vector(7 downto 0);		-- Switches
		btn : in std_logic_vector(3 downto 0);		-- Buttons
		
		sigled : out std_logic_vector(3 downto 0);
		
		seg7 : out std_logic_vector(7 downto 0);
		seg7de : out std_logic_vector(3 downto 0);
		
		DCM_clk : out std_logic;
		VGA_clk : out std_logic

	);
end Kamera;


architecture Behavioral of Kamera is

	-- VGA Output Register
	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;

	-- Taktsignale
	signal dcmLocked : std_logic;
	signal clk05x : std_logic;
	signal clk1x : std_logic;
	signal clk2x : std_logic;
	
	signal clk25: std_logic;
	signal clkXclk: std_logic;
	signal clk2: std_logic_vector(31 downto 0);
	signal clk_controlReg: std_logic;

	-- SRAM ADress-Zeiger
	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";
	signal wScreenPos : std_logic_vector(17 downto 0) := "000000000000000000";
	
	-- Kamera Eingangssignale
	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 SDABuf : std_logic;
	
	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 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);
	
	signal captureReset : std_logic;
	
	-- SRAM <-> Kamera Sample Synchronization
	signal packPos : integer range 0 to 3 := 0;
	signal writeRequest : boolean := false;
	signal writeDiffRequest : boolean := false;
	signal writeScreenRequest : 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;
	
	signal buttonpressed: std_logic;
	
	-- Differenzbild Hilfsfunktionen
	subtype Byte is std_logic_vector(7 downto 0);
	subtype Word is std_logic_vector(15 downto 0);
	
	function twoscompToAbs ( val : Byte ) return Byte is
	begin
		if val(6) = '0' then
			return val;
		else
			return x"ff" - val;
		end if;
	end twoscompToAbs;
	
	-- zur Zeit nicht genutzt
	function twoscompToAbs16 ( val : Word ) return Word is
	begin
		if val(15)='0' then
			return val;
		else
			return x"ffff" - val;
		end if;
	end twoscompToAbs16;
	
	-- Infrarot-Punkt Detektion
	signal curCamPixelPosX: std_logic_vector(15 downto 0) := x"0000";
	signal curCamPixelPosY: std_logic_vector(15 downto 0) := x"0000";

	signal iconPosX: std_logic_vector(15 downto 0) := x"0000";
	signal iconPosY: std_logic_vector(15 downto 0) := x"0000";
	signal icon2PosX: std_logic_vector(15 downto 0) := x"0000";
	signal icon2PosY: std_logic_vector(15 downto 0) := x"0000";
	signal blinking: std_logic := '0'; 												-- Signal blinkendes Icon an/aus
	signal switchLed1: std_logic:= '0';												-- Signal blinkende IrLed1 an/aus
	signal switchLed2: std_logic:= '0';												-- Signal blinkende IrLed2 an/aus
	signal controlReg1: std_logic_vector(7 downto 0):="00000000"; 			-- Kontrollregister, enthaelt Soll-Blinkfrequenz von IrLed1
	signal controlReg2: std_logic_vector(7 downto 0):="00000000"; 			-- Kontrollregister, enthaelt Soll-Blinkfrequenz von IrLed2
	signal controlReg1Buf: EdgeDet.tBuffer := EdgeDet.tBuffer_InitLow; 	-- Buffer fuer Kontroll-Register 1
	signal controlReg2Buf: EdgeDet.tBuffer := EdgeDet.tBuffer_InitLow; 	-- Buffer fuer Kontroll-Register 2
	signal lastPicIrPixelLed1: integer range 0 to 840000 := 0; 				-- Raum, in dem sich letzter Ir-Pixel von Led1 befindet
	signal lastPicIrPixelLed2: integer range 0 to 840000 := 0; 				-- Raum, in dem sich letzter Ir-Pixel von Led2 befindet
	signal hitLed1: std_logic := '0';												-- 1. LED gefunden?
	signal hitLed2: std_logic := '0';												-- 2. LED gefunden?
	signal detectedLeds: std_logic_vector (1 downto 0):= "00";				-- Automat LED-Detektion
	-- LED Positionen
	signal detectedLed1X: std_logic_vector(15 downto 0) := x"0000";
	signal detectedLed1Y: std_logic_vector(15 downto 0) := x"0000";
	signal detectedLed2X: std_logic_vector(15 downto 0) := x"0000";
	signal detectedLed2Y: std_logic_vector(15 downto 0) := x"0000";
	signal curCamPixelPosXLed1: std_logic_vector(15 downto 0) := x"0000"; 
	signal curCamPixelPosYLed1: std_logic_vector(15 downto 0) := x"0000"; 
	signal curCamPixelPosXLed2: std_logic_vector(15 downto 0) := x"0000"; 
	signal curCamPixelPosYLed2: std_logic_vector(15 downto 0) := x"0000";
	-- Gueltigkeitsbereich um LED
	signal areaPixelPosXLed1: std_logic_vector(15 downto 0) := x"0000";
	signal areaPixelPosYLed1: std_logic_vector(15 downto 0) := x"0000";
	signal areaPixelPosXLed2: std_logic_vector(15 downto 0) := x"0000";
	signal areaPixelPosYLed2: std_logic_vector(15 downto 0) := x"0000";
	
	signal startDetection: std_logic_vector(1 downto 0) := "00"; 			-- Schieberegister fuer zwei Pixel nebeneinander gefunden
	signal area1LifeTime: integer range 0 to 100 := 100; 						-- Counter fuer Detektionszonenlebenszeit
	signal area2LifeTime: integer range 0 to 100 := 100; 						-- Counter fuer Detektionszonenlebenszeit
	
		
begin

	-- Feste Ausgangssignale

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

	-- Taktsignale

	clk2x_dcm_inst : entity clk2x_dcm(BEHAVIORAL) port map (
		CLKIN_IN => clk,			-- 50MHz DCM Eingang
		RST_IN => '0',
		CLKDV_OUT => clk05x, 	-- unbenutzt
		CLKIN_IBUFG_OUT => open,
		CLK0_OUT => clk1x, 		-- 50 MHz
		CLK2X_OUT => clk2x, 		-- 100 MHz
		LOCKED_OUT => dcmLocked
	);
	
	-- 7-Segment Display
	seg7_inst : entity Seg7Display(Behavioral) port map (
		Clk => clk1x,
		Seg7 => seg7,
		Seg7DE => seg7de,
		Data => seg7data
	);
	
	
	process(clk1x)
			variable Q : std_logic_vector(31 downto 0) := x"00000000";
	begin
		if clk1x'event and clk1x='1' then
			btnSampleClk <= Q(16);
			
			blinking <= Q(25); 			-- Signal fuer blinkendes "POS"-Icon
			switchLed1 <= Q(23); 		-- Blinkfrequenz ca 3 Hz an Led1
			switchLed2 <= Q(24); 		-- Blinkfrequenz ca 1,5 Hz an Led2
			clk_controlReg <= Q(22); 	-- Takt fuer Kontrollregister-Inkrementierung
			clk25 <= Q(0); 				-- VGA Takt 25 MHz
			clkXclk <= Q(2); 				-- eigener Takt fuer XCLK (6,25 MHz)
			
			-- IrLed an/aus
			irLed(0) <= switchLed1;
			irLed(1) <= switchLed2;

			Q := Q + 1;

			mclk <= clk25;					-- VGA Takt 25MHz
			XCLK <= clkXclk;				-- Kamera System-Takt 6,25MHz

		end if;
	end process;
	
	

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

	-- VGA DAC Signale
	m1 <= '0';
	m2 <= '0';
	nblank <= '1';
	nsync <= '1';
	syncT <= '0';
	mVSYNC <= v_Sync;
	mHSYNC <= h_Sync;
	
	-- Debug VGA
	DCM_clk <= clk2x;
	VGA_clk <= clk25;

	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;
			variable detected2 : boolean := False;
			
			variable foundLedFreq1InReg1 : boolean := False;
			variable foundLedFreq2InReg1 : boolean := False;
			variable foundLedFreq1InReg2 : boolean := False;
			variable foundLedFreq2InReg2 : boolean := False;


	begin
		if clk2x'event and clk2x='1' and dcmLocked='1' then
		
		
			-- ### VGA output ### SRAM Speichern ### SRAM Lesen ### 
			
			case VGAState is
			
				when VGA_Output =>
				
					-- Horizontale Ausgabe
					hCounter<=hCounter +1;
					
					if lastPicIrPixelLed1 > 0 then
						lastPicIrPixelLed1 <= lastPicIrPixelLed1 - 1;
					end if;
					
					if lastPicIrPixelLed2 > 0 then
						lastPicIrPixelLed2 <= lastPicIrPixelLed2 - 1;
					end if;
					
					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:=True;									-- neue Zeile
								 h_Sync<='1';  						-- Zeilen-Sync ein
								 rPos <= curPixelPosY * conv_std_logic_vector(160, 18);
								 
								 -- 640 / 4 = 160 : Werte je 8 Bit, Leseposition springt in Y-Richtung immer
								 -- um 160 Adressen weiter. Vier Datenblcke werden immer zusammen  unter
								 -- einer Adresse gespeichert.
								 
						when others => null;
					end case;
				
					-- Vertikale Ausgabe
					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';
											curPixelPosY <= x"0000";
											rPos <= "000000000000000000";
							when 510 => vBlack<='1';
							when 520 => vCounter<=0;
							when others => null;
						end case;
					end if;

					-- Neue Daten einlesen, ansonsten Werte fuer Differenzbild setzen
					if readRequest then
						readDataTmp := srio;
						readRequest <= false;
					elsif readLastRequest then
						writeDataLastFrame <= srio;
						--writeDataLastFrame <= writeData;
						readLastRequest <= false;
					end if;

					-- Schwarzbereich zeichnen
					
					r <= x"00";
					g <= x"00";
					b <= x"00";

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

						curPixelPosX <= curPixelPosX + x"0001";
						
						-- Bayer-Pattern:
						
						-- wenn:						|	00		01		10		11
						--								------------------------
						-- wenn PosY=0, dann:	|	G		R		G		R
						-- sonst:					|	B		G		B		G

						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);
									-- Gruen-Wert um 1 Bit nach rechts verschoben,
									-- Gruenstich im Bild zu beseitigen
								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;	-- neue Daten aus SRAM lesen
							when others => null;
						end case;
						

						-- zeichne links oben ein rotes Quadrat (Erkennung ob ein Bild angezeigt wird
						if (curPixelPosX < x"000a" and curPixelPosY < x"000a")  -- or detected 
						then
							r <= x"ff";
							g <= x"00";
							b <= x"00";
						end if;
						
						----------------------------------------------------------------------------
						-- ueberpruefen, ob in einem beliebigen Schieberegister Blinkmuster gefunden
						----------------------------------------------------------------------------
						if (foundLedFreq1InReg1 = True) and (foundLedFreq1InReg2 = False) then
							if (curPixelPosX = (detectedLed1X)) and (curPixelPosY = (detectedLed1Y)) then
								detected := True;
							else
								detected := False;
							end if;
						end if;
						
						if (foundLedFreq2InReg1 = True) and (foundLedFreq2InReg2 = False) then
							if (curPixelPosX = (detectedLed1X)) and (curPixelPosY = (detectedLed1Y)) then
								detected2 := True;
							else
								detected2 := False;
							end if;
						end if;
						
						if (foundLedFreq1InReg2 = True) and (foundLedFreq1InReg1 = False) then
							if (curPixelPosX = (detectedLed2X)) and (curPixelPosY = (detectedLed2Y)) then
								detected := True;
							else
								detected := False;
							end if;
						end if;
						
						if (foundLedFreq2InReg2 = True) and (foundLedFreq2InReg1 = False) then
							if (curPixelPosX = (detectedLed2X)) and (curPixelPosY = (detectedLed2Y)) then
								detected2 := True;
							else
								detected2 := False;
							end if;
						end if;
						
						-- zeichne Icon (gruenes Quadrat 10x10) fuer Infrarotpunkt-Detektion: Start
						---------------------------------------------------
						-- Quadrat
						----------
						if sw(2) = '1' and (foundLedFreq1InReg1 or foundLedFreq1InReg2) then
						-- zeichne Positions-Icon nur, wenn korrekte Blinkfrequenz erkannt und Schalter 1 an ist
						
							if detected then
								iconPosX <= curPixelPosX;
								iconPosY <= curPixelPosY;
								g <= x"ff";
							end if;

							-- linke aeussere Kante
							if curPixelPosX = iconPosX then
								if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 10)) then
									g <= x"ff";
								end if;
							end if;
							
							-- Kante oben und unten
							if (curPixelPosX > iconPosX) and (curPixelPosX < iconPosX + 10) then
								if (curPixelPosY = iconPosY) or (curPixelPosY = (iconPosY + 9)) then
									g <= x"ff";
								end if;
							end if;
							
							-- rechte aeussere Kante
							if curPixelPosX = (iconPosX + 9) then
								if (curPixelPosY > (iconPosY)) and (curPixelPosY < (iconPosY + 10)) then
									g <= x"ff";
								end if;
							end if;
							
							--------------------------
							-- zeichne "POS", blinkend
							--------------------------
							if blinking = '1' then
							
								-- zeichne links senkrecht von "P"
								if curPixelPosX = (iconPosX + 16) then
									if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 11)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts senkrecht von "P"
								if curPixelPosX = (iconPosX + 22) then
									if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 6)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne links senkrecht von "O"
								if curPixelPosX = (iconPosX + 25) then
									if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 10)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts seknrecht von "O"
								if curPixelPosX = (iconPosX + 31) then
									if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 10)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne links senkrecht von "S"
								if curPixelPosX = (iconPosX + 36) then
									if (curPixelPosY > iconPosY) and (curPixelPosY < (iconPosY + 6)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts senkrecht von "S"
								
								if curPixelPosX = (iconPosX + 41) then
									if (curPixelPosY > (iconPosY + 5)) and (curPixelPosY < (iconPosY + 10)) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne oben waagerecht von "POS"
								if (curPixelPosX > (iconPosX + 15)) and (curPixelPosX < (iconPosX + 23)) then
									if curPixelPosY = iconPosY then
										g <= x"ff";
									end if;
								elsif (curPixelPosX > (iconPosX + 24)) and (curPixelPosX < (iconPosX + 32)) then
									if curPixelPosY = iconPosY then
										g <= x"ff";
									end if;
								elsif (curPixelPosX > (iconPosX + 35)) and (curPixelPosX < (iconPosX + 42)) then
									if curPixelPosY = iconPosY then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne mittig waagerecht von "POS"
								if (curPixelPosX > (iconPosX + 16)) and (curPixelPosX < (iconPosX + 22)) then
									if curPixelPosY = (iconPosY + 5) then
										g <= x"ff";
									end if;
								elsif (curPixelPosX > (iconPosX + 36)) and (curPixelPosX < (iconPosX + 42)) then
									if curPixelPosY = (iconPosY + 5) then
										g <= x"ff";
									end if;
								end if;
								
								-- zeichne unten waagerecht von "POS"
								if (curPixelPosX > (iconPosX + 24)) and (curPixelPosX < (iconPosX + 32)) then
									if curPixelPosY = (iconPosY + 10) then
										g <= x"ff";
									end if;
								elsif (curPixelPosX > (iconPosX + 35)) and (curPixelPosX < (iconPosX + 42)) then
									if curPixelPosY = (iconPosY + 10) then
										g <= x"ff";
									end if;
								end if;
							end if;
						end if;
						
						-- zeichne Icon fuer Infrarotpunkt-Detektion: Ende
						--------------------------------------------------
						
												
					-- Test-Icon Led2
					-----------------
					
						
						if sw(2) = '1' and (foundLedFreq2InReg1 or foundLedFreq2InReg2) then
						
							if detected2 then
								icon2PosX <= curPixelPosX;
								icon2PosY <= curPixelPosY;
								b <= x"ff";
							end if;
							
							-- linke aeussere Kante
							if curPixelPosX = icon2PosX then
								if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 10)) then
									b <= x"ff";
								end if;
							end if;
							
							-- Kante oben und unten
							if (curPixelPosX > icon2PosX) and (curPixelPosX < icon2PosX + 10) then
								if (curPixelPosY = icon2PosY) or (curPixelPosY = (icon2PosY + 9)) then
									b <= x"ff";
								end if;
							end if;
							
							-- rechte aeussere Kante
							if curPixelPosX = (icon2PosX + 9) then
								if (curPixelPosY > (icon2PosY)) and (curPixelPosY < (icon2PosY + 10)) then
									b <= x"ff";
								end if;
							end if;
							
							--------------------------
							-- zeichne "POS", blinkend
							--------------------------
							if blinking = '1' then
							
								-- zeichne links senkrecht von "P"
								if curPixelPosX = (icon2PosX + 16) then
									if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 11)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts senkrecht von "P"
								if curPixelPosX = (icon2PosX + 22) then
									if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 6)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne links senkrecht von "O"
								if curPixelPosX = (icon2PosX + 25) then
									if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 10)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts seknrecht von "O"
								if curPixelPosX = (icon2PosX + 31) then
									if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 10)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne links senkrecht von "S"
								if curPixelPosX = (icon2PosX + 36) then
									if (curPixelPosY > icon2PosY) and (curPixelPosY < (icon2PosY + 6)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne rechts senkrecht von "S"
								
								if curPixelPosX = (icon2PosX + 41) then
									if (curPixelPosY > (icon2PosY + 5)) and (curPixelPosY < (icon2PosY + 10)) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne oben waagerecht von "POS"
								if (curPixelPosX > (icon2PosX + 15)) and (curPixelPosX < (icon2PosX + 23)) then
									if curPixelPosY = icon2PosY then
										b <= x"ff";
									end if;
								elsif (curPixelPosX > (icon2PosX + 24)) and (curPixelPosX < (icon2PosX + 32)) then
									if curPixelPosY = icon2PosY then
										b <= x"ff";
									end if;
								elsif (curPixelPosX > (icon2PosX + 35)) and (curPixelPosX < (icon2PosX + 42)) then
									if curPixelPosY = icon2PosY then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne mittig waagerecht von "POS"
								if (curPixelPosX > (icon2PosX + 16)) and (curPixelPosX < (icon2PosX + 22)) then
									if curPixelPosY = (icon2PosY + 5) then
										b <= x"ff";
									end if;
								elsif (curPixelPosX > (icon2PosX + 36)) and (curPixelPosX < (icon2PosX + 42)) then
									if curPixelPosY = (icon2PosY + 5) then
										b <= x"ff";
									end if;
								end if;
								
								-- zeichne unten waagerecht von "POS"
								if (curPixelPosX > (icon2PosX + 24)) and (curPixelPosX < (icon2PosX + 32)) then
									if curPixelPosY = (icon2PosY + 10) then
										b <= x"ff";
									end if;
								elsif (curPixelPosX > (icon2PosX + 35)) and (curPixelPosX < (icon2PosX + 42)) then
									if curPixelPosY = (icon2PosY + 10) then
										b <= x"ff";
									end if;
								end if;
							end if;
							
						end if;
						
					-- Test Icon 2 Ende
					-------------------
					
					end if;
					
					-- set address to write
					if writeRequest then
						sraddr <= wPos;
					elsif writeDiffRequest then
						sraddr <= wDiffPos;
					elsif writeScreenRequest then
						sraddr <= wScreenPos;
					end if;
					
					VGAState <= Capture1;
				
				when Capture1 =>
					
					EdgeDet.setNext(btnBuf, sw(0));
			
					-- In den SRAM schreiben? Bild oder Differenzbild
					if writeRequest then
						srio <= writeData; 		-- set data
						sr12we <= '0'; 			-- enable write
						wPos <= wPos + 1; 		-- Schreibposition auf naechste Position setzen
						wDiffPos <= wPos + conv_std_logic_vector(640*480/4+1, 18);
						wScreenPos <= wPos + conv_std_logic_vector(2*640*480/4+1, 18);
						writeRequest <= false;
						--readLastRequest <= true; -- Differenzbild kann bei XCLK = 6,25 MHz nicht berechnet werden!
						writeDiffRequest <= true;
					elsif writeDiffRequest then
						srio <= writeDiffData;
						sr12we <= '0';
						writeDiffRequest <= false;
						if buttonpressed = '1' then
							writeScreenRequest <= true;
						end if;
					elsif writeScreenRequest then
						srio <= writeData;
						sr12we <= '0';
						writeScreenRequest <= false;
					end if;
	
					-- Normalbild anzeigen
					if (sw(1 downto 0)="00") or (sw(1 downto 0)="11")  then
						rPosTmp <= rPos;
					elsif sw(1 downto 0)="01" then
					-- Differenzbild nach Normalbild im Speicher anzeigen
						rPosTmp <= rPos + conv_std_logic_vector(640*480/4+1, 18);
					elsif sw(1 downto 0)="10" then
					-- Screenshot nach Differenzbild im Speicher anzeigen
						rPosTmp <= rPos + conv_std_logic_vector(2*640*480/4+1, 18);
					end if;
					
					VGAState <= Capture2;
				
				when Capture2 =>
					
					-- disable write
					sr12we <= '1';

					if readRequest then
						-- Leseposition setzen
						sraddr <= rPosTmp;
						-- i/o Richtung Lesen
						srio <= SramInpDir;
					elsif readLastRequest then
						sraddr <= wPos;
						srio <= SramInpDir;
					end if;

					VGAState <= Capture3;
				
				when Capture3 =>
					
					-- Status ohne weitere Funktion
				
					VGAState <= VGA_Output;
					
			end case;
			
			-- ************************************************************************************
			
			-- Biffer Signael der LED-Kontrollregister
			
			EdgeDet.setNext(controlReg1Buf, clk_controlReg);
			EdgeDet.setNext(controlReg2Buf, clk_controlReg);
			
			-- Kontrollregister enthaelt Muster fuer Blinkfrequenz
			-- mit 1 inkrementieren, wenn LED1 leuchtet, mit 0 inkrementieren, wenn LED1 aus (im letzten Bild IrPixel)
			if (EdgeDet.risingEdge(controlReg1Buf)) and (lastPicIrPixelLed1 > 0) then
				controlReg1 <= controlReg1 (6 downto 0) & '1';
			elsif (EdgeDet.risingEdge(controlReg1Buf)) then
				controlReg1 <= controlReg1 (6 downto 0) & '0';
			end if;
			
			-- Kontrollregister enthaelt Muster fuer Blinkfrequenz
			-- mit 1 inkrementieren, wenn LED2 leuchtet, mit 0 inkrementieren, wenn LED2 aus (im letzten Bild IrPixel)
			if (EdgeDet.risingEdge(controlReg2Buf)) and (lastPicIrPixelLed2 > 0) then
				controlReg2 <= controlReg2 (6 downto 0) & '1';
			elsif (EdgeDet.risingEdge(controlReg1Buf)) then
				controlReg2 <= controlReg2 (6 downto 0) & '0';
			end if;
			
			-- Abfrage, ob Blinkfrequenz LED1 mit bekanntem Muster uebereinstimmt
			-- liefere True, wenn Uebereinstimmung
			-- liefere False, wenn Frequenz nicht erkannt
			case controlReg1 is
			-- Blinkmuster von Led1 in Register1
				when "01010101" =>
					foundLedFreq1InReg1 := True;
					foundLedFreq2InReg1 := False;
				when "10101010" =>
					foundLedFreq1InReg1 := True;
					foundLedFreq2InReg1 := False;
			-- Blinkmuster von Led2 in Register1
				when "11001100" =>
					foundLedFreq1InReg1 := False;
					foundLedFreq2InReg1 := True;
				when "10011001" =>
					foundLedFreq1InReg1 := False;
					foundLedFreq2InReg1 := True;
				when "00110011" =>
					foundLedFreq1InReg1 := False;
					foundLedFreq2InReg1 := True;
				when "01100110" =>
					foundLedFreq1InReg1 := False;
					foundLedFreq2InReg1 := True;
				when others =>
					foundLedFreq1InReg1 := False;
					foundLedFreq2InReg1 := False;
			end case;
			
			case controlReg2 is
			-- Blinkmuster von Led1 in Register2
				when "01010101" =>
					foundLedFreq1InReg2 := True;
					foundLedFreq2InReg2 := False;
				when "10101010" =>
					foundLedFreq1InReg2 := True;
					foundLedFreq2InReg2 := False;
			-- Blinkmuster von Led2 in Register2
				when "11001100" =>
					foundLedFreq1InReg2 := False;
					foundLedFreq2InReg2 := True;
				when "10011001" =>
					foundLedFreq1InReg2 := False;
					foundLedFreq2InReg2 := True;
				when "00110011" =>
					foundLedFreq1InReg2 := False;
					foundLedFreq2InReg2 := True;
				when "01100110" =>
					foundLedFreq1InReg2 := False;
					foundLedFreq2InReg2 := True;
				when others =>
					foundLedFreq1InReg2 := False;
					foundLedFreq2InReg2 := False;
			end case;
			
			pictCnt <= pictCnt + 1;
			
			if btn(3) = '1' then
				buttonpressed <= '1';
			end if;
			
			-- Neues Bild, Schreibposition zu Beginn immer auf 0
			if EdgeDet.risingEdge(vsyncBuf) then
				isFrame <= False;
				wPos <= "000000000000000000";
				
				-- Reset Led-Stati
				hitLed1 <= '0';
				hitLed2 <= '0';
				
				-- Counter fuer Detektionszone 1 Lebenszeit dekrementieren
				if area1LifeTime > 0 then
					area1LifeTime <= area1LifeTime - 1;
				end if;
				
				-- Counter fuer Detektionszone 2 Lebenszeit dekrementieren
				if area2LifeTime > 0 then
					area2LifeTime <= area2LifeTime - 1;
				end if;

			-- VSYNC aus, Bild beginnt
			elsif EdgeDet.fallingEdge(vsyncBuf) then
				isFrame <= True;
				curCamPixelPosY <= x"0000";  -- Reset Pixelposition Y-Richtung
				
				pictCnt <= x"00000000";
				buttonpressed <= '0';
			end if;
			
			if isFrame then
				
				-- Neue Zeile?
				-- wenn Spalten fertig gelesen, Zeilenzaehler inkrementieren,
				-- Spaltenzaehler auf Startposition
				if EdgeDet.risingEdge(hrefBuf) then
					isLine := True;
				elsif EdgeDet.fallingEdge(hrefBuf) then
					isLine := False;
					curCamPixelPosY <= curCamPixelPosY + 1;
					curCamPixelPosX <= x"0000";
				end if;
				
				-- eingelesene Daten speichern
				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;
							
							-- Differnezbild berechnen
							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;
					
					-- wenn schwarz (keine Ledposition)
					if dinBufPrev < x"bc" then 
					
						-- Schieberegister fuer gefundenen Pixel, bei < Schwellwert dekrementieren
						startDetection <= startDetection(0)& '0';
					
					end if;
						
					-- ---------------------------------------------------------------------
					-- Detektionsautomat, der zwei LEDs sucht
					-- ---------------------------------------------------------------------
					
					-- Schwellwert, ab dem detektiert wird
					if dinBufPrev > x"bb" then
						
						-- Schieberegister fuer gefundenen Pixel, bei > Schwellwert inkrementieren
						startDetection <= startDetection(0)& '1';
						
						-- Detektion nur starten, wenn zwei Pixel nebeneinander gefunden wurden
						if startDetection = "11" then					
							
							case detectedLeds is
							
								when "00" => -- keine Led gefunden, 1. LED im Bild erwartet

									-- Detektionsautomaten in den Startzustand zurueckversetzen, wenn eine der beiden Leds nicht mehr gefunden
						
									-- 1. LED zuruecksetzen
									if (area1LifeTime = 0) then
										curCamPixelPosXLed1 <= x"0000";
										curCamPixelPosYLed1 <= x"0000";
										area1LifeTime <= 10;
									end if;
							
									if (hitLed1 = '0') and (curCamPixelPosXLed1 = x"0000") and (curCamPixelPosYLed1 = x"0000") then
										detectedLed1X <= curCamPixelPosX; 			-- Position X Icondarstellung
										detectedLed1Y <= curCamPixelPosY; 			-- Position Y Icondarstellung
										curCamPixelPosXLed1 <= curCamPixelPosX; 	-- Position X zwischenspeichern
										curCamPixelPosYLed1 <= curCamPixelPosY; 	-- Position Y zwischenspeichern
										hitLed1 <= '1'; 									-- 1. LED gefunden
										areaPixelPosXLed1 <= curCamPixelPosX; 		-- Position X zwischenspeichern
										areaPixelPosYLed1 <= curCamPixelPosY; 		-- Position X zwischenspeichern
										lastPicIrPixelLed1 <= 840000; 				-- 800*520 = 416000 (hcounter * vcounter) und im naechsten Bild, also *2
										area1LifeTime <= 10;								-- Zone um 1. LED gueltig
										
										-- Led1 nur in bestimmter Umgebung akzeptieren (20 x 20) Pixel
									elsif (hitLed1 = '0') and (((curCamPixelPosX > (areaPixelPosXLed1 - 10)) and (curCamPixelPosX < (areaPixelPosXLed1 + 10))) and ((curCamPixelPosY > (areaPixelPosYLed1 - 10)) and (curCamPixelPosY < (areaPixelPosYLed1 + 10))))then
										detectedLed1X <= curCamPixelPosX; 			-- Position X Icondarstellung
										detectedLed1Y <= curCamPixelPosY; 			-- Position Y Icondarstellung
										curCamPixelPosXLed1 <= curCamPixelPosX; 	-- Position X zwischenspeichern
										curCamPixelPosYLed1 <= curCamPixelPosY; 	-- Position Y zwischenspeichern
										hitLed1 <= '1'; 									-- 1. LED gefunden
										areaPixelPosXLed1 <= curCamPixelPosX; 		-- Position X zwischenspeichern
										areaPixelPosYLed1 <= curCamPixelPosY; 		-- Position X zwischenspeichern
										lastPicIrPixelLed1 <= 840000; 				-- 800*520 = 416000 (hcounter * vcounter) und im naechsten Bild, also *2
										area1LifeTime <= 10;								-- Zone um 1. LED gueltig
									end if;
									detectedLeds <= "01";
									
								when "01" => -- 2. LED noch nicht gefunden, 2. LED erwartet, naechsten Pixel ausserhalb des Bereichs von Led1 finden 

									-- 2. LED zuruecksetzen
									if (area2LifeTime = 0) then
										curCamPixelPosXLed2 <= x"0000";
										curCamPixelPosYLed2 <= x"0000";
										area2LifeTime <= 10;
									end if;
									
									if (hitLed2 = '0') and (curCamPixelPosXLed2 = x"0000") and (curCamPixelPosYLed2 = x"0000") and  ((curCamPixelPosX > curCamPixelPosXLed1 + 10) or (curCamPixelPosX < curCamPixelPosXLed1 - 10) or (curCamPixelPosY > curCamPixelPosYLed1 + 10) or (curCamPixelPosY < curCamPixelPosYLed1 - 10))then 
										detectedLed2X <= curCamPixelPosX; 			-- Position X Icondarstellung
										detectedLed2Y <= curCamPixelPosY; 			-- Position Y Icondarstellung
										curCamPixelPosXLed2 <= curCamPixelPosX; 	-- Position X zwischenspeichern
										curCamPixelPosYLed2 <= curCamPixelPosY; 	-- Position X zwischenspeichern
										hitLed2 <= '1'; 									-- 2. LED gefunden
										areaPixelPosXLed2 <= curCamPixelPosX; 		-- Position X zwischenspeichern
										areaPixelPosYLed2 <= curCamPixelPosY; 		-- Position X zwischenspeichern
										area2LifeTime <= 10;								-- Zone um 2. LED gueltig
										lastPicIrPixelLed2 <= 840000; 				-- 800*520 = 416000 (hcounter * vcounter) und im naechsten Bild, also *2
										
										-- Led2 nur in bestimmter Umgebung akzeptieren (20 x 20) Pixel
									elsif (hitLed2 = '0') and  ((curCamPixelPosX > curCamPixelPosXLed1 + 10) or (curCamPixelPosX < curCamPixelPosXLed1 - 10) or (curCamPixelPosY > curCamPixelPosYLed1 + 10) or (curCamPixelPosY < curCamPixelPosYLed1 - 10)) and (((curCamPixelPosX > (areaPixelPosXLed2 - 10)) and (curCamPixelPosX < (areaPixelPosXLed2 + 10))) and ((curCamPixelPosY > (areaPixelPosYLed2 - 10)) and (curCamPixelPosY < (areaPixelPosYLed2 + 10))))then 
										detectedLed2X <= curCamPixelPosX; 			-- Position X Icondarstellung
										detectedLed2Y <= curCamPixelPosY; 			-- Position Y Icondarstellung
										curCamPixelPosXLed2 <= curCamPixelPosX; 	-- Position X zwischenspeichern
										curCamPixelPosYLed2 <= curCamPixelPosY; 	-- Position Y zwischenspeichern
										hitLed2 <= '1'; 									-- 2. LED gefunden
										areaPixelPosXLed2 <= curCamPixelPosX; 		-- Position X zwischenspeichern
										areaPixelPosYLed2 <= curCamPixelPosY; 		-- Position Y zwischenspeichern
										area2LifeTime <= 10;								-- Zone um 2. LED gueltig
										lastPicIrPixelLed2 <= 840000; 				-- 800*520 = 416000 (hcounter * vcounter) und im naechsten Bild, also *2
									end if;
									detectedLeds <= "11";

										
								when others =>
											detectedLeds <= "00";
									
							end case;
							
						end if;
					end if;
					
					curCamPixelPosX <= curCamPixelPosX + 1;
					
				end if;
			end if;
			
			-- Buffer Signale Cam1 oder Cam2
			dinBufPrev <= dinBuf;
			case sw(6) is
				when '0' =>
					dinBuf <= D1(9 downto 2);
					EdgeDet.setNext(pclkBuf, PCLK1);
					EdgeDet.setNext(hrefBuf, HREF1);
					EdgeDet.setNext(vsyncBuf, camVSYNC1);
				when others =>
					dinBuf <= D2(9 downto 2);
					EdgeDet.setNext(pclkBuf, PCLK2);
					EdgeDet.setNext(hrefBuf, HREF2);
					EdgeDet.setNext(vsyncBuf, camVSYNC2);
			end case;
			
			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 => SDABuf,
		reset => captureReset
	);
	
	-- sende RS232-daten an beide Kameras
	SDA1 <= SDABuf;
	SDA2 <= SDABuf;
	
end Behavioral;