% 本程序用于實現擴頻系統中，信號捕獲之后的跟蹤過程。

% 輸入信號為，捕獲階段得到的多普勒頻率和碼相位
% 程序將進一步找到更精確的頻率和相位，實現載波的跟蹤
% 目前實際多普勒頻率1000，相位86
function Carrier_Trace(fd_estimate, Correl_Peak_location_real)

% 獲取系統參數
[code_cycle,SNR,Ts,T,T_interp,fi,M,N,fd,f0,delay_time,fd_estimate_init,sigma_n,fs,Bandwidth,dot_insert,coef,coef_mod,B,A1]=set_parameter;

%%%%產生接受中頻信號偽碼，抽頭為1，8
code_phase=[1,8];
[inputcode]=code_gen(code_phase);   % 1023大小的GOLD偽碼
%%%%本地偽碼
native_code=inputcode;
inputcode_2=[native_code,native_code];
EmulateIndex = 0;
k = 0;

half_dot_insert = dot_insert/2;

while (1)

    % 模擬輸入的中頻信號
    EmulateIndex = EmulateIndex + 1;
    [signal,signal_noise]=SignalGenerator(EmulateIndex,inputcode,fs,Ts,fi,delay_time,fd,SNR,Bandwidth,code_cycle,sigma_n,f0,N);

    IF_signal(1:N)=signal(1:N);
    IF_signal_noise(1:N)=signal_noise(1:N);
%     % 繪制信號及噪聲
%     figure(1);
%     plot(IF_signal, 'b');  % 繪制生成的信號(含噪聲)
%     hold on;
%     plot(IF_signal_noise, 'r');  % 信號的噪聲
%     hold off;

    % 當前的載波NCO ××× fd_estimate在不斷調整
    [NCO_I_Quanti,NCO_Q_Quanti]=Nco_gen(Ts,fi,fd_estimate,N);
    
    % 下變頻
	Rece_Signal_Quan_down_I = IF_signal(1:N).* NCO_I_Quanti;
	Rece_Signal_Quan_down_Q = IF_signal(1:N).* NCO_Q_Quanti;
	Rece_Signal_Quan_down_I_noise=IF_signal_noise.*NCO_I_Quanti;
	Rece_Signal_Quan_down_Q_noise=IF_signal_noise.*NCO_Q_Quanti;

    %%%%經過低通濾波器，截止頻率為1.023MHz
    [signal_I,signal_Q,signal_I_noise,signal_Q_noise]=LP_filter(N,N+1,code_cycle,fs,Rece_Signal_Quan_down_I,Rece_Signal_Quan_down_Q,Rece_Signal_Quan_down_I_noise,Rece_Signal_Quan_down_Q_noise,EmulateIndex);       %進過低通濾波器
    
%     % 繪制比較，低通濾波器的作用
%     figure(2);
%     plot(Rece_Signal_Quan_down_I, 'b'); % 原始
%     hold on
%     plot(signal_I, 'r');    % 
%     hold off
    
    %%%%采用線性內插
	for index_new=1:dot_insert
        index_previous=floor((index_new-1)*T_interp/Ts)+1;                                                 %產生內插抽取所需的原序列索引值
        Rece_Signal_down_I_1(index_new)=signal_I(index_previous)+round(((signal_I(index_previous+1)-signal_I(index_previous)))*mod(((index_new-1)*coef),coef_mod)/coef_mod);
        Rece_Signal_down_Q_1(index_new)=signal_Q(index_previous)+round(((signal_Q(index_previous+1)-signal_Q(index_previous)))*mod(((index_new-1)*coef),coef_mod)/coef_mod);
	end%%內插完成     
    % 下變頻之后的兩路信號，保存到Rece_Signal_down_I_1，和Rece_Signal_down_Q_1中，4096點
    
    % 獲取本地即時碼，此處依然是連續的兩對GOLD碼，
    Code_native = code_native(inputcode_2,fd_estimate,T_interp,T,f0,dot_insert);

    k = k+1;
    Ips(k) = sum(Code_native(Correl_Peak_location_real:(Correl_Peak_location_real+half_dot_insert-1)).*Rece_Signal_down_I_1(1:half_dot_insert));
    Qps(k) = sum(Code_native(Correl_Peak_location_real:(Correl_Peak_location_real+half_dot_insert-1)).*Rece_Signal_down_Q_1(1:half_dot_insert));
    
	FdQueue(k) = fd_estimate;
	PhaseQueue(k) = Correl_Peak_location_real;
    
    if(k==1)
        continue;
    end

    % 載波環鎖定判決
    E(k) = (Ips(k)*Ips(k-1) + Qps(k)*Qps(k-1))/(Ips(k)*Ips(k) + Qps(k)*Qps(k));
    % 低通濾波-平滑E(k)
    y = 0.9*E(k-1) + 0.1*E(k);
    E(k) = y;
    
    if(E(k) > 0.95)
        % 已經鎖定載波
        % 進入碼相位跟蹤
        if(PN_Phase_Trace(fd_estimate, Correl_Peak_location_real) ~= 0)
            break;
        end;
    end;
    
    Tid = T;   % 積分清除的周期為1023個GOLD碼占的時間
    % 頻率判決
    % 生成結果如：0   52.2740  -42.6017  -52.2780  144.9547   -7.2020
    Delta(k) = (Ips(k-1)*Qps(k)-Ips(k)*Qps(k-1))/(2*pi*Tid*(Ips(k)*Ips(k)+Qps(k)*Qps(k)));    % 公式1
    %Delta(k) = (atan(Qps(k)/Ips(k))-atan(Qps(k-1)/Ips(k-1)))/(2*pi*Tid);    % 公式2
    
    if(abs(Delta(k)) > 10)
        % 頻率牽引---四相鑒頻器矯正
        if(abs(Ips(k)) >= abs(Qps(k)))
            Beta(k) = sign0(Ips(k))*(Qps(k)-Qps(k-1))/sqrt(Ips(k)*Ips(k)+Qps(k)*Qps(k));
        else
            Beta(k) = (-1)*sign0(Qps(k))*(Ips(k)-Ips(k-1))/sqrt(Ips(k)*Ips(k)+Qps(k)*Qps(k));
        end;
        fd_estimate = fd_estimate + Beta(k);
    else
        % 相位判決
        if(abs(Qps(k)/Ips(k)) > 0.176)
            % FLL 鑒頻器，頻率跟蹤
            Efk = atan((Ips(k-1)*Qps(k)-Ips(k)*Qps(k-1))/(2*pi*Tid*(Ips(k)*Ips(k)+Qps(k)*Qps(k))));
            fd_estimate = fd_estimate + Efk;
        else
            % PLL
            Epk = atan2(Qps(k), Ips(k));
            fd_estimate = fd_estimate + Epk;
            %Correl_Peak_location_real = Correl_Peak_location_real + int32(Epk);
        end;
    end;

    if(mod(length(Delta),1000)==1)
        % 繪制多普勒頻率跟蹤的結果
        figure(81);
        plot(FdQueue);
        % 繪制跟蹤過程中的頻率判決
        figure(82);
        plot(E);
    end;
    
end