----------------------------------------------------------------------------------
-- Company: 
-- Engineer:a.Kurka 
-- 
-- Create Date:   06.06.2021 by a.kurka
-- Design Name:  AS21
-- Module Name:    atanFP - Behavioral 
-- modified : 15.06.2021 by a.kurka 
--Berechnet arctang aus Xin / Yin für Bereich -PI .... PI
--    output Winkel in rad FP Format, benötigt 20 clk
--    dauer 22 clk
----------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;


entity atanFP is
	port(	nrst			: in std_logic;
			clk 			: in std_logic;
			ceatan     : in std_logic;-- startimpuls afkt
			Xinp			:in std_logic_vector(31 downto 0);-- input X FP FP  format
			Yinp			:in std_logic_vector(31 downto 0);-- input Y FP FP  format
			atanFP		:OUT std_logic_vector(31 downto 0):=X"00000000"-- output Winkel(rad) in FP  format
			);
end atanFP;

architecture Behavioral of atanFP is
--=============================================
COMPONENT afkt is
	generic(	P0:std_logic_vector(31 downto 0):=X"00000000";
				P1:std_logic_vector(31 downto 0):=X"00000000";
				P2:std_logic_vector(31 downto 0):=X"00000000";
				Q0:std_logic_vector(31 downto 0):=X"00000000";
				Q1:std_logic_vector(31 downto 0):=X"00000000";
				Q2:std_logic_vector(31 downto 0):=X"00000000"
				);
	port(	nrst			: in std_logic;
			clk 			: in std_logic;
			ceafkt     : in std_logic;-- startimpuls afkt
			Xinp			:in std_logic_vector(31 downto 0);--input afkt FP  format, immer positiv
			aout		:OUT std_logic_vector(31 downto 0)-- output afkt FP  format
			);
END COMPONENT;
------------------------------------------------------------
COMPONENT mulFP IS
	port (
	a: IN std_logic_VECTOR(31 downto 0);
	b: IN std_logic_VECTOR(31 downto 0);
	clk: IN std_logic;
	ce: IN std_logic;
	result: OUT std_logic_VECTOR(31 downto 0));
END COMPONENT;
------------------------------------------------------------
COMPONENT divFP IS
	port (
	a: IN std_logic_VECTOR(31 downto 0);
	b: IN std_logic_VECTOR(31 downto 0);
	clk: IN std_logic;
	ce: IN std_logic;
	result: OUT std_logic_VECTOR(31 downto 0)
	);
END COMPONENT;

------------------------------------
----if a >= b then result = 1
--COMPONENT compFP IS
--	port (
--	a: IN std_logic_VECTOR(31 downto 0);
--	b: IN std_logic_VECTOR(31 downto 0);
--	clk: IN std_logic;
--	ce: IN std_logic;
--	result: OUT std_logic_VECTOR(0 downto 0));
--END COMPONENT;
-----------------------------------------------
-- if a > b then result = "0001"
COMPONENT cmpgr IS
  PORT (
    a : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
    b : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
    clk : IN STD_LOGIC;
    ce : IN STD_LOGIC;
    result : OUT STD_LOGIC_VECTOR(0 DOWNTO 0)
  );
END COMPONENT;-- cmpgr;
-----------------------------------------
COMPONENT subFP IS
	port (
	a: IN std_logic_VECTOR(31 downto 0);
	b: IN std_logic_VECTOR(31 downto 0);
	clk: IN std_logic;
	ce: IN std_logic;
	result: OUT std_logic_VECTOR(31 downto 0));
END COMPONENT;
---------------------------------------------
COMPONENT addFP IS
	port (
	a: IN std_logic_VECTOR(31 downto 0);
	b: IN std_logic_VECTOR(31 downto 0);
	clk: IN std_logic;
	ce: IN std_logic;
	result: OUT std_logic_VECTOR(31 downto 0));
END COMPONENT;
--=======================================================
--==================== Signale für afkt ==================================

CONSTANT PIdiv2 	:std_logic_vector(31 DOWNTO 0):=X"3FC90FDB";--1.57079632679 PI/2=+90 deg
CONSTANT PIndiv2 	:std_logic_vector(31 DOWNTO 0):=X"BFC90FDB";--1.57079632679 -PI/2=-90 deg
CONSTANT PI 		:std_logic_vector(31 DOWNTO 0):=X"40490FDB";-- +3.14159265359-- PI=180 deg
CONSTANT PIn 		:std_logic_vector(31 DOWNTO 0):=X"C0490FDB";--  -3.14159265359-- PI=-180 deg
CONSTANT PIn135 		:std_logic_vector(31 DOWNTO 0):=X"C016CBE4";--  -2.35619449019-- -135 deg
CONSTANT PI135 		:std_logic_vector(31 DOWNTO 0):=X"4016CBE4";--  +2.35619449019-- +135 deg
-----------------------------------------------
SIGNAL stateatan	:integer range 0 to 15:= 0;
--SIGNAL ceatan extern
SIGNAL cecomp	 	:std_logic:= '0';
SIGNAL fYgr	 		:std_logic_vector(0 DOWNTO 0):=(OTHERS => '0');--if Y(a)>X(b) then := '1'
SIGNAL Yneg		:std_logic:='0';-- flag Y negativ  = 1
SIGNAL Xneg		:std_logic:='0';-- flag X negativ  = 1
SIGNAL Yabs		:std_logic_vector(31 DOWNTO 0):=X"00000000";-- Abs Input Yinp
SIGNAL Xabs		:std_logic_vector(31 DOWNTO 0):=X"00000000";-- Abs Input Xinp
SIGNAL adiv  	:std_logic_VECTOR(31 downto 0):=X"00000000";
SIGNAL bdiv  	:std_logic_VECTOR(31 downto 0):=X"00000000";
SIGNAL ainp		:std_logic_VECTOR(31 downto 0):=X"00000000";-- inputs für Addition/subtraktion
SIGNAL binp		:std_logic_VECTOR(31 downto 0):=X"00000000";--inputs für Addition/subtraktion
SIGNAL cediv	 :std_logic:= '0';
signal resdiv	:std_logic_VECTOR(31 downto 0):=X"00000000";-- resutat a/b
SIGNAL ce1			:std_logic:='0';-- Start resdiv^2
--
SIGNAL ain  	:std_logic_VECTOR(31 downto 0):=X"00000000";-- resultat resdiv^2= input afkt
SIGNAL ce2	 	:std_logic:= '0'; -- start afkt
SIGNAL aou  	:std_logic_VECTOR(31 downto 0):=X"00000000";--resultaat afkt(resdiv^2)
---
SIGNAL ce3	 		:std_logic:= '0';-- start atang = aou* ainp
SIGNAL atang   	:std_logic_VECTOR(31 downto 0):=X"00000000";--resultat  atang[-1,1]
-----------
SIGNAL ceadd	 	:std_logic:= '0';	-- start add
SIGNAL cesub	 	:std_logic:= '0';	-- start sub
SIGNAL atanFPa		:std_logic_VECTOR(31 downto 0):=X"00000000";-- resultat nach addition
SIGNAL atanFPs		:std_logic_VECTOR(31 downto 0):=X"00000000";-- resultat nach substraction


-- atanFP resultat ----
--==================================================================
BEGIN
--------------------------------------------------------------
patan :PROCESS (clk)
VARIABLE cntrclk : INTEGER RANGE 0 TO 127:= 0;
--variable state :INTEGER RANGE 0 TO 15:= 0;
variable sign :std_logic_vector(1 DOWNTO 0):= "00";--Yneg(1),Xneg(0)
BEGIN
	IF rising_edge(clk) THEN
		IF nrst = '0' THEN
			stateatan <= 0;
			cntrclk := 0;
			cecomp <= '0';
			sign := "00";
			--(OTHERS => '0');
		ELSE
			CASE stateatan IS
				WHEN 0 =>
					IF ceatan = '1' THEN
						IF (Yinp = X"00000000") AND (Xinp = X"00000000")  THEN
							atanFP <= X"00000000";
							stateatan <= 0;-- ungültige wert
						ELSE
							stateatan <= 1;
						END IF;
					ELSE
						stateatan <= 0;
					END IF;
				WHEN 1 => ---testen Yinp
						IF Yinp(31) = '1' THEN -- if negativ
							Yabs <= Yinp AND X"7FFFFFFF";
							sign(1) := '1';-- Y negativ
						ELSE
							Yabs <= Yinp;
							sign(1) := '0';-- Y positiv
						END IF;
						IF Xinp(31) = '1' THEN -- if negativ
							Xabs <= Xinp AND X"7FFFFFFF";
							sign(0) := '1'; -- X negativ
						ELSE
							Xabs <= Xinp;
							sign(0) := '0'; -- X positi
						END IF;
						cecomp <= '1';-- start für comp Xabs >= Yabs
						stateatan <= 2;
				WHEN 2 =>
						cecomp <= '0';-- reset
						stateatan <= 3;
				WHEN 3 =>  --cmpgr if (a)Yabs > (b)Xabs th
						-- oktanten 2,3,6, 7
						IF  fYgr = "0001" THEN -- if Yabs > Xabs :
							adiv <= Xabs;-- Inputs vertauschen
							bdiv <= Yabs;
						ELSE  -- Yabs <= Xabs,Oktant 1,4,5,8
							adiv <= Yabs;
							bdiv <= Xabs;
						END IF;
						cntrclk := 1;
						cediv <= '1';-- div starten
						stateatan <= 4;
				WHEN 4 =>  -- div ausführen
					IF cntrclk > 0 THEN
						cntrclk := cntrclk -1;
						cediv <= '0';-- reset
						stateatan <= 4;
					ELSE
						ce1 <= '1';-- star für x^2 
						stateatan <= 5;
					END IF;
				WHEN 5 =>  -- sq(x) cinpsq ausführen
						ce1 <= '0';
						ce2 <= '1';-- start für afkt
						cntrclk := 11;
						stateatan <= 6;
				WHEN 6 => ---- afkt ausführen
					IF cntrclk = 0 THEN
						ce3 <= '1';-- aou * resdiv starten
						stateatan <= 7;--afkt fertig
					ELSE
						ce2 <= '0'; --reset
						cntrclk := cntrclk -1;
						stateatan <= 6;
					END IF;
				WHEN 7 =>	--aou * ainp=resdiv ausführen, atang[0,1] fertig
					ce3 <= '0';-- reset
					stateatan <= 8;
				WHEN 8 =>
					binp <= atang;-- resultat arctang[0,1]
					IF  fYgr = "0001" THEN -- if Yabs > Xabs :,Okt=2
						IF sign = "00" THEN -- oktant 2: 90grd - resdiv
							ainp <= PIdiv2;--start 90gr - resdiv
							stateatan <= 12;-- zu sub
						ELSIF sign = "01" THEN -- oktant 3:90grd + resdiv
								ainp <= PIdiv2;--
								stateatan <= 9;-- zu add
								-------
						ELSIF sign = "10" THEN -- oktant 7:270Grd + resdiv
								ainp <= PIndiv2;--start -90grd + resdiv
								stateatan <= 9;
						ELSE ---------sign = "11" THEN -- oktant6
								ainp <= PIndiv2;--start -90grd - resdiv
								stateatan <= 12;
						END IF;
					ELSE -- Yabs >= Xabs, 
						IF sign = "00" THEN -- oktant 1
								ainp <= X"00000000";-- ohne aenderung
								stateatan <= 9;
						ELSIF sign = "01" THEN --oktant 4: 180grd - resdiv
								ainp <= PI;--start 180grd - resdiv
								stateatan <= 12;
								-------
						ELSIF sign = "10" THEN -- oktant 8
								ainp <= X"00000000";----start 0 grd - resdiv
								stateatan <= 12;
						ELSE ----- sign = "11" THEN -- oktant5
								ainp <= PIn;----start -180grd + resdiv
								stateatan <= 9;
						END IF;
					END IF;
					--------------------------------
				WHEN 9 =>  -- adition ausführen		
						ceadd <= '1';-- start add
						--atanFP <= atanFPa;-- resultat speichern
						stateatan <= 10;--warten auf nächste Start
				WHEN 10 =>
						ceadd <= '0';-- reset
						--atanFP <= atanFPa;-- resultat speichern
						stateatan <= 11;--warten auf nächste Start
				WHEN 11 =>
						atanFP <= atanFPa;-- resultat speichern
						stateatan <= 0;
						------------------------------
				WHEN 12 => -- subtraktion ausführen
						cesub <= '1';-- start sub
						--atanFP <= atanFPs;-- resultat speichern
						stateatan <= 13; --warten auf nächste Start
				WHEN 13 => 
						cesub <= '0';-- reset
						--atanFP <= atanFPs;-- resultat speichern
						stateatan <= 14; --
				WHEN 14 =>
						atanFP <= atanFPs;-- resultat speichern
						stateatan <= 0; --warten auf nächste Start
						---------------------------------
				WHEN OTHERS =>
					stateatan <= 0;
			END CASE;
		END IF; -- if nrst/else
  END IF; -- clk
END PROCESS;--end pacos

--=======Implementation==================================
--  Werte aus cfunctg-------------------------
--X"40b5aa96",--P0	+5.67707365457	index 0
--X"40723f49",--P1	+3.78511273085
--X"3e76b19e",--P2  +0.240911935532 index 2
--X"00000000" 						

--X"40b5aa9c",--Q0	+5.67707629595 index 0
--X"40b5ac55",--Q1	+5.6772864448
--X"3f800000",--Q2  +1.0 				indesx 2
--X"00000000"

Cafkt:   afkt
		generic map(P0=>X"40b5aa96",-- +5.67707365457	index 0 
						P1=>X"40723f49",-- +3.78511273085
						P2=>X"3e76b19e",-- +0.240911935532 index 2	
						Q0=>X"40b5aa9c",-- +5.67707629595 index 0 
						Q1=>X"40b5ac55",-- +5.6772864448
						Q2=>X"3f800000"--  +1.0 			indesx 2 
						)	 
							port map(nrst=>nrst,clk=>clk,ceafkt=>ce2,Xinp=>ain,aout=>aou);
--cfunctg: func_atg 	port map(nrst=>nrst,clk=>clk,cefkt=>ce2,Xinp=>ain,aout=>aou);-- abgeschaltet

ccomp: cmpgr port map(a=>Yabs,b=>Xabs,clk=>clk,ce=>cecomp,result=>fYgr);

cdiv:	divFP port map(a=>adiv,b=>bdiv,clk=>clk,ce=>cediv,result=>resdiv);	

cinpsq:	mulFP port map(a=>resdiv,b=>resdiv,clk=>clk,ce=>ce1,result=>ain);

cfuncmul: mulFP port map(a=>resdiv,b=>aou,clk=>clk,ce=>ce3,result=>atang);

cadd:	addFP port map(a=>ainp,b=>binp,clk=>clk,ce=>ceadd,result=> atanFPa);
csub:	subFP port map(a=>ainp,b=>binp,clk=>clk,ce=>cesub,result=> atanFPs);



---------------------------------------------------	


END Behavioral;