%Synthesizer Simulation
%Time Responses and Phase Plane
%Sample & Hold PD, 1 cycle range
%Phase Range Limit Acquisition Aid
 
clear;

% MODIFY PARAMETERS BELOW & GO
%*****************************
Fref = 10000;% Reference frequency in Hz
N1 = 500;    % divide number during simulation
N0 = 400;    % initial divide number
Npre = 8;    % prescaler count contained in N1 (prescaler is not stopped when divider is held)
Nearly = 32; % Variable Ö stops if no new reference Nearly before end
PhaLate = .9;% Variable Ö reset if no sample by PhaLate
PNL = 0.5; % radian final phase, PHA
Kp = 2;    % PD gain
KLF = 1e5;    % Filter DC gain
Fp1 = .001;   % Filter first pole in Hz
Fz = 100;  % Filter zero in Hz
Fp2 = 1000;% Filter second pole in Hz
Kv = 1e6;  % VCO gain
Tend = 2.8e-3;    % Length of simulation.  Student Matlab Fref*Tend ² 1k (v3.5) or 8k (v4) or 16k (v5)
truncatePh = 1; % Phase Plane Plot uses 1 = Phe, 0 = Pha
Punits = 1;     % Plot Phase Units: 0 = radians  1 = cycles  2 = degrees
Funits = 1;     % Plot Frequency Units: 0 = radians/sec   1 = Hz
% Available Outputs
%  op=1:  Phase Plane = Freq. vs. Pha
%  op=2:  Phase Plane + Phase Error vs. Time
%  op=3:  Frequency + PD Output vs. Time
%  op=4:  All of above = 4 plots
op = 3; % Choose Output
ACC = 1e-8; % phase error that ends iteration
%******************************
FNL = N1*Fref; F0 = N0*Fref; K = Kp*KLF*Kv; % final frequency; initial frequency; K 
Pha0 = PNL+(F0 - FNL)/K;
Tref = 1/Fref;
fprintf('\nLoop with 1 cycle Sample-and-Hold Phase Detector');
fprintf('\n   and phase-range-limit acquisition aid.')
fprintf('\nKp = %g V/cycle; KLF = %g; Kv = %g Hz/V;',Kp,KLF,Kv);
fprintf('\nFp1 = %g Hz; Fz = %g Hz; Fp2 = %g Hz;',Fp1,Fz,Fp2);
fprintf('\nFref = %g Hz; N goes from %g to %g; ',Fref, N0, N1);
fprintf('\nPrescaler = %g is part of divide ratio.',Npre);
fprintf('\nEarly Count = %g; Late Phase = %g cycle.',Nearly,PhaLate);
fprintf('\nFrequency from %g Hz to %g Hz;', F0,FNL);
fprintf('\nPhase Error from %g cycle to %g cycle.\n', Pha0,PNL);
twopi = 2*pi;
wp1 = twopi * Fp1;
wp2 = twopi * Fp2;
wz = twopi * Fz;
p = [-wp1,-wp2]; % form a vector of open-loop poles
z = [-wz]; % form a vector of open-loop zeros
Kx = wp1*wp2*K/wz;
ydelta = Nearly/N1; % phase corresponding to counts in Nearly
yearly = 1-ydelta; % phase corresponding to early count

%Polynominals for open loop gain:
[N,Df] = zp2tf(z,p,Kx); % convert zeros, poles, and gain factor to open-loop numerator and denominator.
sd = size(Df); % array size; sd(2) = number of columns = number of poles
D = Df; D(sd(2)+1) = 0; % multiply denominator by s by shifiting left (adding 0 to right)

clf; % clear previous graphs; for v.3.5, use clg #########

[a,b,c,d] = tf2ss(N,D); % elements of dynamic matrix equation
x0 = [0; 0; 0];
yywas = 0;
xf0 = [0; 0];
[af,bf,cf,df] = tf2ss(N,Df);
	
%    BEGIN SIMULATION

TT(1) = 0;
Pha(1) = Pha0;
Phe(1) = Pha0;
if (Pha0>1)|(Pha0<0) % require that Pha0 is in hold-in range
	TT(1) = Tend;
	disp('EXECUTION ABORTED!  Cannot lock under initial conditions.')
end
FF(1) = F0;
i = 1;
T = Tref;

Tnxtref = -Pha(1)*Tref; % Tnxtref means time of next reference
%  transition but, initially, this is for the reference prior to start.
%  This equation implies first sample occurs at time = 0.

while TT(i)<Tend,
	% Compute divider count at next reference
	Tnxtref = Tnxtref + Tref; % time of next reference
	TTatref = Tnxtref - TT(i); % time from last sample to next reference
	[Phi, Gamma] = C2D(a,b,TTatref); % matrices for use in phase solution at next reference
	u1(1) = Phe(i) - Pha0;
	u1(2) = u1(1);
	x = ltitr(Phi,Gamma,u1',x0); % x(2) is state variable solution (for phase) at next reference
	yy = c*x'; % yy(2) is divider input phase (less offset) at next reference
	NN = yy(2) - yywas + TTatref*F0; % divider input phase, which is also value of divide count, at next reference
	y = 0;
	if NN>(N1-Nearly) % Count at next reference would be too high so divider 
	                  %  stopped at N1-Nearly to restart at next reference.
                      % Compute time to complete count after restart at reference.
		yywas = yy(2); % Computations will start from phase at next reference.
		
		% Find how much of the count is left.  With a prescaler, count in prescaler (which continues to divide) 
		% at restart shortens time to sample.  Swallowed pulses [= rem(N1,Npre)] are taken out of NN first,
        % ASSUMING swallows are gone by time count is stopped.
		ydel = ydelta - rem((NN-rem(N1,Npre)),Npre)/N1; % first rem is number in prescaler at next reference.
		x0 = x(2,:)'; % initial state for this segment from final state at refernece
		Tdel = Tref*ydel; % estimate of time for remaining counts
		while abs(y-ydel)>ACC,
			[Phi, Gamma] = C2D(a,b,Tdel); % Matrics for use in solution
			x = ltitr(Phi,Gamma,u1',x0);  % state variable solution (for phase) at each value of t 
			yy = c*x'; % corresponding outputs (d is null for polynominals above)
			y = (yy(2)-yywas + Tdel*F0)/N1;			
			Tdel = Tdel*ydel/y;
		end; %while abs(y-ydel)
		TT(i+1) = Tnxtref + Tdel;
		T = TT(i+1) - TT(i);

	else % compute time at end of cycle of var. divider
		
		while abs(y-1)>ACC,
			[Phi, Gamma] = C2D(a,b,T); % values for use in solution eqution
			x = ltitr(Phi,Gamma,u1',x0); % solutions at period start and end [see manual] 
			yy = c*x'; % corresponding outputs (d is null for polynominals above)
			y = (yy(2)-yywas + T*F0)/N1;
			T = T/y;
		end; %while abs(y-1)
		TT(i+1) = T + TT(i);

	end % computation of var divider period
	yywas = yy(2);
	Pha(i+1) = Pha(i) + T*Fref - 1;

	if (Pha(i+1) > PhaLate), % sample is late; reset variable divider
		% for simplicity, ignore variation in time delay between reset and next clock cycle
		T = (1+PhaLate-Pha(i))*Tref;
		TT(i+1) = T + TT(i);
		[Phi, Gamma] = C2D(a,b,T); % values at next reference time
		x = ltitr(Phi,Gamma,u1',x0); % solutions at period start and end [see manual] 
		yy = c*x'; % corresponding outputs (d is null for polynominals above)
		yywas = yy(2);
		Pha(i+1) = PhaLate;
	end; % if sample is late

	Phe(i+1) = Pha(i+1);
	[Phif, Gammaf] = C2D(af,bf,T);
	xf = ltitr(Phif,Gammaf,u1',xf0);
	yyf = cf*xf';
	FF(i+1) = yyf(2) + F0;
	i = i+1;
	x0 = x(2,:)';
	xf0 = xf(2,:)';
end; %while TT

% END SIMULATION.  The rest is plotting.

if Punits == 1,
	Pscale = 1;
elseif Punits == 2,
	Pscale = 360;
else,
	Pscale = 2*pi;
end % if Punits

if Funits == 1,
	Fscale = 1;
else
	Fscale = 2*pi;
end % if Funits

if op ~= 3, % selecting phase-plane plot
	if op==1,
		subplot(111),
	elseif op==2,
		subplot(211),
	elseif op==4,
		subplot(221),
	end % if op==1
	if truncatePh,
		plot(Pscale*Phe,Fscale*FF,'r')
		title('Phase Plane, Freq. vs. Phe at samples')
	else
		plot(Pscale*Pha,Fscale*FF,'r')
		title('Phase Plane, Freq. vs. Pha at samples')
	end; %if truncate
	grid
	if Punits == 1, % Set display of phase units
		xlabel('phase error (cycles)');
	elseif Punits == 2,
		xlabel('phase error (degrees)');
	else,
		xlabel('phase error (radians)')
	end % if Punits
	if Funits == 1, % Set display of frequency units
		ylabel('freq (Hz)');
	else,
	 ylabel('freq (rad/sec)');
	end % if Funits
end % if op~=3

if (op == 2)|(op == 4), % selecting phase error vs. time
	if op == 2,
		subplot (212),
	else,
		subplot (223),
	end % if op==2
	plot(TT,Pscale*Pha,'g')
	title('Pha, Phase Error at samples')
	grid
	xlabel('seconds')
	if Punits == 1, % Set display of phase units
		ylabel('phase (cycles)');
	elseif Punits == 2,
		ylabel('degrees');
	else,
		ylabel('radians');
	end % if Punits
end % if ()|()

if (op == 3)|(op == 4), 
	if op == 3, % selecting frequency vs. time
		subplot(211);
	else,
		subplot(222),
	end % if op==3 
	plot(TT,Fscale*FF,'b')
	title('Frequency Out at samples')
	grid
	xlabel('seconds');
	if Funits == 1, % Set display of frequency units
		ylabel('Hz');
	else,
	 ylabel('radians / second');
	end % if Funits

	if op == 3, % selecting PD output vs. time
		subplot(212);
	else,
		subplot(224),
	end % if op==3
	mx = length(Phe);
	TT(mx+1) = TT(mx)  + Tref;
	for i = 1:mx
		T(2*i-1) = TT(i);
		T(2*i) = TT(i+1);
		Ph(2*i-1) = Phe(i);
		Ph(2*i) = Phe(i);
	end; %i 
	plot(T,Ph)
	title('Phe, PD Output/(Kp cycle)')
	grid
	xlabel('seconds')
end % if ()|()


