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

-- Company: 
-- Engineer:  A:Kurka
-- 
-- Create Date:   21.6.2021 modif:21.06.24
-- Design Name: AS21
-- Module Name:    AxConv - Behavioral, Konvertiert FP Winkel[32] in Rad 
-- zu  AchsInkr[24bit] anzahl Inkremente gemaäss Achsen Auflösung Resol =  C_ AxRad (konstanten)
----------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
---use IEEE.STD_LOGIC_ARITH.ALL;
---use IEEE.STD_LOGIC_UNSIGNED.ALL;
---use IEEE.STD_LOGIC_UNSIGNED.ALL;
--use ieee.std_logic_signed.ALL;
--USE ieee.math_real.all;
use IEEE.numeric_std.all;

--library UNISIM;
--use UNISIM.VComponents.all;

entity AxConv is
	port (
		nrst		: in std_logic;
		clk 		: in std_logic;
		ceConv   :	in std_logic;
		AFPinp	:in std_logic_vector(31 downto 0);--Input Achse in  FP
		Resol		:in std_logic_vector(31 downto 0);--Achsen Auflössung in Inkr/Rad :C_AxRad
		AchsInkr   :out std_logic_vector(31 downto 0);-- Anzahl Inkremente
		AXgrd    :out std_logic_vector(15 downto 0)-- Achsenwinkel In Grd.X
		);
end AxConv;


architecture Behavioral of AxConv is

------------------------------------------------------------
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;
----------------------------------------------------
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 FPtoInt16 IS
	port (
	a: IN std_logic_VECTOR(31 downto 0);
	clk: IN std_logic;
	ce: IN std_logic;
	result: OUT std_logic_VECTOR(15 downto 0)
	);
END COMPONENT;
----------------------------------------------------
COMPONENT FPtoInt32 IS  --umwandlung in 24 bit SLV
	port (
	a: 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;

--===========================================================
----------------------------------------

CONSTANT FP3600 :std_logic_vector(31 downto 0):=X"45610000";-- FP von 3600
CONSTANT PI2	 :std_logic_vector(31 downto 0):=X"40c90fdb";--FP von 2PI
-------------------------------------------------
SIGNAL AinkrFP		:std_logic_vector(31 downto 0):=X"00000000";-- FP-Winkel in Inkrementen
SIGNAL AFPMul 		:std_logic_vector(31 downto 0):=X"00000000";-- AFPmul:=Afpinp * FP3600
SIGNAL AFPGrd		:std_logic_vector(31 downto 0):=X"00000000";-- AFPGrd:=AfPmul / 2Pi
-----------------------------
SIGNAL ceAinkr		:STD_LOGIC:= '0';-- Start für mul AFPint * Inkr/Rad
SIGNAL cemul		:STD_LOGIC:= '0';-- Start für sub
SIGNAL ceTo16		:STD_LOGIC:= '0';-- Start für FpToInt16=AxGrad
SIGNAL cediv		:STD_LOGIC:= '0';-- start für AFP/PI2

--==============================================================================
begin
--===========================================================
--AxConv(pAxConv):
--Rechnet Diferenz Alt-Neu  (FP) : AinkrAlt - AinpFP=> DiffFP
-- wenn DiffFP > 1 set: InkrAx:=1 resp. DiffFP <-1 set DekrAX:= 1
-- wird von ITPImpGen benützt für Achsen A0..A5
--================================================================
pAxConv: PROCESS
VARIABLE stateConv : INTEGER RANGE 0 TO 15:= 0;

BEGIN
	----IF rising_edge(clk) THEN
	wait until (rising_edge(clk));
		IF nrst = '0' THEN
			stateConv := 0;
			ceAinkr <= '0';
			cemul <= '0';
			cediv <= '0';
			ceTO16 <= '0';
		ELSE
			CASE stateConv IS
				WHEN 0 =>
					IF ceConv = '1' THEN-- AFPint = Achsenwinkel in RAD FP
						ceAinkr <= '1'; --start AinkrFP := AFPint * C_AxRad[Inkr/Rad]
						stateConv := 1;
					ELSE
						stateConv := 0;
					END IF;
				WHEN 1 => --AinkrFP := AFPint * C_AxRad[Inkr/Rad] fertig
					ceAinkr <= '0'; --reset ce
					cemul <= '1'; --  Start für mulFP und FPtoInt24  
					stateConv := 2;
				WHEN 2 => -- ausführen mulFP und FPtoInt32: AchsInkr bereit	
					cemul <= '0';-- init
					cediv <= '1';-- AFPmul/PI2 starten
					stateConv := 3;-- 
				WHEN 3 => -- ausführen AFPmul/PI2
					cediv <= '0';-- reset
					ceto16 <= '1'; -- start FPToInt16: AXGrd.x
					stateConv := 4;
				WHEN 4 => -- ausführen FPToInt16	
					ceto16 <= '0'; --reset CE
					stateConv := 5;
				WHEN 5 =>
					stateConv := 0;-- und warte auf nächste durchlauf
				WHEN OTHERS  => 
					stateConv := 0;
			END CASE;
		END IF; -- if nrst/else
	---END IF; -- clk
END PROCESS;--end pAxConv


--=======================================================================				
cmulAFp:		mulFP 	port map(a=>AFPinp,b=>Resol,clk=>clk,ce=>ceAinkr,result=>AinkrFP);-- state 1
cmulFP:		mulFP		port map(a=>AFPinp,b=>FP3600,clk=>clk,ce=>cemul,result=>AFPmul);-- state 2
cFPInt32:	FPtoInt32 port map(a=>AinkrFP ,clk=>clk, ce=>cediv ,result=>AchsInkr); -- state 2,Ax inkr[24bit]  
cdivGRD:		divFP 	port map(a=>AFPmul,b=>PI2,clk=>clk,ce=>cediv,result=>AFPGrd);-- state 3   
cFPInt16:	FPtoInt16 port map(a=>AFPGrd ,clk=>clk, ce=>ceTo16 ,result=>AXGrd); --state4 ,AxGrd.x[16bit] 
 
-------------------------------------------------
end Behavioral;