----------------------------------------------
-- MSS copyright 2014
-- Filename: CARRIER_TRACKING2.VHD
-- Author: Alain Zarembowitch / MSS
-- Edit date:  2/13/15
-- Revision: 1
-- Inheritance: CARRIER_TRACKING.VHD 10/1/14, COSTAS1B.VHD 4-2-04 rev6, CARRIER_TRACKING.vhd  COM_1202  3/5/09
-- Extended tracking range beyond the symbol rate.
--
-- Acquisition and tracking circuit for carrier frequency
-- (Costas tracking loop).	
--
-- Rev 1 2/13/15 AZ
-- For very small symbol rates, the phase correction cannot be updated every clock (because the phase correction is too 
-- coarsely quantized). Added NCO_CONTROL_SAMPLE_CLK to pace the rate of phase correction.
----------------------------------------------      
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

entity CARRIER_TRACKING2 is
	generic (
    SIMULATION: std_logic := '0'
        -- 1 during simulation. 
    );
	port (  
	--// GLOBAL CLOCKS
    CLK : in std_logic;				-- System clock for this FPGA
    SYNC_RESET: in std_logic;
	
	--// INPUTS
   OPTIMUM_SAMPLING_CLK: in std_logic;
     	-- One CLK long pulse indicating that the input phase error
     	-- is associated with the optimum sampling instant. 
	PHASE_ERROR: in std_logic_vector(10 downto 0);
		-- 2's complement phase error.
		-- Read at the rising edge of CLK when OPTIMUM_SAMPLING_CLK = '1'	
		-- Expected range: 360deg = 2048
    NOMINAL_SYMBOL_RATE: in std_logic_vector(31 downto 0);
        -- = fsymbol rate / fclk * 2^32 
    LOOP_GAIN1: in std_logic_vector(1 downto 0);
    LOOP_GAIN2: in std_logic_vector(1 downto 0);
        -- 1st order and 2nd order loop branches gain
		-- x1 nominal (00), x2 (01), x4 (10), x8 (11)
		-- for test purposes. Generally commented out within.
	ACCELERATED_CT_RESPONSE: in std_logic;
		-- '1' to clear all memory-based loops (2nd order PLL), for example when external AFC loop is enabled during acquisition
		-- Also temporarily increase the 1st order PLL gain (more noisy, but faster acquisition)
		-- Does not reset rms phase error averaging.
	LOCK_THRESHOLD: in std_logic_vector(3 downto 0);
		-- select the rms phase error threshold based on which the carrier tracking lock 
		-- status will be generated
		-- 0 = 1 deg
		-- 1 = 2 deg
		-- 2 = 5 deg
		-- 3 = 10 deg
		-- 4 = 15 deg
		-- 5 = 20 deg
		-- 6 = 25 deg
		-- 7 = 30 deg
		-- 8 = 35 deg
		-- 9 = 40 deg
		-- 10 = 45 deg

	--// OUTPUTS
	NCO_CONTROL: out std_logic_vector(31 downto 0);
	NCO_CONTROL_SAMPLE_CLK: out std_logic;
		-- signed phase error to drive a NCO. Updated once per CLK (unless symbol rate is very small, kbps)
		-- tends to be small  
        -- instead of discrete phase corrections once per symbol, spread smaller phase corrections more frequently (every clock period)
	FREQ_ERROR: out std_logic_vector(31 downto 0);
	    -- signed frequency error (expressed as 2^32*fe/fclk). Updated once per CLK
	    -- can be larger than the symbol rate. 

	--// MONITORING
	CARRIER_LOCK: out std_logic;
		-- flag indicating with some degree of certainty that the carrier tracking loop is in lock.
		-- '1' for in lock, '0' for out of lock.  Averaging over 1024 symbols approx.
	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 entity;  

architecture behavioral of CARRIER_TRACKING2 is
-----------------------------------------------------------------
-- COMPONENTS
-----------------------------------------------------------------
	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 denotes one CLK clock delayed version of the name with the same name
-- Suffix _N denotes the inverted (not) version of the net with the same name
-- Suffix _X denotes the precision extended version of the net with the same name
signal LOOP_GAIN1B: std_logic_vector(1 downto 0)  := (others => '0');
signal SCALING_A_slv: std_logic_vector(17 downto 0) := (others => '0');
signal SCALING_A: signed(17 downto 0) := (others => '0');
signal SCALING_B: signed(17 downto 0) := (others => '0');
signal SCALING_B_FLAG: integer range 0 to 2:= 0;
signal SCALING_P: signed(35 downto 0) := (others => '0');
signal SCALING_P_D: signed(35 downto 0) := (others => '0');
signal CLK_DIV64: std_logic := '0';
signal CLK_DIV64_CNTR: std_logic_vector(5 downto 0) := (others => '0');
signal OPTIMUM_SAMPLING_CLK_D: std_logic := '0';
signal OPTIMUM_SAMPLING_CLK_D2: std_logic := '0';
signal OPTIMUM_SAMPLING_CLK_D3: std_logic := '0';
signal PHASE_ERROR_ACC: signed(43 downto 0) := (others => '0');
signal NCO_CONTROL_local: signed(31 downto 0) := (others => '0');
signal NCO_CONTROL_SAMPLE_CLK_local: std_logic := '0';
signal FREQ_ERROR_local: signed(31 downto 0) := (others => '0');

--// LOCK STATUS ----------------------------------------------------------------------
signal PHASE_ERROR_X: signed(19 downto 0) := (others => '0');
signal PHASE_ERROR_ACC2: signed(19 downto 0) := (others => '0');
signal PHASE_ERROR_ACC2_DIV512: signed(19 downto 0) := (others => '0');
signal PHASE_ERROR_MINUS_BIAS: signed(19 downto 0) := (others => '0');
signal PHASE_ERROR_MINUS_BIAS_D: signed(19 downto 0) := (others => '0');
signal PHASE_ERROR_SQUARE: signed(23 downto 0) := (others => '0');
signal PHASE_ERROR_SQUARE_D: unsigned(23 downto 0) := (others => '0');
signal PHASE_ERROR_SQUARE_ACC: unsigned(31 downto 0) := (others => '0');
signal PHASE_ERROR_SQUARE_ACC_DIV512: unsigned(22 downto 0) := (others => '0');
signal PHASE_ERROR_SQUARE_THRESHOLD: unsigned(20 downto 0) := (others => '0');
signal CARRIER_LOCK_local: std_logic := '0';
-----------------------------------------------------------------
-- IMPLEMENTATION
-----------------------------------------------------------------
begin   

--// scale phase error, depending on the loop gain.
-- Different loop gain under user control.
-- Gives the user the control to tradeoff phase noise and thermal noise.
-- Also increases the frequency acquisition range.

-- Nominal tracking loop gain is optimized for a maximum BER performance degradation < 0.5dB (TBC)

-- Note: increasing the loop gain may cause instabilities.
--LOOP_GAIN1B <=  LOOP_GAIN1 when (ACCELERATED_CT_RESPONSE = '0') else
--                (LOOP_GAIN1 + 2) when (LOOP_GAIN1 < 2) else 
--                "11";
-- test test test
LOOP_GAIN1B <=  LOOP_GAIN1;
LOOP_GAIN1_001: process(CLK)
begin
    if rising_edge(CLK) then
        OPTIMUM_SAMPLING_CLK_D <= OPTIMUM_SAMPLING_CLK;
        
  	    case(LOOP_GAIN1B) is
           when "00" =>     -- nominal loop gain
            SCALING_A_slv(3 downto 0) <= (others => '0');
            SCALING_A_slv(14 downto 4) <= PHASE_ERROR;
            SCALING_A_slv(17 downto 15) <= (others => PHASE_ERROR(10));
           when "01" =>     -- x2 loop gain
            SCALING_A_slv(4 downto 0) <= (others => '0');
            SCALING_A_slv(15 downto 5) <= PHASE_ERROR;
            SCALING_A_slv(17 downto 16) <= (others => PHASE_ERROR(10));
           when "10" =>     -- x4 loop gain
            SCALING_A_slv(5 downto 0) <= (others => '0');
            SCALING_A_slv(16 downto 6) <= PHASE_ERROR;
            SCALING_A_slv(17 downto 17) <= (others => PHASE_ERROR(10));
           when others =>     -- x8 loop gain
            SCALING_A_slv(6 downto 0) <= (others => '0');
            SCALING_A_slv(17 downto 7) <= PHASE_ERROR;
        end case;
    end if;
end process;
SCALING_A <= signed(SCALING_A_slv);

-- scale phase error per processing CLK period, instead of per modulation symbol period
-- For very small symbol rates, extend multiplier range by 0,5 or 10 bits
SCALING_001: process(CLK)
begin
    if rising_edge(CLK) then
        if(NOMINAL_SYMBOL_RATE(31 downto 21) = 0) then
            -- very small symbol rate (at least 4096 clocks per symbol) 
            SCALING_B <= signed(NOMINAL_SYMBOL_RATE(21 downto 4));   
            SCALING_B_FLAG <= 0;  -- divide result by 2^10
        elsif(NOMINAL_SYMBOL_RATE(31 downto 26) = 0) then
            -- medium symbol rate. 
            SCALING_B <= signed(NOMINAL_SYMBOL_RATE(26 downto 9));   
            SCALING_B_FLAG <= 1;  -- divide result by 2^5
        else
            -- large symbol rate
            SCALING_B <= signed(NOMINAL_SYMBOL_RATE(31 downto 14)); -- left aligned
            SCALING_B_FLAG <= 2;
        end if;
    end if;
end process;

SCALING_P <= SCALING_A * SCALING_B;

-- reclock after multipler for better timing
SCALING_002: process(CLK)
begin
    if rising_edge(CLK) then
        SCALING_P_D <= SCALING_P;
        OPTIMUM_SAMPLING_CLK_D2 <= OPTIMUM_SAMPLING_CLK_D;
    end if;
end process;
 
--// first order branch
-- Calibrated so that the loop corrects a 45deg offset in about 128 symbols 11/10/14

-- generate CLK_DIV64 (slow phase correction update rate when symbol rate is very small)
CLK_DIV64_GEN: process(CLK)
begin
    if rising_edge(CLK) then
        CLK_DIV64_CNTR <= CLK_DIV64_CNTR + 1;
        if(CLK_DIV64_CNTR = 0) then
           CLK_DIV64 <= '1';
        else
           CLK_DIV64 <= '0';
        end if;
    end if;
end process;

FIRST_ORDER_OUT: process(CLK)
begin
    if rising_edge(CLK) then
        if(SCALING_B_FLAG = 0) then
            -- divide result by 7 bits + 10 bits (scaling_b_flag)
            -- increase control by x64 while diminishing the update rate by 64 (to alleviate quantization effects)
            NCO_CONTROL_local <= resize(SCALING_P_D(35 downto 11), NCO_CONTROL_local'length);
            NCO_CONTROL_SAMPLE_CLK_local <= CLK_DIV64;  -- update phase every 64 clocks
        elsif(SCALING_B_FLAG = 1) then
            -- divide result by 7 bits + 5 bits (scaling_b_flag)
            NCO_CONTROL_local <= resize(SCALING_P_D(35 downto 12), NCO_CONTROL_local'length);
            NCO_CONTROL_SAMPLE_CLK_local <= '1';  -- update phase every clock
        elsif(SCALING_B_FLAG = 2) then
            -- divide result by 7 bits 
            NCO_CONTROL_local <= resize(SCALING_P_D(35 downto 7), NCO_CONTROL_local'length);
            NCO_CONTROL_SAMPLE_CLK_local <= '1';  -- update phase every clock
        end if;
    end if;
end process;
NCO_CONTROL <= std_logic_vector(NCO_CONTROL_local);
NCO_CONTROL_SAMPLE_CLK <= NCO_CONTROL_SAMPLE_CLK_local;


--// accumulated phase error (2nd order carrier tracking loop)
-- Accumulate the phase error, EVERY CLK (since we have scaled the phase error)
PHASE_ERROR_ACCUMULATE_001: process(CLK)
begin       
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') then
			-- clear the accumulated phase error (i.e. frequency memory) 
			PHASE_ERROR_ACC <= (others => '0');
		elsif(ACCELERATED_CT_RESPONSE = '1')   then
		    -- erase frequency memory when re-aquiring
			PHASE_ERROR_ACC <= (others => '0');
--			-- progressively unwind the frequency memory (over 512 symbols) when turning ACCELERATED_CT_RESPONSE on 
--			if(OPTIMUM_SAMPLING_CLK_D2 = '1') then
--			     PHASE_ERROR_ACC <= PHASE_ERROR_ACC - resize(PHASE_ERROR_ACC(31 downto 9),PHASE_ERROR_ACC'length);
--			end if;
		elsif (NCO_CONTROL_SAMPLE_CLK_local = '1') and ((SIMULATION = '0') or (NCO_CONTROL_local(0) /= 'X')) then
			-- Prevent X propagation in the accumulator.
	       PHASE_ERROR_ACC <= PHASE_ERROR_ACC + resize(NCO_CONTROL_local,PHASE_ERROR_ACC'length);
	 	end if;
	end if;
end process;

-- frequency error
-- convergence: divide 1st order correction by 4096  (nominal)
FREQ_ERROR_001: process(CLK)
begin       
	if rising_edge(CLK) then
        if(LOOP_GAIN2 = "00") then
            -- nominal
            FREQ_ERROR_local <= resize(PHASE_ERROR_ACC(PHASE_ERROR_ACC'left downto 20), FREQ_ERROR'length); -- /4096
        elsif (LOOP_GAIN2 = "01") then
            FREQ_ERROR_local <= resize(PHASE_ERROR_ACC(PHASE_ERROR_ACC'left downto 18), FREQ_ERROR'length);  -- /1024
        elsif (LOOP_GAIN2 = "10") then
            FREQ_ERROR_local <= resize(PHASE_ERROR_ACC(PHASE_ERROR_ACC'left downto 16), FREQ_ERROR'length);  -- /256
        else
            FREQ_ERROR_local <= (others => '0');
        end if;
    end if;
end process;
FREQ_ERROR <= std_logic_vector(FREQ_ERROR_local);

--// LOCK STATUS ----------------------------------------------------------------------
-- Lock status is based on the rms phase error
-- The mean(phase_error) can be non-zero, for example in presence of acceleration. Therefore, we have to 
-- compute the average(phase_error) and subtract it before squaring. 
-- Averaging is performed over 512 symbols.

LOCK_STATUS_001: process(CLK)
begin       
	if rising_edge(CLK) then
        -- precision extension to 20 bit before the leaky integrator
        PHASE_ERROR_X <= resize(signed(PHASE_ERROR), PHASE_ERROR_X'length);
	end if;
end process;

-- Average phase error (mean) over 512 samples
PHASE_ERROR_ACC2_DIV512 <= resize(PHASE_ERROR_ACC2(19 downto 9), PHASE_ERROR_ACC2_DIV512'length);

-- Remove the mean before squaring for computing the variance.
PHASE_ERROR_MINUS_BIAS <= PHASE_ERROR_X - PHASE_ERROR_ACC2_DIV512;

-- Average the phase error
LOCK_STATUS_002: process(CLK)
begin       
	if rising_edge(CLK) then
		PHASE_ERROR_MINUS_BIAS_D <= PHASE_ERROR_MINUS_BIAS; 	-- reclock for timing before squaring.

		if(OPTIMUM_SAMPLING_CLK_D = '1') and ((SIMULATION = '0') or (PHASE_ERROR_MINUS_BIAS(0) /= 'X')) then
			-- Prevent X propagation in the accumulator.
			PHASE_ERROR_ACC2 <= PHASE_ERROR_ACC2 +  PHASE_ERROR_MINUS_BIAS;
		end if;
	end if;
end process;

-- compute phase error ^2. (PHASE_ERROR_MINUS_BIAS_D is typically in the range +/-2048 so we can drop the MSbs)
PHASE_ERROR_SQUARE <= PHASE_ERROR_MINUS_BIAS_D(11 downto 0) * PHASE_ERROR_MINUS_BIAS_D(11 downto 0);

-- reclock for timing 
LOCK_STATUS_003: process(CLK)
begin       
	if rising_edge(CLK) then
		OPTIMUM_SAMPLING_CLK_D3 <= OPTIMUM_SAMPLING_CLK_D2;
        PHASE_ERROR_SQUARE_D <= unsigned(PHASE_ERROR_SQUARE);
	end if;
end process;

-- Leaky integrator
-- Averages phase error over 512 successive symbols
PHASE_ERROR_SQUARE_ACC_DIV512 <= PHASE_ERROR_SQUARE_ACC(31 downto 9);

LOCK_STATUS_004: process(CLK)
begin       
	if rising_edge(CLK) then
		if(SYNC_RESET = '1') then
			-- initialize at threshold (to avoid a prolongued period of 'locked' while the average is being computed
			PHASE_ERROR_SQUARE_ACC <= resize(PHASE_ERROR_SQUARE_THRESHOLD & "000000000",PHASE_ERROR_SQUARE_ACC'length);
		elsif(OPTIMUM_SAMPLING_CLK_D3 = '1') and ((SIMULATION = '0') or (PHASE_ERROR_SQUARE_D(0) /= 'X')) then
			PHASE_ERROR_SQUARE_ACC <= PHASE_ERROR_SQUARE_ACC +  resize(PHASE_ERROR_SQUARE_D,PHASE_ERROR_SQUARE_ACC'length) - resize(PHASE_ERROR_SQUARE_ACC_DIV512,PHASE_ERROR_SQUARE_ACC'length);
		end if;
	end if;
end process;

-- theshold comparison for detecting lock 
-- Units: (angle in deg / 360 * 2048)^2
LOCK_STATUS_005: process(CLK)
begin  
	if rising_edge(CLK) then	
        case (LOCK_THRESHOLD) is
            when "0000" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000000000000100000";	-- 1 deg threshold (squared)	
            when "0001" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000000000010000001";	-- 2 deg ^2
            when "0010" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000000001100101001";	-- 5 deg ^2
            when "0011" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000000110010100100";	-- 10 deg ^2
            when "0100" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000001110001110010";	-- 15 deg ^2
            when "0101" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000011001010010001";	-- 20 deg ^2
            when "0110" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000100111100000011";	-- 25 deg ^2
            when "0111" => PHASE_ERROR_SQUARE_THRESHOLD <= "000000111000111000100";	-- 30 deg ^2
            when "1000" => PHASE_ERROR_SQUARE_THRESHOLD <= "000001001101011011101";	-- 35 deg ^2
            when "1001" => PHASE_ERROR_SQUARE_THRESHOLD <= "000001100101001000101";	-- 40 deg ^2
            when others => PHASE_ERROR_SQUARE_THRESHOLD <= "000010000000000000000";	-- 45 deg ^2
        end case;
	end if;
end process;


LOCK_STATUS_006: process(CLK)
begin       
	if rising_edge(CLK) then
		if(PHASE_ERROR_SQUARE_ACC_DIV512(20 downto 0) <= PHASE_ERROR_SQUARE_THRESHOLD) then
			CARRIER_LOCK_local <= '1';	-- carrier tracking loop locked	
		else
			CARRIER_LOCK_local <= '0';	-- carrier tracking loop unlocked
		end if;
	end if;
end process;

CARRIER_LOCK <= CARRIER_LOCK_local;


--// simulation. send to a tab-delimited text file for subsequent (Matlab) plot.
SIM2OUTFILE_X: if(SIMULATION = '1') generate

--	SIM2OUTFILE1: SIM2OUTFILE 
--	generic map(
--		FILENAME => "ct2.txt"
--	)
--	PORT MAP(
--		CLK => CLK,
--		DATA1 => LOOP_ERROR1(19 downto 8),
--		DATA2 => LOOP_ERROR2(19 downto 8),
--		DATA3 => PHASE_ERROR_ACC2(19 downto 8),	-- bias
--		SAMPLE_CLK_IN => OPTIMUM_SAMPLING_CLK_D
--	);

end generate;
 
 
end behavioral;					