-------------------------------------------------------------
-- MSS copyright 1999-2017
--	Filename:  CPM_DEMOD.VHD
-- Author: Alain Zarembowitch / MSS
--	Version: 1
--	Date last modified: 8/31/17
-- 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

		--// Settings
		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 (Tier 2) 
			-- 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)

		--// Controls
	   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 AGC (TEST TEST TEST). 

		--// 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: multi-symbol detector output (1 sample per bit)
		-- CS5: FREQ3 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;
		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_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 CPM_MSD3
	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;	
		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;
		FILT_SEL: in std_logic_vector(3 downto 0);
		SAMPLE_CLK_OUT : OUT std_logic;
		DATA_OUT: out std_logic
--		WINNER_OUT: out std_logic_vector(N_TRELLIS_STATES_MAX - 1 downto 0);	
--		BEST_END_STATE: out std_logic_vector((LOG2_N_TRELLIS_STATES_MAX-1) downto 0);
--		BEST_DATA: out std_logic;
--		DELTA_OUT: OUT DELTA_TYPE;
--		BEST_SYMBOL_I: out std_logic_vector(17 downto 0);
--		BEST_SYMBOL_Q: out std_logic_vector(17 downto 0);
--		BEST_SYMBOL_CLK: out std_logic
		);
	END COMPONENT;

	COMPONENT VA_RTU
	GENERIC(
		SIMULATION: std_logic 
	);
	PORT(
		CLK : IN std_logic;
		SYNC_RESET : IN std_logic;
		SAMPLE_CLK_IN : IN std_logic;
		DELTA: IN DELTA_TYPE;
		WINNER : IN std_logic_vector(7 downto 0);
		TB: in TB_TYPE;
		BEST_END_STATE: in std_logic_vector((LOG2_N_TRELLIS_STATES_MAX-1) downto 0);
		RELIABILITY_OUT: out std_logic_vector((SOVA_NBITS-2) downto 0);
		SAMPLE_CLK_OUT : 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 FIRHALFBAND3
	PORT(
		CLK : IN std_logic;
		SAMPLE_CLK_IN : IN std_logic;
		DATA_IN : IN std_logic_vector(13 downto 0);          
		SAMPLE_CLK_OUT : OUT std_logic;
		DATA_OUT : OUT std_logic_vector(13 downto 0)
		);
	END COMPONENT;

	COMPONENT SYMBOL_TIMING_LOOP5
	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

--//-- STATE MACHINE -------------------------------
signal STATE: integer range 0 to 5 := 0;
signal STATE_TIMER: unsigned(13 downto 0) := (others => '0');
signal COHERENT_DEMOD: std_logic := '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 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 DATA3_Ix: std_logic_vector(17 downto 0) := (others => '0');
signal DATA3_Qx: std_logic_vector(17 downto 0) := (others => '0');
signal SAMPLE3_CLK: std_logic := '0';
signal SYMBOL3_CLK: std_logic := '0';
signal SYMBOL3_CLKx2: std_logic := '0';
signal SYMBOL3_CLKx4: std_logic := '0';
signal SAMPLE3_CNTR: unsigned(1 downto 0) := (others => '0');

--//-- FAST FREQUENCY ACQUISITION ---------------------------------------
signal FFD_ENABLE: std_logic := '0';
signal FFD_SATURATION_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_THRESHOLD: std_logic_vector(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');

--//-- 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_CLK: std_logic := '0';

--//-- MULTI-SYMBOL DETECTION ------------------------------
signal SYNC_RESET_NONCOHERENTDET: std_logic := '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'));


--//-- VITERBI ALGORITHM ------------------------------
signal BM: BM_TYPE := (others => (others => '0'));
signal BM_SYMBOL_CLK: std_logic := '0';
signal BM_SYMBOL_CLK_D: std_logic := '0';
signal BM_SYMBOL_CLK_D2: std_logic := '0';
signal BM_SYMBOL_CLK_D4: std_logic := '0';
signal SYNC_RESET_COHERENTDET: std_logic := '0'; 
signal VA_BEST_END_STATE: std_logic_vector((LOG2_N_TRELLIS_STATES_MAX-1) downto 0) := (others => '0');
signal VA_WINNER: std_logic_vector(N_TRELLIS_STATES_MAX-1 downto 0) := (others => '0');
signal VA_BEST_DATA: std_logic := '0';
signal VA_BEST_SYMBOL_I: std_logic_vector(17 downto 0) := (others => '0');
signal VA_BEST_SYMBOL_Q: std_logic_vector(17 downto 0) := (others => '0');
signal VA_BEST_SYMBOL_CLK: std_logic := '0';
signal VA_DELTA: DELTA_TYPE := (others => (others => '0'));
signal VA_TB: TB_TYPE := (others => (others => '0'));
signal VA_RELIABILITY: std_logic_vector((SOVA_NBITS-2) downto 0) := (others => '0');
signal DATA_OUT_local: std_logic_vector(3 downto 0) := (others => '0');
signal SAMPLE_CLK_OUT_local: 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 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 TRACKING ------------------------
signal ST_SYMBOL_CLK_IN: std_logic := '0';
signal ST_SYMBOL_CLKx2_IN: std_logic := '0';
signal SAMPLE3_CNTR_D: unsigned(1 downto 0) := (others => '0');
signal SAMPLE3_CNTR_D2: unsigned(1 downto 0) := (others => '0');
signal SAMPLE3_CNTR_D3: unsigned(1 downto 0) := (others => '0');
signal SAMPLE3_CNTR_D4: unsigned(1 downto 0) := (others => '0');
signal SAMPLE3_CNTR_D5: unsigned(1 downto 0) := (others => '0');
signal SYMBOL4_CLKx4: std_logic := '0';
signal SAMPLE4_CNTR: unsigned(1 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 SYMBOL4_CLKx4_D: std_logic := '0';
signal SYMBOL4_CLKx4_D2: std_logic := '0';
signal SYMBOL4_CLKx4_D3: std_logic := '0';
signal SYMBOL4_CLKx4_D4: std_logic := '0';
signal SAMPLE4_CNTR_D: unsigned(1 downto 0) := (others => '0');
signal SAMPLE4_CNTR_D2: unsigned(1 downto 0) := (others => '0');
signal SAMPLE4_CNTR_D3: unsigned(1 downto 0) := (others => '0');
signal SAMPLE4_CNTR_D4: unsigned(1 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 FREQ2_D: std_logic_vector(17 downto 0) := (others => '0');
signal FREQ3: signed(18 downto 0) := (others => '0');
signal NCO_TIMING_CONTROL: std_logic_vector(31 downto 0) := (others => '0');
signal NCO_TIMING_CONTROL_CLK: std_logic := '0';
signal NCO_TIMING_CONTROLB: std_logic_vector(31 downto 0) := (others => '0');

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

--// general
COHERENT_DEMOD <= '1' when (MODULATION_INDEX = x"0800") and (unsigned(FILT_SEL) /= 1) else '0';
-- 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.


--//-- 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) then
		    -- goto FFT-based fast frequency acquisition + scanning mode 
		   STATE <= 1; 
			STATE_TIMER <= (others => '0');	-- expired
		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 (SIGNAL_ABSENT = '1') then
			-- persistent signal absent from FFT. Back to acquisition.
			STATE <= 0;
			
	   -- TODO: persistent loss of carrier lock ?
		elsif(STATE_TIMER /= 0) and (SYMBOL3_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 => "00110000000000",
	CONTROL => "1",	-- always enabled
	SYMBOLx2_CLK_IN => SAMPLE3_CLK,	
	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_PHASE1(31 downto 20)),	-- CCF_PHASE1 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
		-- divide by 4 to update 4x per symbol)
		CF_NCO_CONTROL1a <= resize(CF_NCO_CONTROL(31 downto 2),32); -- carrier tracking loop 
		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');
		elsif(SYMBOL3_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;

-- 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";
NCO_TIMING_CONTROLB <= NCO_TIMING_CONTROL when (SIMULATION = '0') or (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 => NCO_TIMING_CONTROLB,
	NCO_OFFSET_SAMPLE_CLK => NCO_TIMING_CONTROL_CLK,
	DATA_I_OUT => DATA3_I,
	DATA_Q_OUT => DATA3_Q,
	TT_OUT => open,
	SAMPLE_CLK_OUT => SAMPLE3_CLK
);


-- arbitrarily select one sample as end of symbol (the symbol tracking loop will make sure it becomes true)
-- The symbol tracking loop will place SYMBOL3_CLK (SAMPLE3_CNTR = 0) at the symbol center 
SYMBOL3_CLK_GEN: process(CLK)
begin
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') then
			SAMPLE3_CNTR <= (others => '0');
		elsif(SAMPLE3_CLK = '1') then
			SAMPLE3_CNTR <= SAMPLE3_CNTR + 1;	-- modulo-4 counter since 4 samples/symbol
		end if;
	end if;
end process;
SYMBOL3_CLK <= '1' when (SAMPLE3_CLK = '1') and (SAMPLE3_CNTR = 0)  else '0';
SYMBOL3_CLKx2 <= '1' when (SAMPLE3_CLK = '1') and (SAMPLE3_CNTR(0) = '0')  else '0';
SYMBOL3_CLKx4 <= SAMPLE3_CLK;

--//-- FAST FREQUENCY ACQUISITION ---------------------------------------
-- Algorithm: despread input is squared or **4 (doubles or quadruple the frequency and removes the PSK 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.

DATA3_Ix <= DATA3_I & "0000";
DATA3_Qx <= DATA3_Q & "0000";
FFD_SATURATION_IN <= SATURATION1 or SATURATION2;
SYNC_RESET_COHERENTDET <= SYNC_RESET when (COHERENT_DEMOD = '1') else '1';
	-- save power when non-coherent demod
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 => DATA3_Ix,
	DATA_Q_IN => DATA3_Qx,  
	SAMPLE_CLK_IN => SAMPLE3_CLK, 
	SATURATION_IN => FFD_SATURATION_IN,	
	ENABLE => FFD_ENABLE,	-- trigger. enable only during acquisition
	CONTROL => "01",	-- square
	FFTM => FFTM,	
	FFTM2 => FFTM2,   
	FFTM_AVRG => FFTM_AVRG, 
	FFTF => FFTF,
	FFTP => FFTP,     -- unused
	FFT_READY => FFT_READY,
	SATURATION => open,    
	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
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(FFTM(13 downto 2) > FFTM_THRESHOLD(13 downto 2)) 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
-- test test test
		if (SYNC_RESET = '1') then
			-- clear frequency error
			FFT_FREQ_ERROR <= (others => '0');
		elsif(FFT_READY = '1') and (FFD_IGNORE_OUTPUT = '0') then
			-- extend to 32 bits NCO
		  FFT_FREQ_ERROR(31 downto 30) <= (others => not FFTF(10));
		  FFT_FREQ_ERROR(29 downto 20) <= FFTF(9 downto 0);
		  FFT_FREQ_ERROR(19 downto 0) <= (others => '0');	
		end if;
	end if;
end process;

-- test test test
--FFT_FREQ_CORRECTION <= - signed(FFT_FREQ_ERROR);	-- relative to symbol rate
FFT_FREQ_CORRECTION <= (others => '0');


--//-- MATCHED FILTERS ------------------------
-- For each estimated phase (P cases), and 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.
-- Step 3: rotate each by 2*pi/P and take the real value. Thus generating pM^L real branch metrics
-- for use by follow-on Viterbi algorithm
-- 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 => DATA3_I(13 downto 14-CPM_MF_NBITS),
	DATA_Q_IN => DATA3_Q(13 downto 14-CPM_MF_NBITS),
	SAMPLE_CLK_IN => SAMPLE3_CLK,
	SAMPLE_CNTR_IN => std_logic_vector(SAMPLE3_CNTR),   -- 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,	-- SYMBOL3_CLK_D10 TODO REGRESSION TESTS CHANGED!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	BM_OUT => BM_MF,	-- branch metrics
	MF_I_OUT => MF_I,
	MF_Q_OUT => MF_Q,
	MF_SYMBOL_CLK_OUT => MF_SYMBOL_CLK,  -- SYMBOL3_CLK_D8
	COS_PIH => COS_PIH,
	SIN_PIH => SIN_PIH,
	MF_INITIALIZED => open
);

--//-- 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, 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_001: CPM_MSD3
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,  -- SYMBOL3_CLK_D8
	COS_PIH => COS_PIH,
	SIN_PIH => SIN_PIH,
	SYMBOL_CLK_OUT => BM_MSD3_SYMBOL_CLK,	
	BM_OUT => BM_MSD3	-- branch metrics
);
	
	
--//-- VITERBI ALGORITHM ------------------------------
-- 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;

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,
	FILT_SEL => FILT_SEL,
	SAMPLE_CLK_OUT => SAMPLE_CLK_OUT_local,	
	DATA_OUT => DATA_OUT_local(3)					
--	WINNER_OUT => VA_WINNER,	-- available at BM_SYMBOL_CLK_D4
--	BEST_END_STATE => VA_BEST_END_STATE,
--	BEST_DATA => VA_BEST_DATA,
--	SAMPLE_CLK_OUT => BM_SYMBOL_CLK_D4,	-- Latency: 4 CLKs after BM_SYMBOL_CLK
--	DELTA_OUT => VA_DELTA,	-- available at BM_SYMBOL_CLK_D2
--	BEST_SYMBOL_I => VA_BEST_SYMBOL_I,
--	BEST_SYMBOL_Q => VA_BEST_SYMBOL_Q,
--	BEST_SYMBOL_CLK => VA_BEST_SYMBOL_CLK
);



RECLOCK_001: process(CLK)
begin
	if rising_edge(CLK) then
		BM_SYMBOL_CLK_D <= BM_SYMBOL_CLK;
		BM_SYMBOL_CLK_D2 <= BM_SYMBOL_CLK_D;
	end if;
end process;

-- reliability traceback unit (only when soft output is selected)
SOVA_1: if(SOVA = '1') generate
	CPM_VA_RTU_001: VA_RTU 
	GENERIC MAP(
		SIMULATION => SIMULATION
	)
	PORT MAP(
		CLK => CLK,
		SYNC_RESET => SYNC_RESET,
		SAMPLE_CLK_IN => BM_SYMBOL_CLK_D4,
		DELTA => VA_DELTA,	-- aligned with BM_SYMBOL_CLK_D2
		WINNER => VA_WINNER,	-- aligned with BM_SYMBOL_CLK_D4
		TB => VA_TB,		-- aligned with BM_SYMBOL_CLK_D5
		BEST_END_STATE => VA_BEST_END_STATE, -- aligned with BM_SYMBOL_CLK_D4
		RELIABILITY_OUT => VA_RELIABILITY,		-- aligned with BM_SYMBOL_CLK_D6
		SAMPLE_CLK_OUT => open					-- aligned with BM_SYMBOL_CLK_D6
	);
	
	DATA_OUT_local(2 downto 0) <= VA_RELIABILITY when (DATA_OUT_local(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
	DATA_OUT_local(2 downto 0) <= (others => DATA_OUT_local(3));
end generate;


-- Select Multi-symbol detection for PCM/FM, SOVA for others
--DATA_OUT <=  when (unsigned(FILT_SEL) = 1)  else DATA_OUT_local;
--SAMPLE_CLK_OUT <=   when (unsigned(FILT_SEL) = 1) else SAMPLE_CLK_OUT_local;
DATA_OUT <=  DATA_OUT_local;
SAMPLE_CLK_OUT <= SAMPLE_CLK_OUT_local;


--//-- 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
	FREEZE => CONTROL(2),	-- TEST TEST TEST	
	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)
-- Compute the phase error PE as 
-- Q*sign(I) - I*sign(Q) 
-- Note: this method seems more resilient at very low SNRs than the POLAR3 cartesian to polar coordinate look-up table.
-- Computation is done only once per symbol period at the presumed optimum sampling instant.
-- 1 bit sign extension for PE to prevent overflow, dropped further below.
-- Design note about scaling: DATA3_x is normalized at 64K when AGC18 is tracking.
-- Although PE is an approximation, we scale it such that it roughly matches a 11 bit phase (2047 = 360 deg)
CARRIER_TRACK_001: process(CLK)
begin
	if rising_edge(CLK) then
		PE_SYMBOL_CLK <= SYMBOL3_CLK;

		if(SYMBOL3_CLK = '1') then
			-- 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 +/- 500 (targeting +/-512)
			if(DATA3_Ix(17) = '0') and (DATA3_Qx(17) = '0') then
				PE <= resize(signed(DATA3_Qx(17 downto 7)),12) - resize(signed(DATA3_Ix(17 downto 7)),12);
			elsif(DATA3_Ix(17) = '0') and (DATA3_Qx(17) = '1') then
				PE <= resize(signed(DATA3_Qx(17 downto 7)),12) + resize(signed(DATA3_Ix(17 downto 7)),12);
			elsif(DATA3_Ix(17) = '1') and (DATA3_Qx(17) = '0') then
				PE <= - resize(signed(DATA3_Qx(17 downto 7)),12) - resize(signed(DATA3_Ix(17 downto 7)),12) ;
			else
				PE <= - resize(signed(DATA3_Qx(17 downto 7)),12) + resize(signed(DATA3_Ix(17 downto 7)),12);
			end if;
		end if;
	end if;
end process;


CT_SYNC_RESET <= '1' when (STATE > 0) and (COHERENT_DEMOD = '1') else '0';	-- save power when non-coherent demod
CT_ACCELERATED_CT_RESPONSE <= '1' when (STATE < 2) else '0';
CT_LOCK_THRESHOLD <= "0111";	-- 30 deg rms threshold
CT_002: CARRIER_TRACKING 
GENERIC MAP(
	SIMULATION => SIMULATION
)
port map (
	CLK => CLK,
	SYNC_RESET => CT_SYNC_RESET,
	OPTIMUM_SAMPLING_CLK => PE_SYMBOL_CLK,	-- SYMBOL3_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,	-- SYMBOL3_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))
CARRIER_TRACK2_004: process(CLK)
begin
	if rising_edge(CLK) then
	   CF_NCO_CONTROL_SAMPLE_CLK <= RESIDUAL_FREQUENCY_OFFSET_CLK;	-- once per symbol. -- SYMBOL3_CLK_D17
	   
		-- Correct sign depending on spectrum inversion or not
		if(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 TRACKING ------------------------
-- 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)

-- band-limiting first to reduce the effect of noise
-- latency 6 CLK + 10 sample group delay (2.5 symbols)
FIRHALFBAND3_I1: FIRHALFBAND3 PORT MAP(
	CLK => CLK,
	SAMPLE_CLK_IN => SYMBOL3_CLKx4,		
	DATA_IN => DATA3_I,
	SAMPLE_CLK_OUT => SYMBOL4_CLKx4,		
	DATA_OUT => DATA4_I
);
FIRHALFBAND3_Q1: FIRHALFBAND3 PORT MAP(
	CLK => CLK,
	SAMPLE_CLK_IN => SYMBOL3_CLKx4,		
	DATA_IN => DATA3_Q,
	SAMPLE_CLK_OUT => open,		
	DATA_OUT => DATA4_Q
);

FIRHALFBAND_DELAY: process(CLK)
begin
	if rising_edge(CLK) then
		SAMPLE3_CNTR_D <= SAMPLE3_CNTR;
		SAMPLE3_CNTR_D2 <= SAMPLE3_CNTR_D;
		SAMPLE3_CNTR_D3 <= SAMPLE3_CNTR_D2;
		SAMPLE3_CNTR_D4 <= SAMPLE3_CNTR_D3;
		SAMPLE3_CNTR_D5 <= SAMPLE3_CNTR_D4;
		SAMPLE4_CNTR <= SAMPLE3_CNTR_D5;
	end if;
end process;

-- 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
		MULT41_D <= MULT41;
		MULT42_D <= MULT42;
		
		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;

		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 process;

-- Add 1/2 sample delay in the ST loop by using the average between two successive FREQ2 samples
ST_LOOP_002: process(CLK)
begin
	if rising_edge(CLK) then
		SAMPLE4_CNTR_D4 <= SAMPLE4_CNTR_D3;
		SYMBOL4_CLKx4_D4 <= SYMBOL4_CLKx4_D3;

		if(SYMBOL4_CLKx4_D3 = '1') then
			FREQ2_D <= FREQ2;
			FREQ3 <= resize(signed(FREQ2),19) + resize(signed(FREQ2_D),19);	-- we'll divide by two later
		end if;
	end if;
end process;
ST_SYMBOL_CLK_IN <= SYMBOL4_CLKx4_D4 when (SAMPLE4_CNTR_D4 = 1) else '0';
ST_SYMBOL_CLKx2_IN <= SYMBOL4_CLKx4_D4 when (SAMPLE4_CNTR_D4(0) = '1') else '0';
	-- compensate for various delays between RESAMPLING3 and SYMBOL_TIMING_LOOP5 so that
	-- the SYMBOL_CLK is at the peak phase (= start of matched filter)
	-- 1/2 symbol offset because of the HBF group delay + one extra clock for yet unknown reason.

-- ST loop controls the resampling NCO phase so that SYMBOL4_CLK_D4 is at peak frequency FREQ3,
SYMBOL_TIMING_LOOP5_001: SYMBOL_TIMING_LOOP5 PORT MAP(
	CLK => CLK,
	SYNC_RESET => SYNC_RESET,
	SYMBOL_CLK_IN => ST_SYMBOL_CLK_IN,
	SYMBOL_CLKx2_IN => ST_SYMBOL_CLKx2_IN,
	DATA_IN => std_logic_vector(FREQ3(18 downto 1)),
	FREEZE_SYMBOL_TIMING => '0',
	LOOP_GAIN => x"00",
	NCO_TIMING_CONTROL => NCO_TIMING_CONTROL,
	NCO_TIMING_CONTROL_CLK => NCO_TIMING_CONTROL_CLK,
	CS1 => CS2,
	CS1_CLK => CS2_CLK
);

--// MONITORING, TEST POINTS --------------------------------------------
-- 1: AFC lock (abs(residual frequency error) < symbol rate/16)
-- 2: signal presence
-- 3: recovered carrier
-- 4: recovered bit timing
TP(1) <= AFC_LOCK;
TP(2) <= '0' when (SIGNAL_PRESENCE_CONFIDENCE = 0) else '1';
TP(3) <= CCF_PHASE1(31);
TP(4) <= SYMBOL3_CLK;


-- 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: multi-symbol detector output (1 sample per bit)
-- CS5: FREQ3 discriminator (easier to follow symbol tracking loop ability to track)
CS1 <= DATA3_I(13 downto 6);
CS1_CLK <= SAMPLE3_CLK;
-- reserved CS2
CS3 <= std_logic_vector(CCF_PHASE1(31 downto 24));
CS3_CLK <= SAMPLE3_CLK;
-- reserved CS4
CS5 <= std_logic_vector(FREQ3(11 downto 4));
CS5_CLK <= SYMBOL4_CLKx4_D4;

--// simulation. send to a tab-delimited text file for subsequent (Matlab) plot.
SIM2OUTFILE_X: if(SIMULATION = '1') generate
 	SIM2OUTFILE_000: SIM2OUTFILE 
	generic map(
		FILENAME => "cpmdemod0.txt"
	)
	PORT MAP(
		CLK => CLK,
		DATA1 => DATA3_I(13 downto 2),		-- input samples
		DATA2 => DATA3_Q(13 downto 2),
		DATA3 => "0000000000" & std_logic_vector(SAMPLE3_CNTR),
		SAMPLE_CLK_IN => SYMBOL3_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 => "0000000000" & 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(FREQ3(18 downto 7)),
		SAMPLE_CLK_IN => SYMBOL4_CLKx4_D4
	);
end generate;

end behavioral;
