function  [sys, x0]  = sfunpsd(t,x,u,flag,fftpts,npts,HowOften,offset,ts,averaging)
%SFUNPSD an S-function which performs spectral analysis using ffts.
%
%       This M-file is designed to be used in a SIMULINK S-function block.
%       It stores up a buffer of input and output points of the system
%       then plots the power spectral density of the input signal.
%
%       The input arguments are:
%       npts:           number of points to use in the fft (e.g. 128)
%       HowOften:       how often to plot the ffts (e.g. 64)
%       offset:         sample time offset (usually zeros)
%       ts:             how often to sample points (secs)
%       averaging:      whether to average the psd or not
%
%	Two or three plots are given: the time history, the instantaneous psd 
%	the average psd. 
%
%	See also, FFT, SPECTRUM, SFUNC, SFUNTF.

%	Copyright (c) 1990-94 by The MathWorks, Inc.
%	Andrew Grace 5-30-91.
%	Revised Wes Wang 4-28-93, 8-17-93.

if abs(flag) == 2   % Flag equals 2 on a real time hit 
	% Is it a sample hit ?
	sys = x;
	h_fig = x(npts + 2 + averaging * round(fftpts/2) + 1);
	if h_fig <= 0
		% Graph initialization

		[sl_name,blocks] = get_param;
		[n_b, m_b]= size(blocks);
		if n_b < 1
			error('Cannot delete block while simulating')
		end;
		if n_b > 1
			error('Something wrong in get_param, You don''t have the current SIMULINK')
		end;
		% findout if the graphics window exist
		ind = find(sl_name == setstr(10));
		for i = ind
			sl_name(i) = '_';
		end;
		Figures = get(0,'Chil');
		new_figure = 1;
		for i = 1:length(Figures)
			if strcmp(get(Figures(i), 'Type'), 'figure')
				if strcmp(get(Figures(i), 'Name'), sl_name)
					h_fig = Figures(i);
					handels = get(h_fig,'UserData');
					if length(handels) == 9
						new_figure = 0;
						%handels = [h_sub211, h_plot1, h_title, h_sub212, h_plot1, h_title]
						for j=1:3
							set(handels(j*3-2),'Visible','off');
							set(handels(j*3-1),'XData',0,'YData',0,'Erasemode','none');
							set(handels(j*3),  'String','');
							xl = get(handels(j*3-2),'Xlabel');
							set(xl,'String','');
						end;
						xl = get(handels(7),'Ylabel');
						set(xl,'String','');
					end;
				end;
			end;
		end;
		if new_figure
			h_fig = figure('Unit','pixel','Pos',[100 100 400 500],'Name',sl_name);
			set(0, 'CurrentF', h_fig);
			%subplot211
			handels(1) = subplot(311);
			handels(2) = plot(0,0,'m','EraseMode','None');
			handels(3) = get(handels(1),'Title');
			set(handels(1),'Visible','off');
			%subplot212
			handels(4) = subplot(312);
			handels(5) = plot(0,0,'EraseMode','None');
			handels(6) = get(handels(4),'Title');
			set(handels(4),'Visible','off');
			%subplot313
			handels(7) = subplot(313);
			handels(8) = plot(0,0,'EraseMode','None');
			handels(9) = get(handels(4),'Title');
			set(handels(7),'Visible','off');
		end;
		set(handels(6), 'String','Working - Please Wait');
		nstates = npts + 2 + averaging * round(fftpts/2) + 1;
		sys(nstates) = h_fig;
		set(h_fig, 'UserData', handels);
		set(h_fig,'NextPlot','new');
	end;

	if rem(t - offset + 1000*eps, ts) < 10000*eps
		x(1,1) = x(1,1) + 1;
		sys(x(1,1),1) = u;
		div = x(1,1)/HowOften;
		if (div == round(div))
			handels = get(h_fig,'UserData');
			buffer = [sys(x(1,1)+1:npts+1);sys(2:x(1,1))];
			n = fftpts/2;
			freq = 2*pi*(1/ts); % Multiply by 2*pi to get radians
			w = freq*(0:n-1)./(2*(n-1));

			% Detrend the data: remove best straight line fit
			a = [(1:npts)'/npts ones(npts,1)];
			y = buffer - a*(a\buffer);

			% Hanning window to remove transient effects at the
			% beginning and end of the time sequence.
			nw = min(fftpts, npts);
			win = .5*(1 - cos(2*pi*(1:nw)'/(nw+1)));

			g = fft(win.*y(1:nw),fftpts);

			% Perform averaging with overlap if number of fftpts
			% is less than buffer
			ng = fftpts;
			while (ng <= (npts - fftpts))
				g = (g + fft(win.*y(ng+1:ng+fftpts),fftpts)) / 2;
				ng = ng + n; % For no overlap set: ng = ng + fftpts;
			end
			
			g = g(1:n);
			psd = (abs(g).^2)./norm(win)^2;

			tvec = (t - ts * npts + ts * (1:npts));
			set(handels(1),'Visible','on','Xlim',[min(tvec) max(tvec)],'Ylim',[min(buffer*.99) max(buffer*1.01+eps)])
			set(handels(2),'XData',tvec,'YData',buffer)
			set(handels(3),'String','Time history')
			xl = get(handels(1),'Xlabel');
			set(xl,'String','Time (secs)')

			if averaging
				cnt  = sys(npts+2+n);		% Counter for averaging
				sys(npts+2:npts+1+n) = cnt/(cnt+1) *  sys(npts+2:npts+1+n) + psd /(cnt + 1);
				sys(npts+2+n) = sys(npts+2+n) + 1;
				psd = sys(npts+3:npts+1+n);
				tmp = 'Average Power Spectral Density';
			else
				tmp = 'Power Spectral Density';
				psd = psd(2:n);
			end
			ysc = psd(~isnan(psd));
			if isempty(ysc)
				ysc=[0 1];
			else
				ysc = sort([min(ysc*.99), max(ysc*1.01+eps)]);
			end;
			set(handels(4),'Visible','on','Xlim',[min(w(2:n)), max(w(2:n))],'Ylim',ysc);
			set(handels(5),'XData',w(2:n),'YData',psd);
			set(handels(6),'String', tmp);
			xl = get(handels(4), 'Xlabel');
			set(xl,'String','Frequency (rads/sec)')

			%For phase plot,
			phase = (180/pi)*unwrap(atan2(imag(g),real(g)));
			phase = phase(2:n);
			ysc = phase(~isnan(phase));
			if isempty(ysc)
				ysc=[0 1];
			else
				ysc = sort([min(ysc*.99), max(ysc*1.01+eps)]);
			end;
			set(handels(7),'Visible','on','Xlim',[min(w(2:n)) max(w(2:n))],'Ylim',ysc)
			set(handels(8),'XData',w(2:n),'YData',phase)
			set(handels(9), 'String',[tmp '(phase)'])
			xl = get(handels(7), 'Xlabel');
			set(xl, 'String', 'Frequency (rads/sec)')
			yl = get(handels(7), 'Ylabel');
			set(yl, 'String','Degrees')		    
		end
		if sys(1,1) == npts
			x(1,1) = 1;
		end
		sys(1,1) = x(1,1);
	end
elseif flag == 4			% Return next sample hit

	ns = (t - offset)/ts;  % Number of samples
	sys = offset + (1 + floor(ns + 1e-13*(1+ns)))*ts;

elseif flag  == 0  			% Initialization
	nstates = npts + 2 + averaging * round(fftpts/2) + 1; % No. of discrete states
	sys = [0; nstates; 0; 1; 0; 0];  % Sizes of system (see SFUNC)
	x0 = [1; zeros(nstates-1,1)]; 	 % Initial conditions
	if HowOften > npts
		error('The number of points in the buffer must be more than the plot frequency')
	end
	x0(nstates) = 0;
else 					% Other flag options ignored
	sys = [];
end
