-------------------------------------------------------------
-- Filename:  CPM_MSD3.VHD
-- Authors: 
--		Alain Zarembowitch / MSS
-- Version: Rev 0
-- Last modified: 11/13/17

-- Inheritance: 	N/A
--
-- Algorithm: multi-symbol detection 
-- Currently written for L=2 (PCM/FM) non-coherent detection, any h.
-- Then compute the branch metrics for the subsequent trellis decoding.
-- Maximum symbol rate: fclk/4
-- Not applicable for coherent demodulation.
-- See Matlab demod_pcmfm.m for more details on the algorithm.

-- Registers:  725
-- LUTs: 1683
-- DSP48: 8
--    5.261ns (Maximum Frequency: 190.096MHz)

---------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;
use work.CPM_MF_PKG.all;	-- defines types for this interface

entity CPM_MSD3 is
 	generic (
		SIMULATION: std_logic := '0'
	);
    Port ( 
		--GLOBAL CLOCK, RESET
		CLK   : in  std_logic;
		SYNC_RESET: in std_logic;	
			-- synchronous reset
			-- must use SYNC_RESET after changing the modulation scheme
		
		-- INPUT SAMPLES
		-- matched filters complex samples
		-- MPOWERLMAX complex numbers. Aligned with SYMBOL_CLK_IN_D8. CPM_MF_NBITS precision on both I and Q
		MF_I_IN: in MF_TYPE;
		MF_Q_IN: in MF_TYPE;
		MF_SYMBOL_CLK_IN: in std_logic;

		-- CONTROLS
		L: in std_logic_vector(1 downto 0);
			-- truncated frequency impulse length (in number of symbols)
			-- valid range IN THIS COMPONENT: 1,2 
			-- filter selection
		COS_PIH: in std_logic_vector(7 downto 0);
		SIN_PIH: in std_logic_vector(7 downto 0);
			-- cos(pi*h) and sin(pi*h) to be used by multi-symbol detector to rotate the matched filter outputs

		-- Branch metrics outputs, once per symbol
		BM_OUT: out BM_TYPE;
			-- Branch metrics = magnitude of coherent accumulated sum of rotated successive matched filters
			-- L=2 (PCM/FM): 8 branches, 4 states. Uses BM(0-7)
			-- L=1 (FSK/GFSK): 4 branches, 2-states. Uses BM(0,1,6,7)
			-- The index represents the received bits hypothesis from earlier (LSb) to the latest(MSb)
		SYMBOL_CLK_OUT: out std_logic
			-- latency: 11 CLKs
		
		);
end entity;


architecture Behavioral of CPM_MSD3 is
-----------------------------------------------------------------
-- COMPONENTS
-----------------------------------------------------------------
	COMPONENT POLAR3_DUALMAG
	PORT(
		CLK : IN std_logic;
		XA_IN: in std_logic_vector(11 downto 0);
		YA_IN: in std_logic_vector(11 downto 0);
		SAMPLEA_CLK_IN: in std_logic;
		XB_IN: in std_logic_vector(11 downto 0);
		YB_IN: in std_logic_vector(11 downto 0);
		SAMPLEB_CLK_IN: in std_logic;
		MAGNITUDEA_OUT : OUT std_logic_vector(13 downto 0);
		SAMPLEA_CLK_OUT : OUT std_logic;
		MAGNITUDEB_OUT : OUT std_logic_vector(13 downto 0);
		SAMPLEB_CLK_OUT : OUT std_logic
		);
	END COMPONENT;
--------------------------------------------------------
--     SIGNALS
--------------------------------------------------------
signal SYNC_RESET_local: std_logic := '0';
signal MF_SYMBOL_CLK_IN_SHIFT: std_logic_vector(10 downto 1) := (others => '0');

type MF_TYPE4s is array (integer range 0 to 3) of signed((CPM_MF_NBITS-1) downto 0);
type MF_TYPE8s is array (integer range 0 to 7) of signed((CPM_MF_NBITS-1) downto 0);
signal MF3_I: MF_TYPE8s := (others => (others => '0'));
signal MF3_I_M1: MF_TYPE4s := (others => (others => '0'));
signal MF3_I_Z1: MF_TYPE4s := (others => (others => '0'));
signal MF3_I_P1: MF_TYPE4s := (others => (others => '0'));
signal MF3_Q: MF_TYPE8s := (others => (others => '0'));
signal MF3_Q_M1: MF_TYPE4s := (others => (others => '0'));
signal MF3_Q_Z1: MF_TYPE4s := (others => (others => '0'));
signal MF3_Q_P1: MF_TYPE4s := (others => (others => '0'));

signal MF3_ABS: MF_TYPE8s := (others => (others => '0'));
signal MF3_I_D: MF_TYPE4s := (others => (others => '0'));
signal MF3_Q_D: MF_TYPE4s := (others => (others => '0'));

signal MULT0A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT1A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT2A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT3A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT4A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT5A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT6A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT7A: signed(CPM_MF_NBITS-1 downto 0) := (others => '0');
signal MULT0B: signed(7 downto 0) := (others => '0');
signal MULT1B: signed(7 downto 0) := (others => '0');
signal MULT2B: signed(7 downto 0) := (others => '0');
signal MULT3B: signed(7 downto 0) := (others => '0');
signal MULT4B: signed(7 downto 0) := (others => '0');
signal MULT5B: signed(7 downto 0) := (others => '0');
signal MULT6B: signed(7 downto 0) := (others => '0');
signal MULT7B: signed(7 downto 0) := (others => '0');
signal MULT0: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT1: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT2: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT3: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT4: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT5: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT6: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');
signal MULT7: signed(CPM_MF_NBITS+7 downto 0) := (others => '0');

signal POLAR1A_X_IN: signed(11 downto 0) := (others => '0');
signal POLAR1A_Y_IN: signed(11 downto 0) := (others => '0');
signal POLAR1B_X_IN: signed(11 downto 0) := (others => '0');
signal POLAR1B_Y_IN: signed(11 downto 0) := (others => '0');
signal POLAR_SAMPLE_CLK_IN: std_logic := '0';
signal POLAR1A_M: std_logic_vector(13 downto 0) := (others => '0');
signal POLAR1B_M: std_logic_vector(13 downto 0) := (others => '0');
signal POLAR_M_SAMPLE_CLK: std_logic := '0';

signal BM: BM_TYPE := (others => (others => '0'));

--------------------------------------------------------
--      IMPLEMENTATION
--------------------------------------------------------
begin

-- generate all delayed versions of the symbol clock
SYMBOL_CLK_GEN_001: process(CLK)
begin
	if rising_edge(CLK) then
		MF_SYMBOL_CLK_IN_SHIFT <= MF_SYMBOL_CLK_IN_SHIFT(9 downto 1) & MF_SYMBOL_CLK_IN;
	end if;
end process;

SYNC_RESET_local <= '1' when (SYNC_RESET = '1') or  
									((SIMULATION = '1') and ((MF3_I(0)(0) = 'X') or (COS_PIH(0) = 'U'))) else '0';

-- sum the matched filter outputs with the best estimates of the previous symbol (rotated to be aligned with 
-- the incoming matched filter outputs).
STORE_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET_local = '1') then
			MF3_I <= (others => (others => '0'));
			MF3_Q <= (others => (others => '0'));
			MF3_ABS <= (others => (others => '0'));
		elsif(MF_SYMBOL_CLK_IN = '1') then
			if(unsigned(L) = 1) then
				-- to optimize sharing code when L=1 and L=2, use indices 0,1,6,7 for branches, 0,3 for states
				MF3_I(0) <= MF3_I_M1(0) + signed(MF_I_IN(0));
				MF3_I(1) <= MF3_I_P1(3) + signed(MF_I_IN(0));
				MF3_I(6) <= MF3_I_M1(0) + signed(MF_I_IN(1));
				MF3_I(7) <= MF3_I_P1(3) + signed(MF_I_IN(1));

				MF3_Q(0) <= MF3_Q_M1(0) + signed(MF_Q_IN(0));
				MF3_Q(1) <= MF3_Q_P1(3) + signed(MF_Q_IN(0));
				MF3_Q(6) <= MF3_Q_M1(0) + signed(MF_Q_IN(1));
				MF3_Q(7) <= MF3_Q_P1(3) + signed(MF_Q_IN(1));
				
				MF3_ABS(0) <= abs(MF3_I_M1(0) + signed(MF_I_IN(0))) + abs(MF3_Q_M1(0) + signed(MF_Q_IN(0)));
				MF3_ABS(1) <= abs(MF3_I_P1(3) + signed(MF_I_IN(0))) + abs(MF3_Q_P1(3) + signed(MF_Q_IN(0)));
				MF3_ABS(6) <= abs(MF3_I_M1(0) + signed(MF_I_IN(1))) + abs(MF3_Q_M1(0) + signed(MF_Q_IN(1)));
				MF3_ABS(7) <= abs(MF3_I_P1(3) + signed(MF_I_IN(1))) + abs(MF3_Q_P1(3) + signed(MF_Q_IN(1)));

			elsif(unsigned(L) = 2) then
				MF3_I(0) <= MF3_I_M1(0) + signed(MF_I_IN(0));
				MF3_I(1) <= MF3_I_Z1(1) + signed(MF_I_IN(0));
				MF3_I(2) <= MF3_I_Z1(2) + signed(MF_I_IN(1));
				MF3_I(3) <= MF3_I_P1(3) + signed(MF_I_IN(1));
				MF3_I(4) <= MF3_I_M1(0) + signed(MF_I_IN(2));
				MF3_I(5) <= MF3_I_Z1(1) + signed(MF_I_IN(2));
				MF3_I(6) <= MF3_I_Z1(2) + signed(MF_I_IN(3));
				MF3_I(7) <= MF3_I_P1(3) + signed(MF_I_IN(3));
				
				MF3_Q(0) <= MF3_Q_M1(0) + signed(MF_Q_IN(0));
				MF3_Q(1) <= MF3_Q_Z1(1) + signed(MF_Q_IN(0));
				MF3_Q(2) <= MF3_Q_Z1(2) + signed(MF_Q_IN(1));
				MF3_Q(3) <= MF3_Q_P1(3) + signed(MF_Q_IN(1));
				MF3_Q(4) <= MF3_Q_M1(0) + signed(MF_Q_IN(2));
				MF3_Q(5) <= MF3_Q_Z1(1) + signed(MF_Q_IN(2));
				MF3_Q(6) <= MF3_Q_Z1(2) + signed(MF_Q_IN(3));
				MF3_Q(7) <= MF3_Q_P1(3) + signed(MF_Q_IN(3));
				
				MF3_ABS(0) <= abs(MF3_I_M1(0) + signed(MF_I_IN(0))) + abs(MF3_Q_M1(0) + signed(MF_Q_IN(0)));
				MF3_ABS(1) <= abs(MF3_I_Z1(1) + signed(MF_I_IN(0))) + abs(MF3_Q_Z1(1) + signed(MF_Q_IN(0)));
				MF3_ABS(2) <= abs(MF3_I_Z1(2) + signed(MF_I_IN(1))) + abs(MF3_Q_Z1(2) + signed(MF_Q_IN(1)));
				MF3_ABS(3) <= abs(MF3_I_P1(3) + signed(MF_I_IN(1))) + abs(MF3_Q_P1(3) + signed(MF_Q_IN(1)));
				MF3_ABS(4) <= abs(MF3_I_M1(0) + signed(MF_I_IN(2))) + abs(MF3_Q_M1(0) + signed(MF_Q_IN(2)));
				MF3_ABS(5) <= abs(MF3_I_Z1(1) + signed(MF_I_IN(2))) + abs(MF3_Q_Z1(1) + signed(MF_Q_IN(2)));
				MF3_ABS(6) <= abs(MF3_I_Z1(2) + signed(MF_I_IN(3))) + abs(MF3_Q_Z1(2) + signed(MF_Q_IN(3)));
				MF3_ABS(7) <= abs(MF3_I_P1(3) + signed(MF_I_IN(3))) + abs(MF3_Q_P1(3) + signed(MF_Q_IN(3)));
			end if;
		end if;
	end if;
end process;

-- ACS
-- Select the best of two paths for each state, based on the trellis branches.
-- This is an approximation: mathematically, we should compare the magnitude, but we don't have
-- the time for this (all computations must be completed by the time the next batch of matched filter outputs
-- is received, every 4 CLKs).
ACS_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET_local = '1') then
			MF3_I_D <= (others => (others => '0'));
			MF3_Q_D <= (others => (others => '0'));
		elsif(MF_SYMBOL_CLK_IN_SHIFT(1)  = '1') then
			-- L=1,2
			if(MF3_ABS(0) > MF3_ABS(1)) then	-- from state 00 to state 00
				MF3_I_D(0) <= MF3_I(0);
				MF3_Q_D(0) <= MF3_Q(0);
			else
				MF3_I_D(0) <= MF3_I(1);	-- from state 01 to state 00
				MF3_Q_D(0) <= MF3_Q(1);
			end if;
			
			-- L=2 only, unused for L=1
			if(MF3_ABS(2) > MF3_ABS(3)) then	-- from state 10 to state 01
				MF3_I_D(1) <= MF3_I(2);
				MF3_Q_D(1) <= MF3_Q(2);
			else
				MF3_I_D(1) <= MF3_I(3);	-- from state 11 to state 01
				MF3_Q_D(1) <= MF3_Q(3);
			end if;
			-- L=2 only, unused for L=1
			if(MF3_ABS(4) > MF3_ABS(5)) then	-- from state 00 to state 10
				MF3_I_D(2) <= MF3_I(4);
				MF3_Q_D(2) <= MF3_Q(4);
			else
				MF3_I_D(2) <= MF3_I(5);	-- from state 01 to state 10
				MF3_Q_D(2) <= MF3_Q(5);
			end if;
			
			-- L=1,2
			if(MF3_ABS(6) > MF3_ABS(7)) then	-- from state 10 to state 11
				MF3_I_D(3) <= MF3_I(6);
				MF3_Q_D(3) <= MF3_Q(6);
			else
				MF3_I_D(3) <= MF3_I(7);	-- from state 11 to state 11
				MF3_Q_D(3) <= MF3_Q(7);
			end if;
		end if;
	end if;
end process;

-- compute the rotated versions of the matched filters
-- MF3_D(0) -pi*h 
-- MF3_D(3) +pi*h 
-- Since these 2 complex vector rotations have to be completed within 1 clock, we need 8 multipliers
MULT0A <= MF3_I_D(0);
MULT0B <= signed(COS_PIH);
MULT1A <= MF3_Q_D(0);
MULT1B <= signed(SIN_PIH);
MULT2A <= MF3_I_D(0);
MULT2B <= -signed(SIN_PIH);
MULT3A <= MF3_Q_D(0);
MULT3B <= signed(COS_PIH);

MULT4A <= MF3_I_D(3);
MULT4B <= signed(COS_PIH);
MULT5A <= MF3_Q_D(3);
MULT5B <= -signed(SIN_PIH);
MULT6A <= MF3_I_D(3);
MULT6B <= signed(SIN_PIH);
MULT7A <= MF3_Q_D(3);
MULT7B <= signed(COS_PIH);

CMULT_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET_local = '1') then
			MULT0 <= (others => '0');
			MULT1 <= (others => '0');
			MULT2 <= (others => '0');
			MULT3 <= (others => '0');
			MULT4 <= (others => '0');
			MULT5 <= (others => '0');
			MULT6 <= (others => '0');
			MULT7 <= (others => '0');
		else
			MULT0 <= MULT0A * MULT0B;
			MULT1 <= MULT1A * MULT1B;
			MULT2 <= MULT2A * MULT2B;
			MULT3 <= MULT3A * MULT3B;
			MULT4 <= MULT4A * MULT4B;
			MULT5 <= MULT5A * MULT5B;
			MULT6 <= MULT6A * MULT6B;
			MULT7 <= MULT7A * MULT7B;
		end if;
	end if;
end process;

-- complete the vector rotations, then scale by 1/2 to prevent growth/saturation during the accumulation
SUM_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET_local = '1') then
			MF3_I_M1(0) <= (others => '0');
			MF3_Q_M1(0) <= (others => '0');
			MF3_I_Z1(1) <= (others => '0');
			MF3_Q_Z1(1) <= (others => '0');
			MF3_I_Z1(2) <= (others => '0');
			MF3_Q_Z1(2) <= (others => '0');
			MF3_I_P1(3) <= (others => '0');
			MF3_Q_P1(3) <= (others => '0');
		elsif(MF_SYMBOL_CLK_IN_SHIFT(3)  = '1') then
			MF3_I_M1(0) <= resize(MULT0(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS) + resize(MULT1(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS);
			MF3_Q_M1(0) <= resize(MULT2(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS) + resize(MULT3(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS);
			MF3_I_Z1(1) <= resize(MF3_I_D(1)(CPM_MF_NBITS-1 downto 1),CPM_MF_NBITS);
			MF3_Q_Z1(1) <= resize(MF3_Q_D(1)(CPM_MF_NBITS-1 downto 1),CPM_MF_NBITS);
			MF3_I_Z1(2) <= resize(MF3_I_D(2)(CPM_MF_NBITS-1 downto 1),CPM_MF_NBITS);
			MF3_Q_Z1(2) <= resize(MF3_Q_D(2)(CPM_MF_NBITS-1 downto 1),CPM_MF_NBITS);
			MF3_I_P1(3) <= resize(MULT4(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS) + resize(MULT5(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS);
			MF3_Q_P1(3) <= resize(MULT6(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS) + resize(MULT7(CPM_MF_NBITS+7 downto 8),CPM_MF_NBITS);
		end if;
	end if;
end process;

-- Input mux before magnitude lut
MAG_MUX_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(MF_SYMBOL_CLK_IN_SHIFT(1)  = '1') then
			POLAR1A_X_IN <= resize(MF3_I(0),12); 
			POLAR1A_Y_IN <= resize(MF3_Q(0),12); 
			POLAR1B_X_IN <= resize(MF3_I(1),12); 
			POLAR1B_Y_IN <= resize(MF3_Q(1),12); 
		elsif(MF_SYMBOL_CLK_IN_SHIFT(2)  = '1') then
			POLAR1A_X_IN <= resize(MF3_I(2),12); 
			POLAR1A_Y_IN <= resize(MF3_Q(2),12); 
			POLAR1B_X_IN <= resize(MF3_I(3),12); 
			POLAR1B_Y_IN <= resize(MF3_Q(3),12); 
		elsif(MF_SYMBOL_CLK_IN_SHIFT(3)  = '1') then
			POLAR1A_X_IN <= resize(MF3_I(4),12); 
			POLAR1A_Y_IN <= resize(MF3_Q(4),12); 
			POLAR1B_X_IN <= resize(MF3_I(5),12); 
			POLAR1B_Y_IN <= resize(MF3_Q(5),12); 
		elsif(MF_SYMBOL_CLK_IN_SHIFT(4)  = '1') then
			POLAR1A_X_IN <= resize(MF3_I(6),12); 
			POLAR1A_Y_IN <= resize(MF3_Q(6),12); 
			POLAR1B_X_IN <= resize(MF3_I(7),12); 
			POLAR1B_Y_IN <= resize(MF3_Q(7),12); 
		end if;
	end if;
end process;

-- Complex/Real input to magnitude
-- Latency: 5 CLK
POLAR_SAMPLE_CLK_IN <= MF_SYMBOL_CLK_IN_SHIFT(2) or MF_SYMBOL_CLK_IN_SHIFT(3) or MF_SYMBOL_CLK_IN_SHIFT(4) or MF_SYMBOL_CLK_IN_SHIFT(5);
POLAR3_DUALMAG_001: POLAR3_DUALMAG PORT MAP(
	CLK => CLK,
	XA_IN => std_logic_vector(POLAR1A_X_IN),
	YA_IN => std_logic_vector(POLAR1A_Y_IN),
	SAMPLEA_CLK_IN => POLAR_SAMPLE_CLK_IN,
	XB_IN => std_logic_vector(POLAR1B_X_IN),
	YB_IN => std_logic_vector(POLAR1B_Y_IN),
	SAMPLEB_CLK_IN => POLAR_SAMPLE_CLK_IN,
	MAGNITUDEA_OUT => POLAR1A_M,	-- sqrt(X_IN(13:0)^2 + Y_IN(13:0)^2)
	SAMPLEA_CLK_OUT => POLAR_M_SAMPLE_CLK, 
	MAGNITUDEB_OUT => POLAR1B_M,	-- sqrt(X_IN(13:0)^2 + Y_IN(13:0)^2)
	SAMPLEB_CLK_OUT => open 
);

-- latch the branch metrics
BM_OUT_GEN_001:  process(CLK)
begin
	if rising_edge(CLK) then
		SYMBOL_CLK_OUT <= MF_SYMBOL_CLK_IN_SHIFT(10);
		
		if(MF_SYMBOL_CLK_IN_SHIFT(7) = '1') then
			BM(0) <= POLAR1A_M(11 downto 12-CPM_MF_NBITS);
			BM(1) <= POLAR1B_M(11 downto 12-CPM_MF_NBITS);
		end if;
		if(MF_SYMBOL_CLK_IN_SHIFT(8) = '1') then
			BM(2) <= POLAR1A_M(11 downto 12-CPM_MF_NBITS);
			BM(3) <= POLAR1B_M(11 downto 12-CPM_MF_NBITS);
		end if;
		if(MF_SYMBOL_CLK_IN_SHIFT(9) = '1') then
			BM(4) <= POLAR1A_M(11 downto 12-CPM_MF_NBITS);
			BM(5) <= POLAR1B_M(11 downto 12-CPM_MF_NBITS);
		end if;
		if(MF_SYMBOL_CLK_IN_SHIFT(10) = '1') then
			for I in 0 to 5 loop
				BM_OUT(I) <= BM(I);
			end loop;
			BM_OUT(6) <= POLAR1A_M(11 downto 12-CPM_MF_NBITS);
			BM_OUT(7) <= POLAR1B_M(11 downto 12-CPM_MF_NBITS);
		end if;
	end if;
end process;


end Behavioral;
