-------------------------------------------------------------
-- MSS copyright 2019
--	Filename:  CPM_DEMOD.VHD
-- Author: Alain Zarembowitch / MSS
--	Version: 1
--	Date last modified: 4/10/19
-- Inheritance: 	ComBlock library
--
-- description:   continuous phase modulation (CPM) coherent demodulator
-- Input: complex samples, between 4 samples per symbol and 8 samples per symbol.
-- Maximum symbol rate: 99.5% of fclk/4
--
-- Device utilization:
-- FF: 3237
-- LUT: 3994
-- BRAM: 5
-- GCLK: 1
-- DSP48: 52
---------------------------------------------------------------
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_DEMOD is
	generic (
		-- NOTE: most customization parameters defined in CPM_MF_PKG (SEE cpm_mf.vhd)
		SIMULATION: std_logic := '0'
			-- 1 during simulation. 
	 );
    port ( 
		-- RESET, CLOCK
		CLK: in std_logic;	-- processing clock. global. 
		SYNC_RESET: in std_logic;

		--// Input samples
		DATA_I_IN: in std_logic_vector(13 downto 0);
		DATA_Q_IN: in std_logic_vector(13 downto 0);
			-- input samples, 14-bit precision
			-- read at rising edge of CLK when SAMPLE_CLK_IN = '1'.
			-- Format: 2's complement 
		SAMPLE_CLK_IN: in std_logic;
			-- sample clock
			-- CAUTION!!! Must be STRICTLY greater than 2 samples per symbol, 
			-- including the possible clock drifts between transmitter and receiver.
		INPUT_SAMPLING_RATE: in std_logic_vector(31 downto 0);
			-- = fsampling rate / fclk * 2^32

		--// Controls
		NOMINAL_SYMBOL_RATE: in std_logic_vector(31 downto 0);
			-- = fsymbol rate / fclk * 2^32
		M_SEL: in std_logic_vector(2 downto 0);
 			-- modulation order M selection 
			-- 0: M=2, 1: M=4, 2: M=8, 3: M=3 SOQPSK precoder
		FILT_SEL: in std_logic_vector(3 downto 0);
			-- filter selection
			-- 0 = FSK
			-- 1 = PCM/FM
			-- 2 = GFSK BT=0.7
			-- 3 = GFSK BT=0.5
			-- 4 = GFSK BT=0.3
			-- 5 = SOQPSK-MIL 
			-- 6 = SOQPSK-TG 
			-- 7 = multi-h ARTM CPM (future) 
			-- 8 = GFSK BT=0.25
		MODULATION_INDEX: in std_logic_vector(15 downto 0);
			-- user-specified modulation index. unsigned fixed-point format 4.12
			-- When modulation index is NOT h=0.5, non-coherent detection is enabled (multi-symbol detection + AFC)
		FRAME_LENGTH: in std_logic_vector(12 downto 0);
			-- frame length in bits, including the 32-bit sync word. 
			-- Needed only when sync word detection is enabled (generally SOQPSK)
	   CONTROL: in std_logic_vector(15 downto 0);
			-- bit 0: spectrum inversion enabled(1)/disabled(0)
			-- bit 1: AFC enabled(1)/disabled(0). 
			-- bit 2: freeze AFC  
			-- bit 3: sync word detection enabled (1)/disabled(0)  SOQPSK ONLY
			-- bit 4: FFT enabled(1)/disabled(0)

		--// Demodulator output
		DATA_OUT: out std_logic_vector(3 downto 0) := (others => '0');
			-- 4 bit soft-quantized demodulator output
			-- Read at rising edge of CLK when SAMPLE_CLK_OUT = '1';
			-- 4-bit sample format:
			-- 0000 most negative, 1111 most positive, with no zero representation 
		SAMPLE_CLK_OUT: out std_logic;
			-- one CLK-wide pulse

		-- Monitoring Information
		AFC_LOCK_OUT: out std_logic;
		CARRIER_LOCK_OUT: out std_logic;
		SIGNAL_PRESENT_OUT: out std_logic;
		CARRIER_FREQUENCY_ERROR: out std_logic_vector(31 downto 0);
			-- frequency error, expressed as cfe/fsymbol_rate*2^30
		POWER_OUT: out std_logic_vector(13 downto 0);
			-- detected power, raw unfiltered.
		
		-- Test Points
		STATE_OUT: out std_logic_vector(2 downto 0);
		TP: out std_logic_vector(10 downto 1);	
			-- 1: AFC lock (abs(residual frequency error) < symbol rate/16)
			-- 2: signal presence
			-- 3: recovered carrier
			-- 4: recovered bit timing

		--// ComScope traces
		-- CS1: I channel after AGC, frequency translation to baseband, resampling at 4 samples/symbol
		-- CS2: symbol tracking loop: accumulated ST phase correction
		-- CS3: carrier tracking loop: accumulated carrier phase correction
		-- CS4: FREQ2 discriminator (easier to follow symbol tracking loop ability to track)
		CS1: out std_logic_vector(7 downto 0);	
		CS1_CLK: out std_logic;
		CS2: out std_logic_vector(7 downto 0);	
		CS2_CLK: out std_logic;
		CS3: out std_logic_vector(7 downto 0);	
		CS3_CLK: out std_logic;
		CS4: out std_logic_vector(7 downto 0);	
		CS4_CLK: out std_logic;
		CS5: out std_logic_vector(7 downto 0);	
		CS5_CLK: out std_logic
			);
end entity;

architecture behavioral of CPM_DEMOD is
--------------------------------------------------------
--      COMPONENTS
--------------------------------------------------------
	COMPONENT AGC18
	 Generic(
		AVERAGING_LENGTH: integer;
		SIMULATION: std_logic
	 );
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		DATA_I_IN : IN std_logic_vector(13 downto 0);
		DATA_Q_IN : IN std_logic_vector(13 downto 0);
		SAMPLE_CLK_IN : IN std_logic;
		MTARGET : IN std_logic_vector(13 downto 0);
		CONTROL : IN std_logic_vector(0 to 0);
		SYMBOLx2_CLK_IN : IN std_logic;          
		DATA_I_OUT : OUT std_logic_vector(13 downto 0);
		DATA_Q_OUT : OUT std_logic_vector(13 downto 0);
		SAMPLE_CLK_OUT : OUT std_logic;
		SATURATION : OUT std_logic;
		DATA_M : OUT std_logic_vector(13 downto 0);
		SAMPLE_M_CLK : OUT std_logic;
		TP : OUT std_logic_vector(10 downto 1);
		CS1 : OUT std_logic_vector(7 downto 0);
		CS1_CLK : OUT std_logic
		);
	END COMPONENT;

	COMPONENT DIGITAL_DC3
	PORT(
		CLK : IN std_logic;
		DATA_I_IN : IN std_logic_vector(17 downto 0);
		DATA_Q_IN : IN std_logic_vector(17 downto 0);
		SAMPLE_CLK_IN : IN std_logic;
		PHASE_ROTATION : IN std_logic_vector(11 downto 0);          
		DATA_I_OUT : OUT std_logic_vector(17 downto 0);
		DATA_Q_OUT : OUT std_logic_vector(17 downto 0);
		SAMPLE_CLK_OUT : OUT std_logic;
		SATURATION : OUT std_logic
		);
	END COMPONENT;

	COMPONENT RESAMPLING31
	 Generic(
		NBITS: integer;
		CLK_PERIOD: integer;	
		SIMULATION: std_logic 
	 );
	PORT(
		CLK : IN std_logic;
		DATA_I_IN : IN std_logic_vector((NBITS-1) downto 0);
		DATA_Q_IN : IN std_logic_vector((NBITS-1) downto 0);
		TT_IN: in std_logic_vector(63 downto 0);
		SAMPLE_CLK_IN : IN std_logic;
		INPUT_SAMPLING_RATE: in std_logic_vector(31 downto 0);
		NOMINAL_RESAMPLING_RATE : IN std_logic_vector(31 downto 0);
		NCO_OFFSET : IN std_logic_vector(31 downto 0);
		NCO_OFFSET_SAMPLE_CLK : IN std_logic;          
		DATA_I_OUT : OUT std_logic_vector((NBITS-1) downto 0);
		DATA_Q_OUT : OUT std_logic_vector((NBITS-1) downto 0);
		TT_OUT: out std_logic_vector(63 downto 0);
		SAMPLE_CLK_OUT : OUT std_logic
		);
	END COMPONENT;

	COMPONENT FAST_FREQUENCY_DETECTION
	generic (
		NID: integer; 
		NID_LOG2: integer;	
		SIMULATION: std_logic 
	);
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		DATA_I_IN : IN std_logic_vector(17 downto 0);
		DATA_Q_IN : IN std_logic_vector(17 downto 0);
		SAMPLE_CLK_IN : IN std_logic;
		NID_SYNC_IN: in std_logic;
		SATURATION_IN: in std_logic;
		ENABLE: in std_logic;
		CONTROL: in std_logic_vector(1 downto 0);
		FFTM: out std_logic_vector(13 downto 0);
		FFTM2: out std_logic_vector(13 downto 0); 
		FFTM_AVRG: out std_logic_vector(13 downto 0);
		FFTF: out std_logic_vector(10 downto 0);
		FFTP: out std_logic_vector(10 downto 0);
		FFT_READY: out std_logic;
		SATURATION: out std_logic;
		EXCESSIVE_NONLINEARITY: out std_logic;
		TP : OUT std_logic_vector(10 downto 1)
		);
	END COMPONENT;

	COMPONENT CPM_MF
	PORT(
		CLK   : in  std_logic;
		SYNC_RESET: in std_logic;	
		DATA_I_IN  : in  std_logic_vector((CPM_MF_NBITS-1) downto 0);
		DATA_Q_IN  : in  std_logic_vector((CPM_MF_NBITS-1) downto 0);
		SAMPLE_CLK_IN: in std_logic;	
		SAMPLE_CNTR_IN: in std_logic_vector(1 downto 0);
		M_SEL: in std_logic_vector(2 downto 0);
		FILT_SEL: in std_logic_vector(3 downto 0);
		MODULATION_INDEX: in std_logic_vector(15 downto 0);
		MODULATION_INDEX_VALID: in std_logic;
		BM_OUT: out BM_TYPE;
		SYMBOL_CLK_OUT: out std_logic;
		MF_I_OUT: out MF_TYPE;
		MF_Q_OUT: out MF_TYPE;
		MF_SYMBOL_CLKx2_OUT: out std_logic;
		MF_SYMBOL_CLK_OUT: out std_logic;
		COS_PIH: out std_logic_vector(7 downto 0);
		SIN_PIH: out std_logic_vector(7 downto 0);
		MF_INITIALIZED: out std_logic
	);
	END COMPONENT;

	COMPONENT DELAY2
	 Generic(
		ADDR_WIDTH: integer;	
		DATA_WIDTH: integer;
		NSAMPLES_DELAY: integer
	);
	PORT(
		CLK : IN std_logic;
		DATA_IN : IN std_logic_vector((DATA_WIDTH-1) downto 0);
		SAMPLE_CLK_IN : IN std_logic;          
		DATA_OUT : OUT std_logic_vector((DATA_WIDTH-1) downto 0);
		SAMPLE_CLK_OUT : OUT std_logic
		);
	END COMPONENT;

	COMPONENT CPM_MSD3
	GENERIC(
		SIMULATION: std_logic 
	);
	PORT(
		CLK   : in  std_logic;
		SYNC_RESET: in std_logic;	
		MF_I_IN: in MF_TYPE;
		MF_Q_IN: in MF_TYPE;
		MF_SYMBOL_CLK_IN: in std_logic;	
		L: in std_logic_vector(1 downto 0);
		COS_PIH: in std_logic_vector(7 downto 0);
		SIN_PIH: in std_logic_vector(7 downto 0);
		BM_OUT: out BM_TYPE;
		SYMBOL_CLK_OUT: out std_logic
	);
	END COMPONENT;
	
	COMPONENT CPM_VA_PMU
	GENERIC(
		NBITS: integer;	
		SIMULATION: std_logic 
	);
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		BM: in BM_TYPE;
		BM_SAMPLE_CLK_IN : IN std_logic;
		TRELLIS_TYPE: in std_logic_vector(1 downto 0);
		SAMPLE_CLK_OUT : OUT std_logic;
		DATA_OUT: out std_logic;
		RELIABILITY_OUT: out std_logic_vector((SOVA_NBITS-2) downto 0);
		BEST_MF_INDEX: out integer range 0 to MPOWERLMAX-1;
		BEST_TB_STATE: out std_logic_vector(1 downto 0)
		);
	END COMPONENT;

	COMPONENT MATCHED_FILTERNx1
	generic (
		SYNC_LENGTH: integer range 16 to 128;
		SYNC_WORD: std_logic_vector(127 downto 0);
		NSAMPLES_PER_BIT: integer
	);
	PORT(
		CLK : IN std_logic;
		DATA_IN : IN std_logic;
		SAMPLE_CLK_IN : IN std_logic;
		FLIPPED : IN std_logic;          
		DETECT : OUT std_logic;
		MF_OUT : OUT std_logic_vector(7 downto 0);
		MF_OUT_READY : OUT std_logic;
		SIGN_OUT : OUT std_logic
		);
	END COMPONENT;

	COMPONENT FIFO
	GENERIC(
		DATA_WIDTH: integer;	
		DEPTH: integer 
	);
	PORT(
		CLK   : in  std_logic;
		DATA_IN   : in  std_logic_vector(DATA_WIDTH-1 downto 0);
		DATA_IN_VALID  : in std_logic;
		DATA_OUT   : out  std_logic_vector(DATA_WIDTH-1 downto 0);
		DATA_OUT_VALID  : out std_logic
		);
	END COMPONENT;

	COMPONENT AFC2
	GENERIC(
		SIMULATION: std_logic 
	);
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		FREQ_IN  : in  std_logic_vector(17 downto 0);
		SAMPLE_CLK_IN: in std_logic;	
		FREEZE: in std_logic;
		NCO_CONTROL : OUT std_logic_vector(31 downto 0);
		NCO_CONTROL_CLK : OUT std_logic;
		AFC_LOCK: out std_logic
		);
	END COMPONENT;

	COMPONENT CARRIER_TRACKING
	GENERIC(
		SIMULATION: std_logic 
	);
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		OPTIMUM_SAMPLING_CLK : IN std_logic;
		PHASE_ERROR : IN std_logic_vector(10 downto 0);
		LOOP_GAIN : IN std_logic_vector(1 downto 0);
		ACCELERATED_CT_RESPONSE : IN std_logic;
		LOCK_THRESHOLD : IN std_logic_vector(3 downto 0);          
		NCO_CONTROL : OUT std_logic_vector(19 downto 0);
		NCO_CONTROL_CLK : OUT std_logic;
		CARRIER_LOCK : OUT std_logic;
		CS1 : OUT std_logic_vector(7 downto 0);
		CS1_CLK : OUT std_logic;
		CS2 : OUT std_logic_vector(7 downto 0);
		CS2_CLK : OUT std_logic;
		CS3 : OUT std_logic_vector(7 downto 0);
		CS3_CLK : OUT std_logic
		);
	END COMPONENT;

	COMPONENT SYMBOL_TIMING_LOOP5
	 Generic(
		SIMULATION: std_logic
	 );
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		SYMBOL_CLK_IN : IN std_logic;
		SYMBOL_CLKx2_IN : IN std_logic;
		DATA_IN : IN std_logic_vector(17 downto 0);
		FREEZE_SYMBOL_TIMING : IN std_logic;
		LOOP_GAIN : IN std_logic_vector(7 downto 0);          
		NCO_TIMING_CONTROL : OUT std_logic_vector(31 downto 0);
		NCO_TIMING_CONTROL_CLK : OUT std_logic;
		CS1 : OUT std_logic_vector(7 downto 0);
		CS1_CLK : OUT std_logic
		);
	END COMPONENT;

	component SIM2OUTFILE is
		 Generic (
			FILENAME: string
		 );
		 Port ( 
			--// CLK, RESET
			CLK: in std_logic;
				-- Reference clock
			DATA1: in std_logic_vector(11 downto 0);
			DATA2: in std_logic_vector(11 downto 0);
			DATA3: in std_logic_vector(11 downto 0);
			SAMPLE_CLK_IN: in std_logic
				-- DATA_IN is read at the rising edge of CLK when SAMPLE_CLK_IN = '1'

	);
	END COMPONENT;

--------------------------------------------------------
--     SIGNALS
--------------------------------------------------------
-- Suffix _D indicates a one CLK delayed version of the net with the same name
-- Suffix _E indicates an extended precision version of the net with the same name
-- Suffix _N indicates an inverted version of the net with the same name

--//-- GENERAL -------------------------------
signal COHERENT_DEMOD: std_logic := '0';
signal TRELLIS_TYPE: std_logic_vector(1 downto 0) := (others => '0');

--//-- STATE MACHINE -------------------------------
signal STATE: integer range 0 to 5 := 0;
signal STATE_TIMER: unsigned(13 downto 0) := (others => '0');

--// AGC ----------------------	
signal DATA1_I: std_logic_vector(13 downto 0) := (others => '0');
signal DATA1_Q: std_logic_vector(13 downto 0) := (others => '0');
signal SAMPLE1_CLK: std_logic := '0';
signal SATURATION1: std_logic := '0';

--//-- FREQUENCY TRANSLATION 1  -------------------------------
signal DATA1_Ix: std_logic_vector(17 downto 0) := (others => '0');
signal DATA1_Qx: std_logic_vector(17 downto 0) := (others => '0');
signal CCF_PHASE1: signed(31 downto 0) := (others => '0');
signal CCF_PHASE2: signed(31 downto 0) := (others => '0');
signal CCF_PHASE_OFFSET: signed(1 downto 0) := (others => '0');
signal DATA2_I: std_logic_vector(17 downto 0) := (others => '0');
signal DATA2_Q: std_logic_vector(17 downto 0) := (others => '0');
signal DATA2A_Q: std_logic_vector(17 downto 0) := (others => '0');
signal SAMPLE2_CLK: std_logic := '0';
signal SATURATION2: std_logic := '0';


--//-- RESAMPLING AT 4*SYMBOL RATE ---------------------
signal NOMINAL_SYMBOL_RATEx4: std_logic_vector(31 downto 0) := (others => '0');
signal DATA3_I: std_logic_vector(13 downto 0) := (others => '0');
signal DATA3_Q: std_logic_vector(13 downto 0) := (others => '0');
signal SYMBOL3_CLKx4: std_logic := '0';

--//-- HALF-BAND FILTER -------------------------------------------------
signal SYMBOL4_CLKx4: std_logic := '0';
signal SYMBOL4_CLKx2: std_logic := '0';
signal SYMBOL4_CLK: std_logic := '0';
signal SAMPLE4_CNTR: unsigned(2 downto 0) := (others => '0');
signal DATA4_I: std_logic_vector(13 downto 0) := (others => '0');
signal DATA4_Q: std_logic_vector(13 downto 0) := (others => '0');
signal DATA4_Ix: std_logic_vector(17 downto 0) := (others => '0');
signal DATA4_Qx: std_logic_vector(17 downto 0) := (others => '0');

--//-- FAST FREQUENCY ACQUISITION ---------------------------------------
signal FFD_ENABLE: std_logic := '0';
signal FFD_SATURATION_IN: std_logic := '0';
signal FFD_NID_SYNC_IN: std_logic := '0';
signal FFTM: std_logic_vector(13 downto 0) := (others => '0');
signal FFTM2: std_logic_vector(13 downto 0) := (others => '0');
signal FFTM_AVRG: std_logic_vector(13 downto 0) := (others => '0');
signal FFTM_AVRGx4: std_logic_vector(13 downto 0) := (others => '0');
signal FFTM_AVRGx0p5: std_logic_vector(13 downto 0) := (others => '0');
signal FFTM_THRESHOLD: unsigned(13 downto 0) := (others => '0');
signal SIGNAL_PRESENT_local: std_logic := '0';
signal SIGNAL_ABSENT: std_logic := '0';
signal SIGNAL_PRESENCE_CONFIDENCE: unsigned(3 downto 0) := (others => '0');	
signal FFTF: std_logic_vector(10 downto 0) := (others => '0');
signal FFTP: std_logic_vector(10 downto 0) := (others => '0');
signal FFT_READY: std_logic := '0';
signal FFT_READY_D: std_logic := '0';
signal FFD_IGNORE_OUTPUT: std_logic := '0';
signal FFD_IGNORE_OUTPUT_D: std_logic := '0';
signal FFT_FREQ_ERROR: std_logic_vector(31 downto 0) := (others => '0');
signal FFT_FREQ_CORRECTION: signed(31 downto 0) := (others => '0');
signal FFD_SATURATION_OUT: std_logic := '0';

--//-- MATCHED FILTERS ------------------------
signal BM_MF: BM_TYPE := (others => (others => '0'));
signal BM_MF_SYMBOL_CLK: std_logic := '0';
signal MF_I: MF_TYPE := (others => (others => '0'));
signal MF_Q: MF_TYPE := (others => (others => '0'));
signal MF_SYMBOL_CLKx2: std_logic := '0';
signal MF_SYMBOL_CLK: std_logic := '0';
signal MF_SYMBOL_CLK_D: std_logic := '0';

signal MF_DATA1_I: std_logic_vector((MPOWERLMAX*CPM_MF_NBITS)-1 downto 0) := (others => '0');
signal MF_DATA1_Q: std_logic_vector((MPOWERLMAX*CPM_MF_NBITS)-1 downto 0) := (others => '0');
signal MF_DATA2_I: std_logic_vector((MPOWERLMAX*CPM_MF_NBITS)-1 downto 0) := (others => '0');
signal MF_DATA2_Q: std_logic_vector((MPOWERLMAX*CPM_MF_NBITS)-1 downto 0) := (others => '0');
signal MF2_I: MF_TYPE := (others => (others => '0'));
signal MF2_Q: MF_TYPE := (others => (others => '0'));
signal MF2_SYMBOL_CLK: std_logic := '0';
signal MF2_SYMBOL_CLKx2: std_logic := '0';
signal MF_E_I: MF_TYPE := (others => (others => '0'));
signal MF_E_Q: MF_TYPE := (others => (others => '0'));
signal MF_L_I: MF_TYPE := (others => (others => '0'));
signal MF_L_Q: MF_TYPE := (others => (others => '0'));

--//-- BEST MF OUTPUT ------------------------------
signal MF_SYMBOL_CLKx2_SHIFT: std_logic_vector(9 downto 1) := (others => '0');
signal MF_SYMBOL_CLK_SHIFT: std_logic_vector(9 downto 1) := (others => '0');
signal MF_I_D: MF_TYPE := (others => (others => '0'));
signal MF_Q_D: MF_TYPE := (others => (others => '0'));
signal BEST_MF_I: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_Q: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_E_I: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_E_Q: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_L_I: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_L_Q: signed((CPM_MF_NBITS-1) downto 0):= (others => '0');
signal BEST_MF_VALID: std_logic := '0';


--//-- MULTI-SYMBOL DETECTION ------------------------------
signal SYNC_RESET_NONCOHERENTDET: std_logic := '0'; 
signal MSD3_L: std_logic_vector(1 downto 0) := (others => '0');
signal COS_PIH: std_logic_vector(7 downto 0) := (others => '0');
signal SIN_PIH: std_logic_vector(7 downto 0) := (others => '0');
signal BM_MSD3_SYMBOL_CLK: std_logic := '0';
signal BM_MSD3: BM_TYPE := (others => (others => '0'));


--//-- TRELLIS DECODING ------------------------------
signal BM: BM_TYPE := (others => (others => '0'));
signal BM_SYMBOL_CLK: std_logic := '0';
signal BM_SYMBOL_CLK_D7: std_logic := '0';
signal SYNC_RESET_COHERENTDET: std_logic := '0'; 
signal VA_RELIABILITY: std_logic_vector((SOVA_NBITS-2) downto 0) := (others => '0');
signal DATA6: std_logic_vector(3 downto 0) := (others => '0');
signal SAMPLE6_CLK: std_logic := '0';
signal DATA7: std_logic_vector(3 downto 0) := (others => '0');
signal SAMPLE7_CLK: std_logic := '0';
signal DATA7_D: std_logic_vector(3 downto 0) := (others => '0');
signal SAMPLE7_CLK_D: std_logic := '0';
signal BEST_TB_STATE: std_logic_vector(1 downto 0) := (others => '0');
signal BEST_MF_INDEX: integer range 0 to MPOWERLMAX-1 := 0;

--// SYNC DETECTION -----------------------------------------
signal SYNC_DETECT0: std_logic := '0';
signal SYNC_DETECT90: std_logic := '0';
signal SYNC_DETECT270: std_logic := '0';
signal SYNC_DETECT_SIGN0: std_logic := '0';
signal SYNC_DETECT_SIGN90: std_logic := '0';
signal SYNC_DETECT_SIGN270: std_logic := '0';
signal SYNC_DETECT_READY: std_logic := '0';
signal SYNC_DETECT: std_logic := '0';
signal SYNC_CONFIDENCE: unsigned(2 downto 0) := (others => '0');
signal BIT_CNTR: unsigned(12 downto 0) := (others => '0');
signal BIT_CNTR_INC: unsigned(12 downto 0) := (others => '0');
signal OUTPUT_ENABLE: std_logic := '0';

--//-- AFC ------------------------------
signal AFC_EN: std_logic := '0';
signal AFC_SYNC_RESET: std_logic := '0';
signal AFC_NCO_CONTROL: std_logic_vector(31 downto 0) := (others => '0');
signal AFC_NCO_CONTROL_CLK: std_logic := '0';
signal AFC_NCO_CONTROL2: signed(31 downto 0) := (others => '0');
signal AFC_NCO_CONTROL2_CLK: std_logic := '0';
signal AFC_LOCK: std_logic := '0';

--//-- CARRIER TRACKING ------------------------------
signal PE: signed(11 downto 0) := (others => '0');
signal PE_SYMBOL_CLK: std_logic := '0';
signal PE_ACC: signed(23 downto 0) := (others => '0');
signal CT_SYNC_RESET: std_logic := '0';
signal CT_ACCELERATED_CT_RESPONSE: std_logic := '0';
signal CT_LOCK_THRESHOLD: std_logic_vector(3 downto 0) := (others => '0');
signal CARRIER_LOCK_local: std_logic := '0';
signal RESIDUAL_FREQUENCY_OFFSET: std_logic_vector(19 downto 0) := (others => '0');
signal RESIDUAL_FREQUENCY_OFFSET_CLK: std_logic := '0';
signal CF_NCO_CONTROL: signed(31 downto 0) := (others => '0');
signal CF_NCO_CONTROL1a: signed(31 downto 0) := (others => '0');
signal CF_NCO_CONTROL1b: signed(31 downto 0) := (others => '0');

signal CF_NCO_CONTROL_SAMPLE_CLK: std_logic := '0';

--//-- SYMBOL TIMING ERROR (2/2) ------------------------
signal STE_VALID: std_logic := '0';
signal STE: signed(11 downto 0) := (others => '0');
signal STE_slv: std_logic_vector(31 downto 0) := (others => '0');
signal STE_ACC: signed(31 downto 0) := (others => '0');

--//-- SYMBOL TRACKING  ------------------------
signal ST_SYNC_RESET: std_logic := '0';
signal ST_LOOP_GAIN: std_logic_vector(7 downto 0) := (others => '0');
signal ST_SYMBOL_CLK_IN: std_logic := '0';
signal ST_SYMBOL_CLKx2_IN: std_logic := '0';
signal SYMBOL4_CLKx4_D: std_logic := '0';
signal SYMBOL4_CLKx4_D2: std_logic := '0';
signal SYMBOL4_CLKx4_D3: std_logic := '0';
signal SYMBOL4_CLKx2_D3: std_logic := '0';
signal SAMPLE4_CNTR_D: unsigned(2 downto 0) := (others => '0');
signal SAMPLE4_CNTR_D2: unsigned(2 downto 0) := (others => '0');
signal SAMPLE4_CNTR_D3: unsigned(2 downto 0) := (others => '0');
signal DATA4_I_D: std_logic_vector(13 downto 0) := (others => '0');
signal DATA4_Q_D: std_logic_vector(13 downto 0) := (others => '0');
signal MULT41: signed(27 downto 0) :=(others => '0');
signal MULT41_D: signed(27 downto 0) :=(others => '0');
signal MULT42: signed(27 downto 0) :=(others => '0');
signal MULT42_D: signed(27 downto 0) :=(others => '0');
signal FREQ1: signed(27 downto 0) :=(others => '0');
signal FREQ2: std_logic_vector(17 downto 0) := (others => '0');
signal ST_DATA_IN: std_logic_vector(17 downto 0) := (others => '0');
signal ST_DATA_I_IN: std_logic_vector(17 downto 0) := (others => '0');
signal ST_DATA_Q_IN: std_logic_vector(17 downto 0) := (others => '0');
signal ST_NCO_TIMING_CONTROL1: std_logic_vector(31 downto 0) := (others => '0');
signal ST_NCO_TIMING_CONTROL1_CLK: std_logic := '0';
signal ST_NCO_TIMING_CONTROL2: std_logic_vector(31 downto 0) := (others => '0');
signal ST_NCO_TIMING_CONTROL2_CLK: std_logic := '0';
signal ST_NCO_TIMING_CONTROL: std_logic_vector(31 downto 0) := (others => '0');
signal ST_NCO_TIMING_CONTROL_CLK: std_logic := '0';
signal ST_NCO_TIMING_CONTROLB: std_logic_vector(31 downto 0) := (others => '0');

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

--// general
GEN_001: process(CLK)
begin
	if rising_edge(CLK) then
		-- Coherent demodulation when modulation index is 0.5: use Viterbi algorithm, FFT for initial frequency acquisition and Costas loop
		-- Non coherent demodulation for other modulation indices: use multi-symbol detection + Viterbi algorithm, AFC. FFT and Costas loop are disabled.
		if(MODULATION_INDEX = x"0800") and (unsigned(FILT_SEL) /= 1) then
			COHERENT_DEMOD <= '1';
		else
			COHERENT_DEMOD <= '0';
		end if;
			
	end if;
end process;



--//-- STATE MACHINE -------------------------------
-- STATE_TIMER: expiration timer for future use (just in case demod is stuck in a STATE).
-- Until we find such an occurence, STATE_TIMER is unused.
STATE_GEN_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') then
		    -- restart acquisition
			STATE <= 0;
			STATE_TIMER <= (others => '0');	-- expired
		elsif(STATE = 0) and (COHERENT_DEMOD = '1') and (CONTROL(4) = '1') then
		    -- goto FFT-based fast frequency acquisition (only when h = 0.5)
		   STATE <= 1; 
			STATE_TIMER <= (others => '0');	-- expired
		elsif(STATE = 0) and ((COHERENT_DEMOD = '0') or (CONTROL(4) = '0')) then
			STATE <= 2;	-- bypass FFT
		elsif(STATE = 1) and (FFT_READY_D = '1')and (FFD_IGNORE_OUTPUT_D = '0') and (SIGNAL_PRESENT_local = '1') then
			-- 1st FFT computation done and signal presence detected
			STATE <= 2; -- tracking mode
			-- have started when no signal was present and frequency error is inaccurate.
--		elsif(STATE = 2) and (FFT_READY_D = '1') and (SIGNAL_PRESENT_local = '1') then
--            -- 2nd FFT computation done, signal still present. We can trust the FFT frequency error measurement
--				-- as it was performed while the signal was fully present.
--            STATE <= 3; -- await frame sync.

		-- Negative sensors:
		elsif(STATE = 1) and (FFT_READY_D = '1')and (FFD_IGNORE_OUTPUT_D = '1') then
			-- do over. Previous FFT had too much saturation to be taken seriously
			STATE <= 0;
		elsif(FFT_READY_D = '1')and (SIGNAL_ABSENT = '1') and (CONTROL(4) = '1') then
			-- persistent signal absent from FFT. Back to acquisition.
			STATE <= 0;
			
	   -- TODO: persistent loss of carrier lock ?
		elsif(STATE_TIMER /= 0) and (SYMBOL4_CLK = '1') then
			STATE_TIMER <= STATE_TIMER - 1;
		end if;
	end if;
end process;

-- monitor STATE 
STATE_OUT <= std_logic_vector(to_unsigned(STATE,3));

STATE_GEN_002: process(CLK)
begin
	if rising_edge(CLK) then
		if(STATE = 0) then
		    -- goto FFT-based fast frequency acquisition
		    -- trigger FFT
		    FFD_ENABLE <= '1';
--		elsif(FFT_READY_D = '1') then
--		  -- continuously checking for signal presence and frequency error
--		  FFD_ENABLE <= '1';
		else
		    FFD_ENABLE <= '0';
		end if;   
    end if;
end process;


--//-- INTERNAL AGC -------------------------------------------------------------
-- normalize the input
AGC18_000: AGC18 
GENERIC MAP(
	AVERAGING_LENGTH => 128,	-- this is a constant envelope signal. OK to average over just a few (64) symbols.
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	SYNC_RESET => SYNC_RESET,
	DATA_I_IN => DATA_I_IN,
	DATA_Q_IN => DATA_Q_IN,
	SAMPLE_CLK_IN => SAMPLE_CLK_IN,
	MTARGET => "00100000000000",	-- adjusted to prevent FFD saturation
	CONTROL => "1",	-- always enabled
	SYMBOLx2_CLK_IN => SYMBOL4_CLKx2,	
	DATA_I_OUT => DATA1_I,
	DATA_Q_OUT => DATA1_Q,
	SAMPLE_CLK_OUT => SAMPLE1_CLK,
	SATURATION => SATURATION1,
	DATA_M => open,
	SAMPLE_M_CLK => open,
	TP => open,
	CS1 => open,
	CS1_CLK => open
);

--//-- FREQUENCY TRANSLATION 1  -------------------------------
-- Remove center frequency. 
-- latency 3 CLKs
DATA1_Ix <= DATA1_I & "0000";
DATA1_Qx <= DATA1_Q & "0000";
DIGITAL_DC3_001: DIGITAL_DC3 PORT MAP(
	CLK => CLK,
	DATA_I_IN => DATA1_Ix,
	DATA_Q_IN => DATA1_Qx,
	SAMPLE_CLK_IN => SAMPLE1_CLK,
	PHASE_ROTATION => std_logic_vector(CCF_PHASE2(31 downto 20)),	-- CCF_PHASE2 is added to the input signal phase
	DATA_I_OUT => DATA2_I,
	DATA_Q_OUT => DATA2_Q,
	SAMPLE_CLK_OUT => SAMPLE2_CLK,
	SATURATION => SATURATION2
);

-- spectrum inversion
-- invert Q channel when CONTROL(0) = '1'
DATA2A_Q <= DATA2_Q when (CONTROL(0) = '0') else std_logic_vector(- signed(DATA2_Q));

NCO32_001: process(CLK)
begin
	if rising_edge(CLK) then
		CF_NCO_CONTROL1a <= resize(CF_NCO_CONTROL(31 downto 0),32); -- carrier tracking loop  (could adjust loop gain here as well)
		CF_NCO_CONTROL1b <= CF_NCO_CONTROL1a + resize(FFT_FREQ_CORRECTION(31 downto 2),32); 
		
		if(SYNC_RESET = '1') or ((SIMULATION = '1') and (CF_NCO_CONTROL1a(0) = 'X')) then
		     -- prevents X propagation in the accumulator during simulation
				CCF_PHASE1 <= (others => '0');
			 -- or, start simulation with some offset to test demod phase ambiguity removal
			  --CCF_PHASE1 <= x"c0000000";
		elsif(SYMBOL4_CLKx4 = '1') and (COHERENT_DEMOD = '1') then	
			-- accumulate phase based on carrier tracking loop and FFT initial acquisition
			-- update 4x per symbol for smoother phase correction
			CCF_PHASE1 <= CCF_PHASE1 + CF_NCO_CONTROL1b;
		elsif(AFC_NCO_CONTROL2_CLK = '1') and (COHERENT_DEMOD = '0') then	
			-- accumulate phase based on AFC 
			-- update 4x per symbol for smoother phase correction
			CCF_PHASE1 <= CCF_PHASE1 + AFC_NCO_CONTROL2;
		end if;
	end if;
end process;

-- phase ambiguity correction
CCF_PHASE2(31 downto 30) <= CCF_PHASE1(31 downto 30) + CCF_PHASE_OFFSET;
CCF_PHASE2(29 downto 0) <= CCF_PHASE1(29 downto 0);

-- monitoring
CARRIER_FREQUENCY_ERROR <= std_logic_vector(CF_NCO_CONTROL1b) when (COHERENT_DEMOD = '1')  else std_logic_vector(AFC_NCO_CONTROL2);


-- Design note: there is already an anti-aliasing filter (CIC + HBF) in RECEIVER1.VHD.
-- There should not be a need for additional anti-aliasing filter (TBC) before reducing sampling rate 
-- from >8* symbol rate to 4* symbol rate. Just be careful when setting the CIC decimation ratio R.


--//-- RESAMPLING AT 4*SYMBOL RATE ---------------------
NOMINAL_SYMBOL_RATEx4 <= NOMINAL_SYMBOL_RATE(29 downto 0) & "00";
ST_NCO_TIMING_CONTROLB <= ST_NCO_TIMING_CONTROL when (SIMULATION = '0') or (ST_NCO_TIMING_CONTROL(31) /= 'X') else (others => '0');
	-- prevent 'X' propagation during simulation
RESAMPLING31_001: RESAMPLING31 
GENERIC MAP(
	NBITS => 14,
	CLK_PERIOD => 32,	-- n/a no time tag used
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	DATA_I_IN => DATA2_I(17 downto 4),
	DATA_Q_IN => DATA2A_Q(17 downto 4),
	TT_IN => (others => '0'),	
	SAMPLE_CLK_IN => SAMPLE2_CLK,
	INPUT_SAMPLING_RATE => INPUT_SAMPLING_RATE,	
	NOMINAL_RESAMPLING_RATE => NOMINAL_SYMBOL_RATEx4,
	NCO_OFFSET => ST_NCO_TIMING_CONTROLB,
	NCO_OFFSET_SAMPLE_CLK => ST_NCO_TIMING_CONTROL_CLK,
	DATA_I_OUT => DATA3_I,
	DATA_Q_OUT => DATA3_Q,
	TT_OUT => open,
	SAMPLE_CLK_OUT => SYMBOL3_CLKx4
);

-- placeholder for a filter.  
-- Tested HBF, RRC without noticeable BER improvements
DATA4_I <= DATA3_I;
DATA4_Q <= DATA3_Q;
SYMBOL4_CLKx4 <= SYMBOL3_CLKx4;

-- arbitrarily select one sample as end of symbol (the symbol tracking loop will make sure it becomes true)
-- The symbol tracking loop will place the symbol center at cntr = 0 (maximum frequency)
-- Consequently, the symbol edges are at cntr = 2
SYMBOL4_CLK_GEN: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') then
			SAMPLE4_CNTR <= (others => '0');
		elsif(SYMBOL4_CLKx4 = '1') then
			SAMPLE4_CNTR <= SAMPLE4_CNTR + 1;	
				-- bits(1:0) = modulo-4 counter since 4 samples/symbol
				-- bit 2: toggles every bit period (needed for SOQPSK)
		end if;
	end if;
end process;
-- place symbol around the 45/135/225/315deg constellation (when coherent demod)
SYMBOL4_CLK <= '1' when (SYMBOL4_CLKx4 = '1') and (SAMPLE4_CNTR(1 downto 0) = "10")  else '0';
SYMBOL4_CLKx2 <= '1' when (SYMBOL4_CLKx4 = '1') and (SAMPLE4_CNTR(0) = '0')  else '0';

DATA4_Ix <= DATA4_I & "0000";
DATA4_Qx <= DATA4_Q & "0000";

--//-- FAST FREQUENCY ACQUISITION ---------------------------------------
-- Algorithm: despread input is **4 (quadruple the frequency and removes the 90deg step modulation) then
-- 2048-pt FFT  
--   frequency resolution is symbol_rate * 4/2048
--   frequency range: +/- symbol_rate
--   detection duration: 2048/4*symbol_rate
-- It is important to have a small resolution (<< 1% symbol rate) to ease the transition to the Costas loop.

FFD_SATURATION_IN <= SATURATION1 or SATURATION2;
SYNC_RESET_COHERENTDET <= SYNC_RESET when (COHERENT_DEMOD = '1') else '1';
	-- save power when non-coherent demod

-- The integration and dump within the FAST_FREQUENCY_DETECTION component should be synchronized with the symbol period.
-- It varies somewhat with the selected modulation scheme.
FFD_001: process(FILT_SEL,SYMBOL4_CLKx4,SAMPLE4_CNTR)
begin
	if(unsigned(FILT_SEL) = 5) or (unsigned(FILT_SEL) = 6) then
		-- SOQPSK
		if(SYMBOL4_CLKx4 = '1') and (SAMPLE4_CNTR(1 downto 0) = "11") then
			FFD_NID_SYNC_IN <= '1';
		else
			FFD_NID_SYNC_IN <= '0';
		end if;
	else
		-- other coherent modulation schemes
		if(SYMBOL4_CLKx4 = '1') and (SAMPLE4_CNTR(1 downto 0) = "01") then
			FFD_NID_SYNC_IN <= '1';
		else
			FFD_NID_SYNC_IN <= '0';
		end if;
	end if;
end process;

-- Note: adjusted the INPUT_SATURATION_CNTR threshold within (reduced from 100 to 25, heuristic)
FAST_FREQUENCY_DETECTION_001: FAST_FREQUENCY_DETECTION 
generic map(
	NID => 4,
	NID_LOG2 => 2,	-- need at least 3 CLKs between input samples (shared multipliers)
	SIMULATION => SIMULATION
)
port map(
	CLK => CLK,    
	SYNC_RESET => SYNC_RESET_COHERENTDET,
	DATA_I_IN => DATA4_Ix,
	DATA_Q_IN => DATA4_Qx,  
	SAMPLE_CLK_IN => SYMBOL4_CLKx4, 
	NID_SYNC_IN => FFD_NID_SYNC_IN,
	SATURATION_IN => FFD_SATURATION_IN,	
	ENABLE => FFD_ENABLE,	-- trigger. enable only during acquisition
	CONTROL => "10",	-- **4
	FFTM => FFTM,	
	FFTM2 => FFTM2,   
	FFTM_AVRG => FFTM_AVRG, 
	FFTF => FFTF,
	FFTP => FFTP,     -- unused
	FFT_READY => FFT_READY,
	SATURATION => FFD_SATURATION_OUT,    -- unused
	EXCESSIVE_NONLINEARITY => FFD_IGNORE_OUTPUT,
	TP => open
);      

-- compare FFTM and FFTM2 or FFTM_AVRG as reliable signal presence indicator (tweak to optimize Pd/Pfa at threshold SNR)
--FFTM_THRESHOLD(13 downto 2) <= FFTM_AVRG(11 downto 0);  -- 4* average (OK until about 2-3dB Eb/No)
FFTM_AVRGx4 <= FFTM_AVRG(11 downto 0) & "00";
FFTM_AVRGx0p5 <= "0" & FFTM_AVRG(13 downto 1);
FFTM_THRESHOLD <= unsigned(FFTM_AVRGx4) - unsigned(FFTM_AVRGx0p5);  -- 3.5* average (needed to bring the threshold to 1dB Eb/No)
SIGNAL_PRESENCE_001: process(CLK)
begin
	if rising_edge(CLK) then
	   FFT_READY_D <= FFT_READY;
		FFD_IGNORE_OUTPUT_D <= FFD_IGNORE_OUTPUT;
	   
	   if(SYNC_RESET = '1') then
	       SIGNAL_PRESENT_local <= '0';
	   elsif(FFT_READY = '1') and (FFD_IGNORE_OUTPUT = '0') then
--	       if(( FFTM2(13) = '0') and (FFTM(13 downto 1) > FFTM2(12 downto 0))) or 
--	       (FFTM(13 downto 2) > FFTM_THRESHOLD(13 downto 2)) then
--	       -- when FFT peak is larger than 2*second peak
--	       -- or FFT peak > X average FFT magnitude
-- test test test   the original code above is not very good at detecting loss of lock (no inpput signal FFTM=4,FFTM2=0,FFTMAVR=4)  3/27/15
	       if(unsigned(FFTM) > FFTM_THRESHOLD) then
	           -- when FFT peak > X average FFT magnitude
	           SIGNAL_PRESENT_local <= '1';
	       else
	           SIGNAL_PRESENT_local <= '0';
	       end if;
		end if;
    end if;
end process;
SIGNAL_PRESENT_OUT <= SIGNAL_PRESENT_local;

-- signal presence confidence level (flywheel to operate at lower SNR)
-- up/down counter +4/-1  Range [0-11]
-- A signal absence will be declared after 4-11 successive FFTs with SIGNAL_PRESENT_local = 0
SIGNAL_PRESENCE_002: process(CLK)
begin
	if rising_edge(CLK) then
	   if(SYNC_RESET = '1') then
	       SIGNAL_PRESENCE_CONFIDENCE <= (others => '0');
	   elsif(FFT_READY_D = '1') then
	       if (SIGNAL_PRESENT_local = '1') and (SIGNAL_PRESENCE_CONFIDENCE(SIGNAL_PRESENCE_CONFIDENCE'left) = '0') then
	           SIGNAL_PRESENCE_CONFIDENCE <= SIGNAL_PRESENCE_CONFIDENCE + 4;
	       elsif(SIGNAL_PRESENCE_CONFIDENCE > 0) then
	           SIGNAL_PRESENCE_CONFIDENCE <= SIGNAL_PRESENCE_CONFIDENCE - 1;
	       end if;
		end if;
    end if;
end process;
SIGNAL_ABSENT <= '1' when (SIGNAL_PRESENCE_CONFIDENCE = 0) else '0';

-- FFT measures the frequency error. Units are symbol rate/1024.
-- remember to invert sign. 
-- Measurement may (or may not) continue during the tracking phase. 
FREQ_CORRECTION_GEN_001: process(CLK)
begin
	if rising_edge(CLK) then
		if (SYNC_RESET = '1') or (STATE = 0) then
			-- clear frequency error
			FFT_FREQ_ERROR <= (others => '0');
		elsif(FFT_READY = '1') and (FFD_IGNORE_OUTPUT = '0') then
			-- extend to 32 bits NCO
			-- x4 oversampling 
		  FFT_FREQ_ERROR(31 downto 30) <= (others => FFTF(10));
		  FFT_FREQ_ERROR(29 downto 19) <= FFTF;
		  FFT_FREQ_ERROR(18 downto 0) <= (others => '0');    
		end if;
	end if;
end process;

FFT_FREQ_CORRECTION <= - signed(FFT_FREQ_ERROR);	-- relative to symbol rate


--//-- MATCHED FILTERS ------------------------
-- Coherent case (h=0.5): 
-- collect 4 samples in a symbol. Derotate by steps of 90deg/4. Sum the (almost) phase aligned samples.
-- Perform the above computation for the two symbol hypotheses '0' and '1'
-- Rotate the 2 sums by 0,90,180,270 deg. Place the 8 results in the branch metrics, for use in the follow-on Viterbi algorithm.

-- Coherent case (h=0.5) SOQPSK
-- collect 4 samples in a symbol. Derotate by steps of 90deg/4. Sum the (almost) phase aligned samples.
-- Perform the above computation for the three alpha hypotheses -1,0,+1
-- Rotate the 3 sums by 0,90,180,270 deg. Place the 12 results in the branch metrics, for use in the follow-on Viterbi algorithm.

-- Non-coherent case (h != 0.5):
-- For each estimated symbol sequence over L (M^L cases), 
-- we compute a real value as follows
-- Step 1: for each L-symbols hypothesis (there are M^L), look-up the 4 phase shift samples over a symbol T
-- Step 2: for each L-symbols hypothesis (there are M^L), derotate the 4 received complex samples and sum.
-- thus producing M^L complex matched filter outputs.
-- In BM_xyz   x is the bit hypothesis, yz is the initial phase hypothesis
-- current implementation: M=2, L=1, P = 4

MF_001: CPM_MF
PORT MAP(
	CLK => CLK,
	SYNC_RESET => SYNC_RESET,
	DATA_I_IN => DATA4_I(13 downto 14-CPM_MF_NBITS),
	DATA_Q_IN => DATA4_Q(13 downto 14-CPM_MF_NBITS),
	SAMPLE_CLK_IN => SYMBOL4_CLKx4,
	SAMPLE_CNTR_IN => std_logic_vector(SAMPLE4_CNTR(1 downto 0)),   -- 00 = symbol center when symbol timing loop is properly tracking
	M_SEL => M_SEL,
	FILT_SEL => FILT_SEL,
	MODULATION_INDEX => MODULATION_INDEX,
	MODULATION_INDEX_VALID => '0',	-- TODO convert to a pulse when modulation index changes
	SYMBOL_CLK_OUT => BM_MF_SYMBOL_CLK,	-- SYMBOL4_CLK_D7
	BM_OUT => BM_MF,	-- branch metrics
	MF_I_OUT => MF_I,
	MF_Q_OUT => MF_Q,
	MF_SYMBOL_CLKx2_OUT => MF_SYMBOL_CLKx2,  -- SYMBOL4_CLKx2_D7
	MF_SYMBOL_CLK_OUT => MF_SYMBOL_CLK,  -- SYMBOL4_CLK_D7
	COS_PIH => COS_PIH,
	SIN_PIH => SIN_PIH,
	MF_INITIALIZED => open
);

-- delay matched filters outputs by 9 symbol periods until the VA algorithm finds out the best phase state
DELAY2_INPUTS: process(MF_I,MF_Q)
begin
	for I in 0 to (MPOWERLMAX-1) loop
		MF_DATA1_I(((CPM_MF_NBITS*(I+1)) -1) downto CPM_MF_NBITS*I) <= MF_I(I);
		MF_DATA1_Q(((CPM_MF_NBITS*(I+1)) -1) downto CPM_MF_NBITS*I) <= MF_Q(I);
	end loop;
end process;

DELAY2_001: DELAY2 
GENERIC MAP(
	ADDR_WIDTH => 5,	-- max delay 32 samples
	DATA_WIDTH => MF_DATA1_I'length,	
	NSAMPLES_DELAY => 18	-- 18 1/2 bits latency
)
PORT MAP(
	CLK => CLK,
	DATA_IN => MF_DATA1_I,
	SAMPLE_CLK_IN => MF_SYMBOL_CLKx2,
	DATA_OUT => MF_DATA2_I,
	SAMPLE_CLK_OUT => MF2_SYMBOL_CLKx2
);

DELAY2_002: DELAY2 
GENERIC MAP(
	ADDR_WIDTH => 5,	-- max delay 32 samples
	DATA_WIDTH => MF_DATA1_Q'length,	
	NSAMPLES_DELAY => 18	-- 18 1/2 bits latency
)
PORT MAP(
	CLK => CLK,
	DATA_IN => MF_DATA1_Q,
	SAMPLE_CLK_IN => MF_SYMBOL_CLKx2,
	DATA_OUT => MF_DATA2_Q,
	SAMPLE_CLK_OUT => open
);

DELAY2_OUTPUTS: process(MF_DATA2_I,MF_DATA2_Q)
begin
	for I in 0 to (MPOWERLMAX-1) loop
		MF2_I(I) <= MF_DATA2_I((CPM_MF_NBITS*(I+1)) -1 downto CPM_MF_NBITS*I);
		MF2_Q(I) <= MF_DATA2_Q((CPM_MF_NBITS*(I+1)) -1 downto CPM_MF_NBITS*I);
	end loop;
end process;

DELAY2_003: process(CLK)
begin
	if rising_edge(CLK) then
		MF_SYMBOL_CLK_D <= MF_SYMBOL_CLK;
		MF2_SYMBOL_CLK <= MF_SYMBOL_CLK_D;
	end if;
end process;

--//-- BEST MF OUTPUT ------------------------------
-- delay all MF outputs another 5 CLKs before picking the best
-- Objective: trying to minimize the number of FFs to perform the delay
BEST_MF_SEL_001: process(CLK)
begin
	if rising_edge(CLK) then
		MF_SYMBOL_CLK_SHIFT <= MF_SYMBOL_CLK_SHIFT(8 downto 1) & MF2_SYMBOL_CLK;
		MF_SYMBOL_CLKx2_SHIFT <= MF_SYMBOL_CLKx2_SHIFT(8 downto 1) & MF2_SYMBOL_CLKx2;
		
		-- center
		if(MF_SYMBOL_CLK_SHIFT(1) = '1') then	-- quickly before the next 1/2 symbol
			MF_I_D <= MF2_I;							-- valid from MF_SYMBOL_CLK_D2 to MF_SYMBOL_CLK_D5
			MF_Q_D <= MF2_Q;
		end if;

		-- early/late
		if(MF_SYMBOL_CLKx2_SHIFT(1) = '1') and (MF_SYMBOL_CLK_SHIFT(1) = '0') then -- quickly before the next 1/2 symbol
			MF_E_I <= MF2_I;							-- valid from MF_SYMBOL_CLKx2_D2 to MF_SYMBOL_CLKx2_D5
			MF_E_Q <= MF2_Q;
			MF_L_I <= MF_E_I;
			MF_L_Q <= MF_E_Q;
		end if;
	end if;
end process;

	
BEST_MF_SEL_003: process(CLK)
begin
	if rising_edge(CLK) then
		-- upon receiving the center MF
		if(MF_SYMBOL_CLKx2_SHIFT(5) = '1') and (MF_SYMBOL_CLK_SHIFT(5) = '1') then	
			BEST_MF_I <= signed(MF_I_D(BEST_MF_INDEX));
			BEST_MF_Q <= signed(MF_Q_D(BEST_MF_INDEX));
		end if;
		-- upon receiving the early MF, collect the triplet late/center/early
		if(MF_SYMBOL_CLKx2_SHIFT(5) = '1') and (MF_SYMBOL_CLK_SHIFT(5) = '0') then	
			BEST_MF_E_I <= signed(MF_E_I(BEST_MF_INDEX));
			BEST_MF_E_Q <= signed(MF_E_Q(BEST_MF_INDEX));
			BEST_MF_L_I <= signed(MF_L_I(BEST_MF_INDEX));
			BEST_MF_L_Q <= signed(MF_L_Q(BEST_MF_INDEX));
			BEST_MF_VALID <= '1';
		else
			BEST_MF_VALID <= '0';
		end if;
	end if;
end process;

--//-- MULTI-SYMBOL DETECTION ------------------------------
-- This non-coherent detection method is to be used whenever the modulation index varies (for example PCM/FM with h = 0.5 to 0.8
-- For modulation indices associated with a fixed phase constellation of 2pi/n (pi/2 here), use coherent detection in conjunction with 
-- the Viterbi algorithm instead for better BER performance.
-- For 3 consecutive symbols (8 combinations = 8 trellis states), rotate the 3 matched filter outputs, sum coherently and compute magnitude.
-- The resulting magnitude will serve as branch metric for the follow-on viterbi algorithm.

SYNC_RESET_NONCOHERENTDET <= SYNC_RESET when (COHERENT_DEMOD = '0') else '1';
	-- save power when coherent demod
MSD3_L <= "10" when (unsigned(FILT_SEL) = 1) else "01";

-- Latency: 17 CLK 
MSD3_001: CPM_MSD3
GENERIC MAP(
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	SYNC_RESET => SYNC_RESET_NONCOHERENTDET,
	MF_I_IN => MF_I,
	MF_Q_IN => MF_Q,
	MF_SYMBOL_CLK_IN => MF_SYMBOL_CLK,  -- SYMBOL4_CLK_D7
	L => MSD3_L,	-- L = 1 or 2 only
	COS_PIH => COS_PIH,
	SIN_PIH => SIN_PIH,
	SYMBOL_CLK_OUT => BM_MSD3_SYMBOL_CLK,	
	BM_OUT => BM_MSD3	-- branch metrics
);

	
--//-- TRELLIS DECODING ------------------------------
-- select branch metrics from the multi-symbol detector (non-coherent case) or from the matched filter (coherent [h=0.5] case)
BM <=  BM_MSD3 when (COHERENT_DEMOD = '0')  else BM_MF;
BM_SYMBOL_CLK <= BM_MSD3_SYMBOL_CLK  when (COHERENT_DEMOD = '0') else BM_MF_SYMBOL_CLK;
TRELLIS_TYPE <= "10" when ((unsigned(FILT_SEL) = 5) or (unsigned(FILT_SEL) = 6)) and (COHERENT_DEMOD = '1') else -- SOQPSK
					 "01" when (unsigned(FILT_SEL) = 1) and (COHERENT_DEMOD = '0') else -- PCM/FM
					 "00" when (COHERENT_DEMOD = '1') else
					 "11";	
			-- 0 = coherent demodulation,  4-state trellis (MSK,GMSK)
			-- 1 = non-coherent demodulation,  4-state trellis (PCM-FM)
			-- 2 = coherent demodulation,  8-state trellis for SOQPSK-MIL
			-- 3 = non-coherent demodulation, 2-state trellis (FSK,GFSK)

CPM_VA_PMU_001: CPM_VA_PMU 
GENERIC MAP(
	NBITS => CPM_MF_NBITS,	-- matched filter real output precision
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	SYNC_RESET => SYNC_RESET,
	BM => BM,
	BM_SAMPLE_CLK_IN => BM_SYMBOL_CLK,
	TRELLIS_TYPE => TRELLIS_TYPE,
	SAMPLE_CLK_OUT => BM_SYMBOL_CLK_D7,	
	DATA_OUT => DATA6(3),			-- available at BM_SYMBOL_CLK_D7				
	RELIABILITY_OUT => VA_RELIABILITY,		-- aligned with BM_SYMBOL_CLK_D7
	BEST_MF_INDEX => BEST_MF_INDEX,			-- aligned with BM_SYMBOL_CLK_D7 + 9 symbols delay in traceback
	BEST_TB_STATE => BEST_TB_STATE		-- aligned with BM_SYMBOL_CLK_D7 + 9 symbols delay in traceback
);
SAMPLE6_CLK <= BM_SYMBOL_CLK_D7;

SOVA_1: if(SOVA = '1') generate
	DATA6(2 downto 0) <= VA_RELIABILITY when (DATA6(3) = '1') else not VA_RELIABILITY;
		-- 4 bit soft-quantized demodulator output format:
		-- 0000 most negative, 1111 most positive, with no zero representation 
end generate;
SOVA_0: if(SOVA = '0') generate
	DATA6(2 downto 0) <= (others => DATA6(3));
end generate;



-- delay DATA6 to align DATA_OUT with OUTPUT_ENABLE
DELAY_OUTPUT_001: FIFO 
GENERIC MAP(
	 DATA_WIDTH => 4,        
	 DEPTH => 4
)
PORT MAP(
	CLK => CLK,
	DATA_IN => DATA6,	
	DATA_IN_VALID => SAMPLE6_CLK,
	DATA_OUT => DATA7,
	DATA_OUT_VALID => SAMPLE7_CLK
);

DELAY_OUTPUT_002: process(CLK)
begin
	if rising_edge(CLK) then
		DATA7_D <=  DATA7;
		SAMPLE7_CLK_D <= SAMPLE7_CLK;
	end if;
end process;

DATA_OUT <=  DATA7_D;
SAMPLE_CLK_OUT <= SAMPLE7_CLK_D and OUTPUT_ENABLE;

--// SYNC DETECTION (SOQPSK only) -----------------------------------------
-- Applicable only if a synchronization pattern is transmitted
-- Detects a synchronization pattern, then corrects for any phase ambiguity in the demodulated SOQPSK data.

-- Detect periodic synchronization sequence when demodulator is locked at 0/180 deg offset w.r.t. tx signal
MF001: MATCHED_FILTERNx1 
	generic map( 
		SYNC_LENGTH => 32,
		SYNC_WORD => x"5A0FBE66000000000000000000000000",
		NSAMPLES_PER_BIT => 1
	)
	port map(
	CLK => CLK, 
	DATA_IN => DATA6(3), 
	SAMPLE_CLK_IN => SAMPLE6_CLK,
	FLIPPED => '0',
	DETECT => SYNC_DETECT0,	-- aligned with SAMPLE_CLK_IN_D,MF_SAMPLE_CLK
	MF_OUT => open,	-- unused
	MF_OUT_READY => SYNC_DETECT_READY,
	SIGN_OUT => SYNC_DETECT_SIGN0
	);      

-- Detect periodic synchronization sequence when demodulator is locked at 90 deg offset w.r.t. tx signal
--MF002: MATCHED_FILTERNx1 
--	generic map( 
--		SYNC_LENGTH => 32,
--		SYNC_WORD => x"AD0F7D99000000000000000000000000",
--		NSAMPLES_PER_BIT => 1
--	)
--	port map(
--	CLK => CLK, 
--	DATA_IN => DATA6(3), 
--	SAMPLE_CLK_IN => SAMPLE6_CLK,
--	FLIPPED => '0',
--	DETECT => SYNC_DETECT90,	-- aligned with SAMPLE_CLK_IN_D,MF_SAMPLE_CLK
--	MF_OUT => open,	-- unused
--	MF_OUT_READY => open,
--	SIGN_OUT => SYNC_DETECT_SIGN90
--	);      

-- Detect periodic synchronization sequence when demodulator is locked at 270 deg offset w.r.t. tx signal
MF003: MATCHED_FILTERNx1 
	generic map( 
		SYNC_LENGTH => 32,
		SYNC_WORD => x"0F5AEB33000000000000000000000000",
		NSAMPLES_PER_BIT => 1
	)
	port map(
	CLK => CLK, 
	DATA_IN => DATA6(3), 
	SAMPLE_CLK_IN => SAMPLE6_CLK,
	FLIPPED => '0',
	DETECT => SYNC_DETECT270,	-- aligned with SAMPLE_CLK_IN_D,MF_SAMPLE_CLK
	MF_OUT => open,	-- unused
	MF_OUT_READY => open,
	SIGN_OUT => SYNC_DETECT_SIGN270
	);      

SYNC_DETECT <= SYNC_DETECT_READY and (SYNC_DETECT0 or SYNC_DETECT90 or SYNC_DETECT270);

BIT_CNTR_INC <= BIT_CNTR + 1;
SYNC_CONFIDENCE_GEN: process(CLK)
begin
	if rising_edge(CLK) then
		if (SYNC_RESET = '1') or (CONTROL(3) = '0') or (STATE = 0) then
			SYNC_CONFIDENCE <= (others => '0');
			BIT_CNTR <= (others => '0');
		elsif(SYNC_DETECT = '1') and (SYNC_CONFIDENCE = 0) then
			-- goes to SYNC_CONFIDENCE=1 upon first sync (any ambiguity)
			SYNC_CONFIDENCE <= SYNC_CONFIDENCE + 1;
			BIT_CNTR <= (others => '0');
		elsif(SYNC_DETECT_READY = '1') and (BIT_CNTR_INC = unsigned(FRAME_LENGTH)) then
			BIT_CNTR <= (others => '0');
			if(SYNC_DETECT0 = '1') and (SYNC_DETECT_SIGN0 = '0') and (SYNC_CONFIDENCE < 7) then
				-- increases upon follow-on syncs at 0deg (ambiguity resolved)
				SYNC_CONFIDENCE <= SYNC_CONFIDENCE + 1;
			elsif((SYNC_DETECT0 = '0') or (SYNC_DETECT_SIGN0 = '1')) and (SYNC_CONFIDENCE > 0) then
				SYNC_CONFIDENCE <= SYNC_CONFIDENCE - 1;
			end if;
		elsif(SYNC_DETECT_READY = '1') then
			BIT_CNTR <= BIT_CNTR_INC;
		end if;
	end if;
end process;

-- enable the output when BIT_CNTR < FRAME_LENGTH - 32
OUTPUT_ENABLE_001: process(CLK)
begin
	if rising_edge(CLK) then
		if (CONTROL(3) = '0') then
			-- no sync word detection/removal
			OUTPUT_ENABLE <= '1';
		elsif (SYNC_RESET = '1') or (STATE = 0) then
			OUTPUT_ENABLE <= '0';
		elsif((SYNC_DETECT = '1') and (SYNC_CONFIDENCE = 0)) or
		((SYNC_DETECT_READY = '1') and (BIT_CNTR_INC = unsigned(FRAME_LENGTH))) then
			-- goes to SYNC_CONFIDENCE=1 upon first sync (any ambiguity)
			OUTPUT_ENABLE <= '1';
		elsif(SYNC_DETECT_READY = '1') and (BIT_CNTR_INC = unsigned(FRAME_LENGTH) - 32) then
			OUTPUT_ENABLE <= '0';
		end if;
	end if;
end process;


CCF_PHASE_OFFSET_GEN: process(CLK)
begin
	if rising_edge(CLK) then
		if (SYNC_RESET = '1') or (CONTROL(3) = '0') or (STATE = 0) then
			CCF_PHASE_OFFSET <= "00";
		elsif(SYNC_DETECT = '1') and (SYNC_CONFIDENCE = 0) then
			if(SYNC_DETECT0 = '1') and (SYNC_DETECT_SIGN0 = '1') then
				-- 180 phase change
				CCF_PHASE_OFFSET <= CCF_PHASE_OFFSET + "10";
--			elsif(SYNC_DETECT90 = '1') then
--				if(SYNC_DETECT_SIGN90 = '0') then
--					CCF_PHASE_OFFSET <= CCF_PHASE_OFFSET + "11";
--				else
--					CCF_PHASE_OFFSET <= CCF_PHASE_OFFSET + "01";
--				end if;
			elsif(SYNC_DETECT270 = '1') then
				if(SYNC_DETECT_SIGN270 = '0') then
					CCF_PHASE_OFFSET <= CCF_PHASE_OFFSET + "01";
				else
					CCF_PHASE_OFFSET <= CCF_PHASE_OFFSET + "11";
				end if;
			end if;
		end if;
	end if;
end process;

--//-- AFC ------------------------------
-- First convert complex baseband samples to phase difference between successive symbols
-- Delay samples by one symbol, and de-rotate the earlier one with the following symbol.
-- The resulting I axis shows the phase difference (looks like bits). 
-- See symbol tracking (common to both algorithms)
--
AFC_EN <= CONTROL(1);
AFC_SYNC_RESET <= SYNC_RESET or not AFC_EN;
AFC2_001: AFC2
GENERIC MAP(
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	SYNC_RESET => AFC_SYNC_RESET,
	FREQ_IN => FREQ2,
--	SAMPLE_CLK_IN => SYMBOL4_CLKx4_D3,	-- ONCE OR 4X/SYMBOL???? TBC
	SAMPLE_CLK_IN => SYMBOL4_CLKx2_D3,	-- ONCE OR TWICE per symbol TBC
	FREEZE => CONTROL(2),	
	NCO_CONTROL => AFC_NCO_CONTROL,		
	NCO_CONTROL_CLK => AFC_NCO_CONTROL_CLK,
	AFC_LOCK => AFC_LOCK
);
AFC_LOCK_OUT <= AFC_LOCK;


-- Watch out: the sign depends on whether the spectrum inversion is enabled or not (CONTROL(0))
-- x4 loop gain (NCO is updated 4 times per symbol)
AFC2_002: process(CLK)
begin
	if rising_edge(CLK) then
		AFC_NCO_CONTROL2_CLK <= AFC_NCO_CONTROL_CLK;
		
		-- Correct sign depending on spectrum inversion or not
		if(AFC_EN = '0') or ((SIMULATION = '1') and (AFC_NCO_CONTROL(31) = 'X'))  then
			-- prevents X propagation when simulation
			AFC_NCO_CONTROL2 <= (others => '0');
		elsif(CONTROL(0) = '0') then
			-- no spectrum inversion
			AFC_NCO_CONTROL2 <= signed(AFC_NCO_CONTROL);	
		else
			-- spectrum inversion
			AFC_NCO_CONTROL2 <= -signed(AFC_NCO_CONTROL);	
		end if;
	end if;
end process;

--//-- CARRIER TRACKING ------------------------------
-- At the symbol center, estimate the phase error (coherent demodulation valid only when h=0.5)
-- Although PE is an approximation, we scale it such that it roughly matches a 11 bit phase (2047 = 360 deg)

CT_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(COHERENT_DEMOD = '1') and (BEST_MF_VALID = '1') then
			-- best MF output
			-- constellation is naturally 0/90/180/270 (when coherent demod is tracking) 
			PE_SYMBOL_CLK <= '1';
			
			case(BEST_TB_STATE(1 downto 0)) is
				when "00" => PE <= resize(signed(BEST_MF_Q),12);	-- best state is 0
				when "01" => PE <= resize(-signed(BEST_MF_I),12);	-- best state is 90
				when "10" => PE <= resize(-signed(BEST_MF_Q),12);	-- best state is 180
				when others => PE <= resize(signed(BEST_MF_I),12);	-- best state is 270
			end case;
		else
			PE_SYMBOL_CLK <= '0';
		end if;
--		else
--			-- other modulations. Constellation is assessed at the center of the symbol period.
--			if(SYMBOL4_CLKx4 = '1') and (SAMPLE4_CNTR(1 downto 0) = "00") then
--				-- middle of a symbol phase transition
--				-- DATA4 constellation is naturally 45/135/225/315deg (when coherent demod is tracking) 
--				PE_SYMBOL_CLK <= '1';
--
--				-- presumed optimum sampling instant
--				-- compute phase error as for QPSK: Q*sign(I) - I*sign(Q)
--				-- When no noise, QPSK and AGC stable, PE full range is approximately +/- 400 (targeting +/-512)
--				if(DATA4_I(13) = '0') and (DATA4_Q(13) = '0') then
--					PE <= resize(signed(DATA4_Q(13 downto 3)),12) - resize(signed(DATA4_I(13 downto 3)),12);
--				elsif(DATA4_I(13) = '0') and (DATA4_Q(13) = '1') then
--					PE <= resize(signed(DATA4_Q(13 downto 3)),12) + resize(signed(DATA4_I(13 downto 3)),12);
--				elsif(DATA4_I(13) = '1') and (DATA4_Q(13) = '0') then
--					PE <= - resize(signed(DATA4_Q(13 downto 3)),12) - resize(signed(DATA4_I(13 downto 3)),12) ;
--				else
--					PE <= - resize(signed(DATA4_Q(13 downto 3)),12) + resize(signed(DATA4_I(13 downto 3)),12);
--				end if;
--			else
--				PE_SYMBOL_CLK <= '0';
--			end if;
		
	end if;
end process;

-- accumulate phase error (debug)
CT_002: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') or ((SIMULATION = '1') and (PE(0) = 'X')) then
			PE_ACC <= (others => '0');
		elsif(PE_SYMBOL_CLK = '1') then
			PE_ACC <= PE_ACC + resize(PE,PE_ACC'length);
		end if;
	end if;
end process;

CT_SYNC_RESET <= '1' when (STATE = 0) or (COHERENT_DEMOD = '0') else '0';	-- save power when non-coherent demod
CT_ACCELERATED_CT_RESPONSE <= '0';
CT_LOCK_THRESHOLD <= "0111";	-- 30 deg rms threshold
CT_003: CARRIER_TRACKING 
GENERIC MAP(
	SIMULATION => SIMULATION
)
port map (
	CLK => CLK,
	SYNC_RESET => CT_SYNC_RESET,
	OPTIMUM_SAMPLING_CLK => PE_SYMBOL_CLK,	-- SYMBOL4_CLK_D14
	PHASE_ERROR => std_logic_vector(PE(11 downto 1)),    -- expected units: 2048 = 360 deg, 
	LOOP_GAIN => "00",	-- ignored
 	ACCELERATED_CT_RESPONSE => CT_ACCELERATED_CT_RESPONSE,	-- accelerate convergence during acquisition.
	LOCK_THRESHOLD => CT_LOCK_THRESHOLD,
	NCO_CONTROL => RESIDUAL_FREQUENCY_OFFSET,		
	NCO_CONTROL_CLK => RESIDUAL_FREQUENCY_OFFSET_CLK,	-- SYMBOL4_CLK_D16
	CARRIER_LOCK => CARRIER_LOCK_local,
	CS1 => open,
	CS1_CLK => open,
	CS2 => open,
	CS2_CLK => open,
	CS3 => open,
	CS3_CLK => open
);

-- Watch out: the sign depends on whether the spectrum inversion is enabled or not (CONTROL(0))
CT_004: process(CLK)
begin
	if rising_edge(CLK) then
	   CF_NCO_CONTROL_SAMPLE_CLK <= RESIDUAL_FREQUENCY_OFFSET_CLK;	-- once per symbol. -- SYMBOL4_CLK_D17
	   
		-- Correct sign depending on spectrum inversion or not
		if(CT_SYNC_RESET = '1') or 
		((SIMULATION = '1') and (RESIDUAL_FREQUENCY_OFFSET(19) /= '0') and (RESIDUAL_FREQUENCY_OFFSET(19) /= '1')) then
			-- prevents X propagation when simulation
			CF_NCO_CONTROL <= (others => '0');
		elsif(CONTROL(0) = '0') then
			-- no spectrum inversion
			CF_NCO_CONTROL(31 downto 12) <= -signed(RESIDUAL_FREQUENCY_OFFSET);	
		else
			-- spectrum inversion
			CF_NCO_CONTROL(31 downto 12) <= signed(RESIDUAL_FREQUENCY_OFFSET);	
		end if;
	end if;
end process;

-- monitoring
CARRIER_LOCK_OUT <= CARRIER_LOCK_local;

--//-- SYMBOL TIMING ERROR (1/2) ------------------------
-- First convert complex baseband samples to phase difference between successive symbols
-- Delay samples by one symbol, and de-rotate the earlier one with the following symbol.
-- The resulting I axis shows the phase difference (looks like bits). Only then we can use 
-- the Gardner algorithm
-- 1st order loop capable of tracking up to +/- 50ppm at SNR> 0dB (verified 8/26/17)

-- compute the phase difference over 1/2 symbol 
-- method: de-rotate one complex baseband sample using the next complex baseband sample. Keep Q component (proportional to phase difference).
ST_DELAY_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYMBOL4_CLKx4 = '1') then
			DATA4_I_D <= DATA4_I;
			DATA4_Q_D <= DATA4_Q;
		end if;
	end if;
end process;

MULT41 <= signed(DATA4_Q) * signed(DATA4_I_D);
MULT42 <= signed(DATA4_I) * signed(DATA4_Q_D);

MULT_OUTPUT_MUX_001: process(CLK)
begin
	if rising_edge(CLK) then
		SYMBOL4_CLKx4_D <= SYMBOL4_CLKx4;
		SYMBOL4_CLKx4_D2 <= SYMBOL4_CLKx4_D;
		SAMPLE4_CNTR_D <= SAMPLE4_CNTR;
		SAMPLE4_CNTR_D2 <= SAMPLE4_CNTR_D;
		
		-- reclock multiplier output for better timing
		if(SYMBOL4_CLKx4 = '1') then
			MULT41_D <= MULT41;
			MULT42_D <= MULT42;
		end if;
		
		if(SYMBOL4_CLKx4_D = '1') then
			-- frequency is phase difference over a symbol.
			FREQ1 <= MULT41_D - MULT42_D;
		end if;
	end if;
end process;

-- re-center ST loop input for full dynamic range
ST_LOOP_001: process(CLK)
begin
	if rising_edge(CLK) then
		SYMBOL4_CLKx4_D3 <= SYMBOL4_CLKx4_D2;
		SAMPLE4_CNTR_D3 <= SAMPLE4_CNTR_D2;
		SYMBOL4_CLKx2_D3 <= SYMBOL4_CLKx4_D2 and (not SAMPLE4_CNTR_D2(0));

		if(SYMBOL4_CLKx4_D2 = '1') then
			if(FREQ1(27) = '0') and (FREQ1(26 downto 24) /= "000") then
				-- positive clamping
				FREQ2 <= (17 => '0', others => '1');
			elsif(FREQ1(27) = '1') and (not FREQ1(26 downto 24) /= "000") then
				-- negative clamping
				FREQ2 <= (17 => '1', 0 => '1', others => '0');
			else
				FREQ2 <= std_logic_vector(FREQ1(24 downto 7));
			end if;
		end if;
	end if;
end process;

-- CNTR=0 indicates the symbol center = maximum frequency
ST_SYMBOL_CLK_IN <= 	SYMBOL4_CLKx4_D3 when (unsigned(SAMPLE4_CNTR_D3(1 downto 0)) = 0) else '0';
ST_SYMBOL_CLKx2_IN <= SYMBOL4_CLKx4_D3 when (SAMPLE4_CNTR_D3(0) = '0')  else '0';
ST_DATA_IN <= 	FREQ2;


-- tweak the ST loop gain to achieve target acquisition and tracking range. 
-- May change depending on the modulation type.
ST_LOOP_GAIN <= x"02";	-- x4 loop gain for PCM/FM, tested with 100ppm

-- ST loop controls the resampling NCO phase so that SYMBOL4_CLK_D4 is at peak frequency FREQ2,
ST_SYNC_RESET <= '1' when (STATE = 0)  else SYNC_RESET;	

ST_LOOP_003: SYMBOL_TIMING_LOOP5 
GENERIC MAP(
	SIMULATION => SIMULATION
)
PORT MAP(
	CLK => CLK,
	SYNC_RESET => ST_SYNC_RESET,
	SYMBOL_CLK_IN => ST_SYMBOL_CLK_IN,
	SYMBOL_CLKx2_IN => ST_SYMBOL_CLKx2_IN,
	DATA_IN => ST_DATA_IN,
	FREEZE_SYMBOL_TIMING => '0',
	LOOP_GAIN => ST_LOOP_GAIN,	
	NCO_TIMING_CONTROL => ST_NCO_TIMING_CONTROL1,
	NCO_TIMING_CONTROL_CLK => ST_NCO_TIMING_CONTROL1_CLK,
	CS1 => open,
	CS1_CLK => open
);

--//-- SYMBOL TIMING ERROR (2/2) ------------------------
-- An alternative method to measure the symbol timing error is to compute the matched filters twice per symbol.
-- Based on the SOVA, select the best matched filter output (decision-directed)

ST_001: process(CLK)
begin
	if rising_edge(CLK) then
		if(COHERENT_DEMOD = '1') then
			if(BEST_MF_VALID = '1') then
				-- best MF output
				-- constellation is naturally 0/90/180/270 (when coherent demod is tracking) 
				STE_VALID <= '1';

				if(BEST_TB_STATE(1 downto 0) = "00") then
					-- start state is 0 deg
					STE <= resize(signed(BEST_MF_L_I),12) + resize(-signed(BEST_MF_E_I),12);
				elsif(BEST_TB_STATE(1 downto 0) = "01") then
					-- start state is 90 deg
					STE <= resize(signed(BEST_MF_L_Q),12) + resize(-signed(BEST_MF_E_Q),12);
				elsif(BEST_TB_STATE(1 downto 0) = "10") then
					-- start state is 180 deg
					STE <= resize(-signed(BEST_MF_L_I),12) + resize(signed(BEST_MF_E_I),12);
				else
					-- start state is 270 deg
					STE <= resize(-signed(BEST_MF_L_Q),12) + resize(signed(BEST_MF_E_Q),12);
				end if;
			else
				STE_VALID <= '0';
			end if;
		end if;
	end if;
end process;

-- adjust symbol tracking loop gain here
STE_slv <= std_logic_vector(resize(STE,32));
ST_NCO_TIMING_CONTROL2 <= STE_slv(14 downto 0) & "00000000000000000";	
ST_NCO_TIMING_CONTROL2_CLK <= STE_VALID;

-- select which ST loop, since different errors were computed for coherent and non-coherent case.
ST_NCO_TIMING_CONTROL <= ST_NCO_TIMING_CONTROL1 when (COHERENT_DEMOD = '0') else ST_NCO_TIMING_CONTROL2;
ST_NCO_TIMING_CONTROL_CLK <= ST_NCO_TIMING_CONTROL1_CLK when (COHERENT_DEMOD = '0') else ST_NCO_TIMING_CONTROL2_CLK;

-- accumulate symbol timing error (debug)
ST_002: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') or ((SIMULATION = '1') and (ST_NCO_TIMING_CONTROL(31) = 'X')) then
			STE_ACC <= (others => '0');
		elsif(ST_NCO_TIMING_CONTROL_CLK = '1') then
			STE_ACC <= STE_ACC +  signed(ST_NCO_TIMING_CONTROL);
		end if;
	end if;
end process;




--// MONITORING, TEST POINTS --------------------------------------------
-- 1: AFC lock (abs(residual frequency error) < symbol rate/16)
-- 2: signal presence
-- 3: recovered carrier
-- 4: recovered bit timing
-- 5: sync detect (SOQPSK)
TP(1) <= AFC_LOCK;
TP(2) <= '0' when (SIGNAL_PRESENCE_CONFIDENCE = 0) else '1';
TP(3) <= CCF_PHASE1(31);
TP(4) <= SYMBOL4_CLK;
TP(5) <= SYNC_DETECT;
TP(6) <= '1' when (SYNC_DETECT = '1') and ((SYNC_CONFIDENCE = 0) or (BIT_CNTR_INC = unsigned(FRAME_LENGTH)))   else '0';
TP(7) <= SYNC_DETECT270;
TP(8) <= STE_ACC(31);


-- CS1: I channel after AGC, frequency translation to baseband, resampling at 4 samples/symbol
-- CS2: symbol tracking loop: accumulated ST phase correction (1 sample/symbol)
-- CS3: carrier tracking loop: accumulated carrier phase correction (4 samples/symbol)
-- CS4: FREQ2 discriminator (easier to follow symbol tracking loop ability to track)
CS1 <= DATA4_I(13 downto 6);
CS1_CLK <= SYMBOL4_CLKx4;
CS2 <= std_logic_vector(STE_ACC(31 downto 24));
CS2_CLK <= ST_NCO_TIMING_CONTROL_CLK;
CS3 <= std_logic_vector(CCF_PHASE1(31 downto 24));
CS3_CLK <= SYMBOL4_CLKx4;
CS4 <= std_logic_vector(FREQ2(11 downto 4));
CS4_CLK <= SYMBOL4_CLKx4_D3;

--// simulation. send to a tab-delimited text file for subsequent (Matlab) plot.
SIM2OUTFILE_X: if(SIMULATION = '1') generate
-- 	SIM2OUTFILE_001: SIM2OUTFILE 
--	generic map(
--		FILENAME => "C:\Users\Alain\Documents\1VHDL\com-1827 FSK modem 000\matlab\bestmfi.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => std_logic_vector(BEST_MF_L_I) & "00",
--		DATA2 => std_logic_vector(BEST_MF_I) & "00",		
--		DATA3 => std_logic_vector(BEST_MF_E_I) & "00",
--		SAMPLE_CLK_IN => BEST_MF_VALID
--	);
-- 	SIM2OUTFILE_002: SIM2OUTFILE 
--	generic map(
--		FILENAME => "C:\Users\Alain\Documents\1VHDL\com-1827 FSK modem 000\matlab\bestmfq.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => std_logic_vector(BEST_MF_L_Q) & "00",
--		DATA2 => std_logic_vector(BEST_MF_Q) & "00",		
--		DATA3 => std_logic_vector(BEST_MF_E_Q) & "00",
--		SAMPLE_CLK_IN => BEST_MF_VALID
--	);
--
-- 	SIM2OUTFILE_000: SIM2OUTFILE 
--	generic map(
--		FILENAME => "C:\Users\Alain\Documents\1VHDL\com-1827 FSK modem 000\matlab\cpmdemod0.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => DATA3_I(13 downto 2),		-- input samples
--		DATA2 => DATA3_Q(13 downto 2),
--		DATA3 => "000000000000",
--		SAMPLE_CLK_IN => SYMBOL3_CLKx4
--	);
-- 	SIM2OUTFILE_001: SIM2OUTFILE 
--	generic map(
--		FILENAME => "C:\Users\Alain\Documents\1VHDL\com-1827 FSK modem 000\matlab\cpmdemod1.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => DATA4_I(13 downto 2),		-- input samples
--		DATA2 => DATA4_Q(13 downto 2),
--		DATA3 => "000000000000",
--		SAMPLE_CLK_IN => SYMBOL4_CLKx4
--	);
--
-- 	SIM2OUTFILE_001: SIM2OUTFILE 
--	generic map(
--		FILENAME => "cpmdemod1.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => DATA4_I(13 downto 2),		-- input samples
--		DATA2 => DATA4_Q(13 downto 2),
--		DATA3 => "000000000" & std_logic_vector(SAMPLE4_CNTR),
--		SAMPLE_CLK_IN => SYMBOL4_CLKx4
--	);
--
--	SIM2OUTFILE_002: SIM2OUTFILE 
--	generic map(
--		FILENAME => "cpmdemod2.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => DATA4_I(13 downto 2),		-- input samples
--		DATA2 => DATA4_Q(13 downto 2),
--		DATA3 => std_logic_vector(FREQ2(17 downto 6)),
--		SAMPLE_CLK_IN => SYMBOL4_CLKx4_D3
--	);
end generate;

end behavioral;
