%TIME RESPONSE & PHASE PLANE PLOT IN NOISE
%  SINE NONLINEARITY
clear;

% MODIFY PARAMETERS BELOW & GO
%*****************************
% Set either of the following parameters and set the other to zero.
NS = .7227; % N/S in Loop Bandwidth
No = 0; % Noise Power Density, per Hz, to Signal Ratio
SR = 0; % 1 for step response (if also Phinit=Winit=0 and Ap or Af not 0)., 
        % Otherwise start at inital conditions.  For step response, 
        % initial conditions correspond to steady-state conditions with 
        % zero PD output.
Phinit =0;  % radian initial phase
Winit = 0; % radian/sec initial frequency
Ap = 0;      % radian phase input step
Af = 0;      % rad/sec input frequency step
Wn = 1;      % rad/sec natural frequency
z = .707;    % damping factor
alpha = 1;   % Eq. (6.5)
OutInc = .1/Wn;% period between outputs
ending = 100/Wn; % Student Matlab ending/OutInc ¾ 1024(v3.5) or 8192(v4)
SmpPerOut = 10; % samples per output period
Offset = 0;     % offset at PD output (same sign as steady-state output)
truncatePh = 0; % 1:truncate phase; 0:do not
Punits = 1;     % 0:radians  1:cycles  2:degrees
Funits = 0;     % 0:radians/sec   1:Hz
% Choose Output
%  op=1:  Phase Plane = Freq. vs. Phase
%  op=2:  Phase Plane + Phase Error vs. Time
%  op=3:  Frequency + Sampler Output vs. Time
%  op=4:  All of above = 4 plots
op = 4;
%******************************
fprintf('\nLoop with SINUSOIDAL Phase Detector Characteristic')
fprintf('\nWn = %g rad/sec; zeta = %g; alpha = %g',Wn,z,alpha)
Bn = Wn*(1/(2*z) + 2*z*alpha^2)/4; % Noise BW in Hz
fprintf('\nNoise Bandwidth = %g Hz\n',Bn)
% Compute multiplier of noise distribution having unity power.
if NS == 0,
	Dn = No;
	fprintf('Noise Density to Signal Ratio = %g /Hz\n',No)
else
	Dn = NS/Bn;
 fprintf('Signal/Noise in Loop Noise Bandwidth = %g\n',1/NS)
end
Kn = sqrt(Dn*SmpPerOut/(OutInc*2)); 
rand('normal'); %Set noise stats for normal distribution
% Set step response or not and output that choice
%  and other parameters chosen above
if (SR == 1)&(Phinit == 0)&(Winit == 0)&((Ap ~= 0)|(Af ~= 0)),
	stp = 1;
	fprintf('Phase In = %g rad;  Freq In = %g rad/sec;',Ap,Af);
	fprintf(' Offset = %g rad\n',Offset);
	disp('STEP RESPONSE, zero initial phase and frequency');
else
	stp = 0;
	fprintf('Phase In = %g rad;  Freq In = %g rad/sec;',Ap,Af);
	fprintf(' Offset = %g rad\n',Offset);
	disp('STARTING AT INITIAL CONDITIONS');
	fprintf('Init Phase = %g rad;  Init Freq. = %g rad/sec\n',Phinit,Winit);
end % if

last = 1 + ending/OutInc; % index of last output
t = linspace(0,ending,last); % linear sequence of times for plots
dt = (t(2) - t(1))/SmpPerOut; % sample time increment
clg; % clear previous graphs; for v.4, clf #########

%Polynominals for open loop transfer function:
	N = [0     2*z*Wn*alpha   Wn^2]; % numerator
	D = [1   2*z*Wn*(1-alpha)   0 ]; % denominator

[a,b,c,d] = tf2ss(N,D); % elements of dynamic matrix equation
y = Phinit; % initial phase at output

% To help judge inaccuracy of ignoring sampling,
%   compute extra phase shift produced at unity gain
factor = N(2)^2-D(2)^2;
WL = sqrt((factor+sqrt(factor^2+4*N(3)^2))/2); % open-loop gain = 1
SampPhShift = .5*WL*OutInc/SmpPerOut; % radians lag from sampling at WL
fprintf('Added phase due to sampling = %g rad = %g deg.\n',-SampPhShift, -SampPhShift*180/pi)
if truncatePh
	fprintf('Phase will be shown in range ±0.5 cycle, modulo 1 cycle.\n\n')	
else
	fprintf('Phase range will be unrestricted.\n\n')
end

%    BEGIN SIMULATION

NL = sin(Ap-Phinit); % <<<<<<<<<<<< THE NON-LINEARITY
if stp % initial state established with no reference input
	if Offset
		Winit = Offset*Wn/(2*z*(1-alpha));
		   % steady state output frequency due to Offset
	else
		Winit = 0;
	end
	x0(1) = (Winit-N(2)*Offset)/(N(3)-D(2)*N(2));
	x0(2) = -N(2)*x0(1)/N(3);
	e = Ap; % initial phase error
	freq(1) = NL*2*z*Wn*alpha + Winit; 
		  % step from initial value theorem + prior steady state
else % state variables to give specified initial conditions
     % X2 and X3 respectively in Eq. (6.50) and (6.51) with D(3) = 0:
	x0(1) = (Winit-N(2)*(NL+Offset))/(N(3)-D(2)*N(2));
	x0(2) = (Phinit-N(2)*x0(1))/N(3);
	e = Ap-Phinit; % initial phase error
	freq(1) = Winit; % initial frequency at output
end % if
if truncatePh, % truncate e range to ±pi % <<<<<<<<<<<<TRUNCATION
	if e>0
		e = rem(e+pi,2*pi) - pi;
	else
		e = rem(e-pi,2*pi) + pi;
	end
end % if truncatePh
er(1) = e;
[Phi, Gamma] = c2d(a,b,dt); % values for use in solution eqution [see manual]

for i = 1:last-1, % compute loop responses
	for j = 1:SmpPerOut, % loop through unrecorded samples
		PhIn = Ap + Af*dt*(j-.5+(i-1)*SmpPerOut); % phase step plus phase ramp
		NL = sin(PhIn - y); % <<<<<<<<<<<< THE NON-LINEARITY 
		PDn = Kn*rand;
		PDnoise(i+1) = PDn;
		u1(1) = NL + Offset + PDn; % initial input to sampler
		u1(2) = u1(1); %  constant Hold output -> same at beginning and end of dt
		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); % output at end of period is needed
		x0 = x(2,:)'; % final condition gives initial condition for next step
		% For step response, let initial freq equal first computed value
		%  to prevent infinitesimal time at zero from appearing to be finite
		if (stp & i==1 & j==1),
			freq(1) = N(2)*u1(2) + x0(1)*(N(3)-N(2)*D(2));
		end % if
	end % for j
	e = Ap + Af*dt*SmpPerOut*i - y; % untruncated error
	if truncatePh, % truncate e range to ±pi % <<<<<<<<<<<<TRUNCATION
		if e>0
			e = rem(e+pi,2*pi) - pi;
		else
			e = rem(e-pi,2*pi) + pi;
		end
	end % if truncatePh
	er(i+1) = e;
	% corresponding frequency out from Eq. (6.43) with d3 = 0
	freq(i+1) = N(2)*u1(2) + x0(1)*(N(3)-N(2)*D(2)); 
end % for i

% END SIMULATION.  The rest is plotting.

if Punits == 1,
	Pscale = 1/(2*pi);
elseif Punits == 2,
	Pscale = 180/pi;
else,
	Pscale = 1;
end % if Punits

if Funits == 1,
	Fscale = 1/(2*pi);
else
	Fscale = 1;
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
	plot(Pscale*(Ap-er),Fscale*freq,'r')
	title('Phase Plane, Nonlinear Simulation')
	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. out (Hz)');
	else,
	 ylabel('freq. out (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(t,Pscale*er,'g')
	title('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
end % if ()|()

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

	if op == 3, % selecting Sampler Output vs. time
		subplot(212);
	else,
		subplot(224),
	end % if op==3 
	plot(t,sin(er)+PDnoise,'+')
	title('Sampler Output')
	grid
	xlabel('seconds')
end % if ()|()



