%%%%%%%%%%%%%%costas_PLL

close all
clear all
format  long
%%%%%%%%%%%%%%%%%%%%%% 
aa = input('input original phase of signal(degree):')
bb = input('input doppler of signal(Hz):')
dt = 0.001
t = 0:dt:20
num = length(t)
flo = 40;
df = bb;
A = 1;
fin = flo + df;
PHin = aa * pi/180; %pi/12;
sum_time = 100;%積分時(shí)間
PHase_in = 2 * pi * fin * t + PHin;%[PHin * ones(1,2000),PHin * ones(1,num - 2000) + 180];
%PHase_in = 2 * pi * fin * t + [PHin * ones(1,sum_time),PHin * ones(1,sum_time) + 180,PHin * ones(1,num - 2 * sum_time)];%相位調(diào)制輸入
input_sig = A * sin(PHase_in);
%%%%%%%%%%%%%%
PHout = zeros(1,1);
PHase_out = 2 * pi * flo * t(1:sum_time)  + PHout;
lo_sin = zeros(1,sum_time);
lo_cos = zeros(1,sum_time);
lo_sin(1:sum_time) = sin(PHase_out);
lo_cos(1:sum_time) = cos(PHase_out);
%%%%%%%%%%%%%%%%%%%
% w12 = 0;
% w22 = 0;
w32 = 0; 
w52 = 0;
G = 706;%%%%%%%加快收斂速度
par2 = 10;
par1 = 1;
G1 = par2/par1;
G2 = 1/par1;
G3 = 1;
twopi = 360;

K = 1/2;
I = 0;
Q = 0;
%%%%%%%%%%%%%%%%%%%
for n = 1:1:round(num/sum_time)-1
    w12 = 0;%積分前清零
    w22 = 0;%積分前清零
    pI = I;
    pQ = Q;
    for m = 1:1:sum_time
        nn = (n - 1) * sum_time + m;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%下變頻混頻
        mix_sin(nn) = input_sig(nn) * lo_sin(nn);
        mix_cos(nn) = input_sig(nn) * lo_cos(nn);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%低通濾波器
        w11 = w12 + mix_sin(nn) * dt/2;
        w13 = w11 + w12;
        w12 = w11;
        w21 = w22 + mix_cos(nn) * dt/2;
        w23 = w21 + w22;
        w22 = w21;
    end
    %%%%%%%%%%%%%%%%%%%%%Q路輸出數(shù)據(jù)
    I_sin(n) = sin(w23);
    Q_cos(n) = cos(w13);
    
    %%%%%%%%%%%%%%%%%%%%%鑒頻器
    I = w23;
    Q = w13;
    cross = I * pQ - pI * Q;
    dot = I * I + Q * Q;
    fd = cross/dot;
    fdout(n) = fd/(dt * sum_time);
    %%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%PD鑒相器
    %x = w13 * w23;
 %   x = atan(w23/w13);%2相鑒相
    %x = w23/w13;%
      x = atan2(w23,w13);%4相鑒相
    %%%%%%%%%%%%%%%%%%%%
    xouttemp(n) = x;
    pll_locked_sin(n) = sin(x);
    pll_locked_cos(n) = cos(x);
    %%%%%%%%%%%%%%%%%%%有源比例積分環(huán)路濾波器LF
    w40 = G * sin(x);
    w42 = w40 * G1;
    w41 = w40 * G2 + fd * G3;
    w31 = w32 + w41 * (dt * sum_time)/2;
    w33 = w32 + w31;
    w32 = w31;
    y = w42 + w33;
    %%%%%%%%%%%%%%%%%%%%VCO相位輸出
    w51 = w52 + y * dt /2;
    w53 = w51 + w52;
    w52 = w51;
    PHout = K * w52;
    PHouttemp(n) = PHout;
    %%%%%%%%%%%%%%%%%%%%LUT輸出正弦、余弦
    for m = 1:1:sum_time
        nm = n * sum_time + m;
        PHase_out(nm) = 2 * pi * flo * (nm * dt) + PHout;
        PHase_outtemp(nm) = PHase_out(nm); 
        lo_sin(nm) = sin(PHase_out(nm));
        lo_cos(nm) = cos(PHase_out(nm));
    %%%%%%%%%%%%%%%%%%%%
        lo_sin_out(nm) = lo_sin(nm);
        lo_cos_out(nm) = lo_cos(nm);
    end
end
fdfft = fft(fdout,512);
Pff = fdfft.* conj(fdfft)/length(fdfft)
figure(1)
subplot(2,1,1)
plot((1:num),PHin * 180/pi)
title('original phase of input signal')
xlabel('sample times')
ylabel('degree')
grid on

subplot(2,1,2)
plot((1:num),PHase_in.* 180/pi);
title('phase of input signal');
xlabel('sample times')
ylabel('degree')
grid on

figure(2)
subplot(2,1,1)
plot((1:length(PHouttemp)),PHouttemp.* 180/pi + 15)
title('costas loop vco phase output')
xlabel('sample times')
ylabel('degree')
grid on

subplot(2,1,2)
plot((1:length(PHase_outtemp)),PHase_outtemp.* 180/pi)
title('costas loop track phase')
xlabel('sample times');
ylabel('degree');
grid on

figure(3)
subplot(3,1,1)
plot((1:length(xouttemp)),xouttemp.* 180/pi)
title('costas loop track error')
xlabel('sample times')
ylabel('degree');
grid on

subplot(3,1,2)
plot((1:length(pll_locked_sin)),pll_locked_sin)
title('costas loop locked flag(sin)')
xlabel('sample times');
ylabel('v')
grid on

subplot(3,1,3)
plot((1:length(pll_locked_cos)),pll_locked_cos)
title('costas loop locked flag(cos)')
xlabel('sample times');
ylabel('v');
grid on

figure(4)
subplot(2,1,1)
plot((1:length(lo_sin_out)),lo_sin_out)
title('vco sin output')
xlabel('sample times');
ylabel('v');
grid on
subplot(2,1,2)
plot((1:length(lo_cos_out)),lo_cos_out)
title('vco cos output')
xlabel('sample times');
ylabel('v');
grid on

figure(5)
subplot(2,1,1)
plot((1:length(fdout)),fdout)
grid on

subplot(2,1,2)
ff = 1000 * (-length(fdfft)/2:length(fdfft)/2-1)/(512 * sum_time)
plot(ff,Pff)
grid on

figure(6)
subplot(2,1,1)
plot((1:length(I_sin)),I_sin)
title('data out');
xlabel('sample times');
ylabel('v')
grid on

subplot(2,1,2)
plot((1:length(Q_cos)),Q_cos)
title('data out ')
xlabel('sample times')
ylabel('v')
grid on