function [pd_out,lfvect,uf,vco_phase, vco_out] = spll(sigin, Fs, PD_GAIN,lf_Gain0,lf_Gain1,vco_fc,VCO_GAIN,VCO_AMP)
%                                                                         %
% SPLL()  - Implements a software phase-locked loop.                      %
%                                                                         %
%  Inputs:                                                                %
%     sigin    - signal input vector                                      %
%     Fs       - sampling frequency                                       %
%     PD_GAIN  - gain of the phase detector Volts/rad                     %
%     lf_Gain0 - gain of the Direct Path of the loop filter               %
%     lf_Gain1 - gain of the Integral Path of the loop filter             %
%                (set to 1.0 in test program. Not really needed)          %
%     vco_fc   - center frequency of the vco                              %
%     VCO_GAIN - gain of the VCO in rad/(Volts-sec)                       %
%     VCO_AMP  - amplitude of the VCO                                     %
%                                                                         %
%  Outputs:                                                               %
%     pd_out    - output of the phase detector                            %
%     intvect   - output of the integrator in vector format               %
%     loop_out  - output of the loop filter                               %
%     vco_phase - VCO phase history                                       %
%     vco_out   - output of the VCO                                       %
%-------------------------------------------------------------------------%
Ts = 1/Fs;                                     % sampling period          %
N = length(sigin);                             % number of samples        %
t = [0:Ts:(N*Ts)-Ts];                          % time index for samples   %
pd_out = zeros(N,1);                           % phase detector output    %
vco_phase = zeros(N,1);                        % VCO output               %
uf =  zeros(1,N);
lfvect = zeros(1,N);

% The low pass filter used in this phase-locked loop is a Butterworth     %
% 1st order IIR filter. This filter was designed using Matlab's FDATOOL.  %
% The coefficients are:  Fs = 40 kHz, Fcutoff = 1000 Hz. Pole = a1 and    %
% a Zero = -1.                                                            %
b0 =  0.0729596572;
b1 =  0.0729596572;
a1 = -0.8540806854;

% MAIN LOOP                                                               %
vco_out(1) = VCO_AMP * cos(vco_phase(1));        % initial VCO output     %
pd_out(1)  = PD_GAIN * sigin(1) * vco_out(1);    % initial phase det out  %
uf(1)      = lf_Gain0*(b0*pd_out(1) + b1*0 - a1*0);
lfsum = 0;

for n = 2:N
   % There are various ways to compute the next VCO phase. One way is to  %
   % create a time index nT such that we compute w*(nT), i.e. w*0T, w*1T, %
   % w*2T, etc. Another way is to compute w*T and just add that value     %
   % each time keeping a running total. The end result is the same.       %
   % The TOTAL phase is a running summation of both the frequency term &  %
   % the phase term.                                                      %
   % Another issue for the successful operation of a PLL is the VCO_GAIN. %
   % This gain must account for the sampling time interval. Because this  %
   % time multiplies the output of the LFP the numerical value can be     %
   % very small. For example, at 40 kHz sampling, T = 1/40000 = 25e-6. If %
   % the VCO_GAIN is not large enough this phase term will have no        %
   % noticable affect. A good start is to make the VCO_GAIN equal to the  %
   % sampling frequency.                                                  %
   %                        wc*t               k*uf*T                     %   
   vco_phase_change = (2*pi*vco_fc*Ts) + VCO_GAIN*uf(n-1)*Ts;  
   vco_phase(n)     = vco_phase(n-1) + vco_phase_change;
   
   % To insure that the phase term does not grow to infinity addition is  %
   % performed modulo 2*pi.                                               %
   if(vco_phase(n) > 2*pi)
      vco_phase(n) = vco_phase(n) - 2*pi;
   end;
   vco_out(n) = VCO_AMP * cos(vco_phase(n));
   
   % PHASE DETECTOR                                                       %
   pd_out(n) = PD_GAIN * (sigin(n) * vco_out(n));

   % LOW PASS FILTER DIFFERENCE EQ:  y(n) = b0*x(n)+b1*x(n-1) - a1*y(n-1) %
   uf(n) = lf_Gain0*(b0*pd_out(n) + b1*pd_out(n-1) - a1*uf(n-1));
   uf(n) = uf(n) * lf_Gain1;
   
   % INTEGRATOR. This keeps a running summation of the ouput of the LPF.  %
   lfsum = lfsum + uf(n);

   lfvect(n) = lfsum;        % !!! FOR TESTING !!!                        %
end;    

