% Time
T = 0.5e-7;
T_end = 0.006;
N = fix(T_end/T);
time = T*(0:N-1);
 
%Signal 
 
A=1;
w_c=1.6e9;
w_df=40e3;
phase_i=2*i-pi/4;
Signal=A*cos((w_c+w_df)*time+phase_i);
 
%Initilize PLL Loop 
 
phi_hat(1)=30;  %?????
 
e1(1)=0; 
e2(1)=0; 
phd_output1(1)=0; 
phd_output2(1)=0;
phd_output(1)=0;  
d(1)=0;
 
%VCO
%phi= ;
vco_1=cos(w_c*time); %vco_1=cos(w_c*time+phi); - ????
vco_2=sin(w_c*time);
 
% Signal x VCO
 
 
kp=0.15; %Proportional constant 
ki=0.1;  %Integrator constant 
 
for n=2:length(time)   %n=2:length(Signal) 
 
 
phd_output1(n)=Signal(n)*vco_1(n);        %phd_output1(n)=imag(Signal(n)*vco_1(n));         
phd_output2(n)=Signal(n)*vco_2(n);     
 
e1(n)=e1(n-1)+(kp+ki)*phd_output1(n)-ki*phd_output1(n-1); %Filter integrator 
e2(n)=e2(n-1)+(kp+ki)*phd_output2(n)-ki*phd_output2(n-1);
 
 
 
m1(n)=sign(e1(n));
m2(n)=sign(e2(n));
 
v1(n)=e1(n)*m2(n);
v2(n)=e2(n)*m1(n);
 
 
S(n)=v1(n)-v2(n);

% Time
T = 0.625e-9;
T_end = 0.006;
N = fix(T_end/T);
time = T*(0:N-1);
 
 
A=1;
fc=1.6e9;
wc=2*pi*fc;
fd=40e3;
fs=200e3;
phase_i=(2*i-1)*pi/4;
Signal=A*cos(wc*time+2*pi*fd*time+phase_i);
 
%PLL Loop 
 
phi_hat(1)=30;  
e1(1)=0; 
e2(1)=0; 
phd_output1(1)=0; 
phd_output2(1)=0;
 
d(1)=0;
 
 
vco_1=cos(wc*time); 
vco_2=sin(wc*time);
 
 
kp=0.2958; 
ki=0.0118;   
 
for n=2:length(time)   
 
 
phd_output1(n)=Signal(n)*vco_1(n);    %PhD           
phd_output2(n)=Signal(n)*vco_2(n);     
 
e1(n)=e1(n-1)+(kp+ki)*phd_output1(n)-ki*phd_output1(n-1); %Filter 
e2(n)=e2(n-1)+(kp+ki)*phd_output2(n)-ki*phd_output2(n-1);
 
m1(n)=sign(e1(n));
m2(n)=sign(e2(n));
 
v1(n)=e1(n)*m2(n);
v2(n)=e2(n)*m1(n);
 
 
S(n)=v1(n)-v2(n);
 
 
%Loop filter
d(n)=d(n-1)+(kp+ki)*S(n)-ki*S(n-1);
 
 
 
%VCO
 
vco(n)=conj(exp(j*(2*pi*n*(fc+fd)/fs+phi_hat(n-1)))) ; 
 
phi_hat(n)=phi_hat(n-1)+d(n);    
        
end; 
 
 
figure
plot((time*fd),phi_hat(n) ) % result.???
xlabel('time+fd');
ylabel ('E(n)');
hold on
plot((time*fd),phase_i)
xlabel('time+fd');
ylabel ('E(n)');