%TIME RESPONSE & PHASE PLANE PLOT
%CHARGE-PUMP PHASE-FREQUENCY DETECTOR
%Special Third-order Loop (Type 2)
clear;

% MODIFY PARAMETERS BELOW & GO
%*****************************
clear
Fref = 50000;  % Reference frequency in Hz
N1 = 1000;  % divide number during simulation
N0 = 999;   % initial divide number
PNL = 0.0;% radian final phase, Pha
Kp = 2;     % PD gain, U/Cycle (U = v, a, etc.)
KLF = 9870;    % Filter DC gain -> 1/(R1 C) for type 2
Fp1 = 0;    % Filter first pole in Hz
Fz = 500;   % Filter zero in Hz
Fp2 = 2000; % Filter second pole in Hz
Kv = 1e6;   % VCO gain, Hz/U
Tend = .00496;   % Length of simulation.  Student Matlab Fref*Tend ² 1k (v3.5) or 8k (v4) or 16k (v5)
Expand = 1;     % 1: allow, 0: disallow expansion of graphs (no questions to user if 0)
truncatePh = 0; % % Pha displays: 0 = multicycle  1 =  mod 1 cycle
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 = 4; % Choose Output
ACC = 1e-8;  % allowed error in phase change of each divider cycle
reports = 8; % number of time reports during simulation
show = 0;    % 1 = TRUE, 0 = FALSE monitor information
%******************************

FNL = N1*Fref; % finalfrequency
F0 = N0*Fref; % initial frequency
K = Kp*KLF*Kv; %  
Pha0 = PNL; % initial phase same as final for Type 2
Tref = 1/Fref;

fprintf('\nLoop with Charge-Pump Phase-Frequency Detector');
fprintf('\nKp = %g; KLF = %g; Kv = %g;',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('\nFrequency from %g Hz to %g Hz;', F0,FNL);
fprintf('\nPhase from %g cycle to %g cycle;', Pha0,PNL);
fprintf('\nSimulated Time %g seconds\n',Tend);

twopi = 2*pi;
wp1 = twopi * Fp1;
wp2 = twopi * Fp2; % add additional poles below this line & add names to p below
wz = twopi * Fz;
p = [-wp1,-wp2]; % form a vector of open-loop poles
z = [-wz]; % form a vector of open-loop zeros
Kx = wp2*K/wz; % wp1 (=0) removed from Kx for Type 2 (K is now omega-sub-n)
if reports
	Treport = Tend/reports; 
else
	Treport = 2*Tend;
end

%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)
 [a,b,c,d] = tf2ss(N,D); % elements of dynamic matrix equation
 [af,bf,cf,df] = tf2ss(N,Df);


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

%				INITIAL CONDITIONS

x0 = zeros(size(Df)); % zero initial conditions for phase and derivatives
xf0 = x0(1:length(x0) - 1); % zero initial conditions for frequency and derivative
yywas = 0; % prior divider phase (cycles), initally zero
ygoal = 1; % divider phase target during iterations, usually 1
TT(1) = 0; % time at PD transition
TP(1) = 0; % time when phase is determined (not reference time)
Pha(1) = Pha0; % start at initial phase
if Pha0>0, % compute state at end of Late pulse
	u1 = [1 1]; % Phe is 1 during pulse
	Tinit = Pha0*Tref/2; % start at mid pulse, approximately average phase
	[Phi, Gamma] = C2D(a,b,Tinit); % compute change from mid pulse
	x = ltitr(Phi,Gamma,u1'); % phase state at mid pulse and end
	x0 = x(2,:)'; % initial state for next phase computation
	[Phif, Gammaf] = C2D(af,bf,Tinit);
	xf = ltitr(Phif,Gammaf,u1'); % initial freqeuncy state
	Phe(1) = 0; % after pulse
	Tpartial = 0; % partial time for divider period (when computed in 2 segments)
	TTr = (1-Pha0)*Tref; % time of next reference
	T = Tref; % start next iteration with T estimated as divider period
else, % set variables for computation of state at end of early pulse
	Phe(1) = -1; % during Early pulse
	TTr = -Pha0*Tref; % next reference 
end % if Pha0
FF(1) = F0; % FF(j) & PHE(j) occur with TT(j)
F1(1) = F0; % F1(i) & PHA(i) occur with TP(i)
i = 1; % reference period index
j = 1; % divider period index
skipT = -1; %ineffective divider outputs (-1 needed in Pha formula)
RepInt = Treport;

%    BEGIN SIMULATION

while TT(j)<Tend, % while time is less than simulation time

	if Phe(j) > -1, % not in Early pulse
%%%%%%%%%%%%%% Monitor Information
if show, 
where = 1;
	fprintf('\nwhere=1   i=%g   j=%g    Phe=%g    TT=%g',i,j,Phe(j),TT(j));
	fprintf('    yywas=%g',yywas);
end;
%%%%%%%%%%%%%%
		u1(1) = Phe(j) - Pha0; % input to sampler at start of period = held value
		u1(2) = u1(1); %  constant Hold output -> same at beginning and end of period
		T = Tref - Tpartial; % start at steady-state period less what has already been computed
%%%%%% For problem at start with Phe = 0 (Type-2 with PNL = 0)
		if T <= 0,
			T = 0.01*Tref;
		end
%%%%%%
		y = 100; % just to make loop start
		while abs(y-ygoal)>ACC, % ONLY ITERATIVE COMPUTATION OF STATE
			[Phi, Gamma] = C2D(a,b,T); % 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 + (T+Tpartial)*F0)/N1 + ygoal -1;
			% yy(2)-yywas is added to T*F0 to give phase change at divider input.
			% 1-ygoal has already occurred and must be subtracted to give new change.
			if y<=0,
				y = ygoal/10;
			end
			T = T*ygoal/y; % adjust T until yÅygoal	
		end; % iteration;
	
%		if T <= 0, % for some unusual conditions
%			T = Tref*ACC/10
%		end;
				
		TT(j+1) = T + TT(j); % Time of transition
		
	end % if Phe(j)>-1

	if Phe(j) == 1, % END OF LATE PULSE HAS BEEN COMPUTED @ TT(j+1)
%%%%%%%%%%%%%%
if show, 
	where = 2;
	fprintf('\nwhere=2   i=%g   j=%g    Phe=%g    TT=%g',i,j,Phe(j),TT(j));
	fprintf('    yywas=%g',yywas);
end;
%%%%%%%%%%%%%%
		i = i+1; % increment reference cycle
		j = j+1; % increment transition number
		TP(i) = TT(j); % phase determined at end of late pulse
		yywas = yy(2); % divider phase at start of next divider cycle
		TTr = TTr + Tref*(1+floor((TT(j)-TTr)*Fref)); % Next reference time
		[Phif, Gammaf] = C2D(af,bf,T); % for use in frequency solution
		xf = ltitr(Phif,Gammaf,u1',xf0); % for use in frequency solution
		x0 = x(2,:)'; % set initial x for next computation
		xf0 = xf(2,:)'; % set initial xf for next computation
		yyf = cf*xf'; % frequency less offset
		Pha(i) = Pha0 + TP(i)*Fref - i - skipT;  % change = (ref - divider) periods
		if truncatePh,
			Pha(i) = Pha(i) - floor(Pha(i));
		end % if truncatePh
		F1(i) = yyf(2) + F0; % frequency out
		Phe(j) = 0; % at end of late pulse
		Tpartial = 0; % starting new divider cycle
		ygoal = 1; % whole divider cycle time yet to be found

	else, % was not in Late pulse

		if Phe(j) == 0, % AFTER PULSE, COMPUTE START OF NEXT PULSE
%%%%%%%%%%%%%%
if show, 
	where = 3;
	fprintf('\nwhere=3   i=%g   j=%g    Phe=%g    TT=%g',i,j,Phe(j),TT(j));
	fprintf(' yywas=%g  y-1=%g  TTj+1=%g',yywas,y-1,TT(j+1));
end;
%%%%%%%%%%%%%%
			j = j+1; % for next pulse transition

			if TT(j) < TTr, % EARLY PULSE STARTED AT TT(j)
%%%%%%%%%%%%%%
if show, 
	where = 4;
	fprintf('\nwhere=4   i=%g j-1=%g    Phe=%g    TT=%g',i,j-1,Phe(j-1),TT(j-1));
	fprintf('   yywas=%g',yywas);
end;
%%%%%%%%%%%%%%
				i = i+1; 
				TP(i) = TT(j);
				Pha(i) = Pha0 + TP(i)*Fref - i - skipT; % change = (ref - divider) periods
				if truncatePh,
					Pha(i) = Pha(i) - floor(Pha(i)) - 1;
				end % if truncatePh
				yywas = yy(2); % for use in computing next point
				% Compute frequency at start of Early Pulse
					[Phif, Gammaf] = C2D(af,bf,T); % for use in frequency solution
					xf = ltitr(Phif,Gammaf,u1',xf0); % for use in frequency solution
					yyf = cf*xf'; % frequency less offset
					FF(j) = yyf(2) + F0; % frequency out
					F1(i) = FF(j);
				x0 = x(2,:)'; % set initial x for next computation
				xf0 = xf(2,:)'; % set initial xf for next computation
				Phe(j) = -1; % during Early pulse
				Tpartial = 0; % starting new divider cycle
				ygoal = 1;

			else, % if TT(j) ³ TTr % IN LATE PULSE; DEFINED ITS START
%%%%%%%%%%%%%%
if show, 
	where = 5;
	fprintf('\nwhere=5   i=%g j-1=%g    Phe=%g    TT=%g',i,j-1,Phe(j-1),TT(j-1));
	fprintf('    yywas=%g',yywas);
end;
%%%%%%%%%%%%%%		
				TT(j) = TTr; % pulse starts at reference time
				T = TTr-TT(j-1); % divider count time from last transition
				[Phi, Gamma] = C2D(a,b,T); % solution matrix at T
				x = ltitr(Phi,Gamma,u1',x0); % at TTr, existing input and initial state
				yy = c*x'; % to use in getting ygoal below
				[Phif, Gammaf] = C2D(af,bf,T); % for later frequency computation 
				xf = ltitr(Phif,Gammaf,u1',xf0); % at TTr
				x0 = x(2,:)'; % set initial x for next computation
				xf0 = xf(2,:)'; % set initial xf for next computation
				Phe(j) = 1; % during Late Pulse
				Tpartial = Tpartial + T; % divider cycle has gone on for this time
				ygoal = 1 - (yy(2) - yywas + Tpartial*F0)/N1; % phase change for rest of Ö cycle
				
			end % if TT(j)<TTr

		end % if Phe == 0

		if Phe(j) == -1, % within Early pulse; compute state at its end
%%%%%%%%%%%%%%
if show, 
	where = 6;
	fprintf('\nwhere=6   i=%g   j=%g    Phe=%g    TT=%g',i,j,Phe(j),TT(j));
	fprintf('    yywas=%g',yywas);
end;
%%%%%%%%%%%%%%
			T = TTr - TT(j); % Early pulse from prior transition to reference time
			Tpartial = T; % this much time has gone by in the count
			if j==1, % compute change from midpulse initially
				T = T/2; 
			end;
			j = j+1;
			TT(j) = TTr; % transion at reference time
			[Phi, Gamma] = C2D(a,b,T); % values for use in solution eqution [see manual]
			u1(1) = -1 - Pha0; % initial input to sampler
			u1(2) = u1(1); % same at end; constant Hold output
			x = ltitr(Phi,Gamma,u1',x0); % solutions at period start and end
			yy = c*x'; % corresponding outputs (d is null for polynomials above)
			% If more than a partial divider cycle occurs before TTr, remove any whole
			% cycles so next computed time will be after TTr (after Early pulse)
				y = (yy(2)-yywas + T*F0)/N1;
				if j==2, % T was set to T/2 above
					y = 2*y; % 2 times computed y during whole early pulse
				end;
				yywas = yywas + N1*floor(y);
			[Phif, Gammaf] = C2D(af,bf,T); % for use in frequency solution
			xf = ltitr(Phif,Gammaf,u1',xf0); % for use in frequency solution
			x0 = x(2,:)'; % set initial x for next iteration as current x
			xf0 = xf(2,:)'; % set initial x for next iteration as current x	
			Phe(j) = 0; % at pulse end
			skipT = skipT + floor(y); % change number of ineffective divider outputs
			ygoal = 1 - y + floor(y); % phase change still to be found

		end ; % if Phe == -1

		TTr = TTr + Tref; % next reference time

	end % if Phe == 1

	% Now compute frequency corresponding to time of pulse transition
	yyf = cf*xf'; % frequency less offset
	FF(j) = yyf(2) + F0; % frequency out
%%%%%%%%%%%%%%
if show, 
	where = 7;
	fprintf('\nwhere=7   i=%g   j=%g    Phe=%g    TT=%g',i,j,Phe(j),TT(j));
	fprintf('    yywas=%g   i=%g\n',yywas,i);
end;
%%%%%%%%%%%%%%
	if TT(j)>Treport
		fprintf('%g seconds\n',TT(j));
		Treport = Treport + RepInt;
	end % if report

end % while TT(j) < Tend

% 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

imin = 1; jmin = 1; imax = i; jmax = j; % initial indeces for plots

while (imin>0) & (imax>0) % plot; then allow expansion by user
	Tmax = -9; Fmax = -9; Pmax = -9; % code that axes ranges are not yet set

	if (op == 2)|(op == 4), % selecting phase error vs. time
		if op == 2,
			subplot (212),
		else,
			subplot (223),
		end % if op==2 (where to put plot)
		plot(TP(imin:imax),Pscale*Pha(imin:imax),'g')
		title('Pha, Phase Error')
		grid
		xlabel('seconds')
		if Punits == 1, % Set display of phase units
			ylabel('cycles');
		elseif Punits == 2,
			ylabel('degrees');
		else,
			ylabel('radians');
		end % if Punits
		v = axis; Tmin = v(1); Tmax = v(2); Pmin = v(3); Pmax = v(4); % to be used below for common limits
		axis([Tmin Tmax Pmin Pmax]) % set axes limits
	end % if (2)|(4)

	if (op == 3)|(op == 4), 
		if op == 3, % selecting frequency vs. time
			subplot(211);
		else,
			subplot(222);
		end % if op==3 (where to put plot)
		plot(TT(jmin:jmax),Fscale*FF(jmin:jmax),'b')
		title('Frequency Out')
		grid
		xlabel('seconds');
		if Funits == 1, % Set display of frequency units
			ylabel('Hz');
		else,
			ylabel('radians / second');
		end % if Funits
		v = axis; % get axes limits
		Fmin = v(3); Fmax = v(4); % set frequency axis limits
		if Tmax == -9, % if time axis limits not yet set
			Tmin = v(1); Tmax = v(2); % set time axis limits
		end
		axis([Tmin Tmax Fmin Fmax]) % set axes limits
		
		if op == 3, % selecting PD output vs. time
			subplot(212);
		else,
			subplot(224);
		end % if op==3
		mx = length(Phe);
		for ii = 1:mx-1 % create new data points to make display stepped
			T(2*ii-1) = TT(ii);
			T(2*ii) = TT(ii+1);
			Ph(2*ii-1) = Phe(ii);
			Ph(2*ii) = Phe(ii);
		end; %ii 
		plot(T(2*jmin-1:2*jmax-2),Ph(2*jmin-1:2*jmax-2),'m');
		title('Phe, PD Output/(Kp cycle)')
		grid
		xlabel('seconds')
		if Tmax ~= -9, % if time axis limits have been set
			axis([Tmin Tmax -1.1 1.1]) % set time and Phe axes limits
		else
			axis([-inf inf -1.1 1.1]) % set Phe limits only
		end
	end % if (3)|(4)

	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 (where to put plot)
		plot(Pscale*Pha(imin:imax),Fscale*F1(imin:imax),'r')
		title('Phase Plane, Freq. vs. Pha')
		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
		v = axis; % vector of axes limits
		if Pmax == -9, % if phase axis limits not yet set
			Pmin = v(1); Pmax = v(2); % set phase axis limits
		end
		if Fmax == -9, % if freq axis limits not yet set
			Fmin = v(3);Fmax = v(4); % set phase axis limits
		end
		axis([Pmin Pmax Fmin Fmax]) % set axes limits
	end % if op~=3

	pmax = 100; pmin = 0; doAgain = Expand;
	fprintf('\n');
	% allow user to expand graphs by limiting range of plotted data
	while doAgain % set start values
		pmin0 = input('To expand, enter start time as % (<100) of maximum (return = no change; <0 to quit) ');
		if ~isempty(pmin0) % to retain value if user hits return
			pmin = pmin0;
			imin = floor(pmin*i/100)+1; jmin = floor(pmin*j/100)+1;
		end
		if imin<i % if i is too high, repeat input
			doAgain = 0;
		end
	end % doAgain for min
	doAgain = Expand;
	while doAgain, % set end values
		pmax0 = input('To expand enter stop time as % (<100) of maximum (return = no change; <0 to quit) ');
		if ~isempty(pmax0) % to retain value if user hits return
			pmax = pmax0;
			imax = floor(pmax*i/100); jmax = floor(pmax*j/100);
		end
		if (imin >= imax) & (imax > 0),
			disp('  Maximum must be greater than minimum and less than 100 (%)')
		end
		if ((imin < imax) & (imax <= i)) | imax < 0 % if max not in range, repeat input
			doAgain = 0;
		end
	end % doAgain for max
	imin = imin * Expand; % end display loop if Expand == 0

end % imax, imin > 0



