%function y = trsp(sys,input,tfinal,int,x0)
%
%  Calculates the time response of a SYSTEM matrix:
%      SYS:	SYSTEM or CONSTANT matrix.
%      INPUT:	VARYING input vector or constant.
%      TFINAL:	final time value
%	          (optional, default = input final time).
%      X0:	initial state
%	          (optional, default = 0).
%      INT:	integration step
%		  (optional, default based on input & SYS. it
%		   is recommended that the user specify this,
%		   since the interval selected by TRSP is often
%		   quite small, and can make the calculation
%		   take a long time).
%
%   If INT is specified as 0 TRSP works out a suitable
%   integration step. The output is generated at the selected
%   integration step size.  To reduce the number of output points
%   the user is referred to the function VDCMATE.
%
%   See also: DTRSP, SAMHLD, TUSTIN, VINTERP, and VDCMATE.

function y = trsp(sys,input,tfinal,int,x0)

if nargin == 0,
    disp('usage: y = trsp(sys,input,tfinal,int,x0)')
    return
    end

[type,ny,nu,nx] = minfo(sys);
if type == 'syst',
    if nargin < 5,
        x0 = zeros(nx,1);
    else
        [m,n] = size(x0);
        if m == 1 & n == 1,
             if x0 == 0,
                 x0 = zeros(nx,1);
                 [m,n] = size(x0);
                 end
             end
        if m ~= nx | n ~= 1,
    	    error('incorrect initial state vector dimension')
            return
       	    end
        end
    [a,b,c,d] = unpck(sys);
elseif type ~= 'cons',
    error('the system is not in packed or constant form')
    return
    end

%       check that the input vector is compatible and extract
%       the data.
 
[typeu,nru,ncu,nupts] = minfo(input);
if typeu == 'vary' & ncu == 1,
    [udat,uptr,ut] = vunpck(input);
elseif typeu == 'cons' & ncu == 1,
    nupts = 1;
    udat = input;
    ut = 0;
else
    error('invalid input time vector')
    return
    end

if nru ~= nu, 
    error('incompatible input vector dimension')
    return
    end

%       if an integration step size is supplied interpolate the
%       input to this stepsize.  If one is not supplied then calculate
%       a stepsize.

if nargin < 4,
    int = 0;
    end
intstep = max(0,int);
if intstep == 0,
    if nupts > 1,
        utnzdiff = diff(ut);
        utnzdiff = utnzdiff(find(abs(utnzdiff) > eps));
        if isempty(utnzdiff),
            minstep = 1;
        else
            minstep = min(abs(utnzdiff));
            end
    else
        minstep = 1;
        end
    if type == 'syst'
        maxeig = max(abs(eig(a)));
        intstep = min(minstep,0.1/maxeig);
    else
        intstep = minstep;
        end
    end

if intstep ~= int,
    disp(['integration step size: ' num2str(intstep)])
    end

%       an epsilon value for time interval calculation is created.

teps = intstep*1e-8;

%       Now check (or select) a final time and create the output
%       time vector.  

if nargin < 3,
    tfinal = max(ut);
elseif tfinal < min(ut),
    error('final time less than initial time')
    return
    end
yt = [ut(1):intstep:tfinal].';
nypts = length(yt);

%       Check if the final time is less than (to a tolerance of
%       the integration step size) the maximum time in the input.
%       If it is truncate the input.  Doing this now can save a
%       lot of time in the interpolation.

if max(ut) - yt(nypts) > teps,
    nupts = max(find(ut <= yt(nypts)+teps));
    ut = ut(1:nupts);
    udat = udat(1:uptr(nupts)+nru-1,:);
    end

%       Now interpolate the input vector to the integration
%       step size.   The only case where this is not necessary
%       is when the stepsize is equal the spacing of a
%       regular input vector.

interflg = 0;
if nypts ~= nupts,
    interflg = 1;
elseif max(abs(yt - ut)) > teps,
    interflg = 1;
    end

if interflg == 1,
    uint = zeros(nru*nypts,1);
    disp('interpolating input vector (zero order hold)')
    if nypts <= nupts,
        for i = 1:nypts,
            idx = max(find(ut <= yt(i)+teps));
            uint((i-1)*nru+1:i*nru) = udat((idx-1)*nru+1:idx*nru);
            end
    elseif nupts == 1,
        uint = reshape(udat*ones(1,nypts),nru*nypts,1);
    else
        for i = 1:nupts-1,
            idx = find(yt >= ut(i) & yt < ut(i+1));
            uint((min(idx)-1)*nru+1:max(idx)*nru) = ...
              reshape(udat((i-1)*nru+1:i*nru)*ones(1,length(idx)), ...
              nru*length(idx),1);
            end
        idx = find(yt >= ut(nupts));
        uint((min(idx)-1)*nru+1:max(idx)*nru) = ...
          reshape(udat((i-1)*nru+1:i*nru)*ones(1,length(idx)), ...
          nru*length(idx),1);
        end
    udat = uint;
    end

%       reshape the input so that the matrix multiplications
%       line up.  At this point yt should correspond to ut.

udat = reshape(udat,nru,nypts);
	
%       the input vector has now been expanded to a matrix: u
%       constant systems are treated by a multiplication call
%       For a,b,c,d quadruples a digitization is done.

y = zeros(nypts*ny+1,2);
if type == 'syst',
    x = [x0 zeros(length(x0),nypts-1)];
    end

%       First, the single time point case is dealt with.  Here
%       only the d term of a system and x0 affect the output.

if nypts == 1,
    if type == 'syst',
        y(1:ny,1) = c*x + d*udat;
    else
        y(1:ny,1) = sys*udat;
        end

%       Now the multiple timepoint case is treated.  Here 
%       a check is made to determine if the system has dynamics.
%       The no dynamics case can be handled by a simple multiplication.

elseif type == 'syst',
    ab = [a*intstep b*intstep;zeros(nu,nx+nu)];
    eab = expm(ab);
    ad = eab(1:nx,1:nx);
    bd = eab(1:nx,nx+1:nx+nu);
    for i=2:nypts,
        x(:,i) = ad*x(:,i-1) + bd*udat(:,i-1);
        end
    y(1:ny*nypts,1) = reshape(c*x + d*udat,ny*nypts,1);
else
    y(1:ny*nypts,1) = reshape(sys*udat,ny*nypts,1);
    end

%       The output is reshaped into the usual varying format and 
%       the time is packed with it.  In the case of only a single
%       time point, a varying matrix is still created.  This is
%       after all, assumed to be a time response.

y(1:nypts,2) = yt;
y((ny*nypts)+1,2) = inf;
y((ny*nypts)+1,1) = nypts;
%
% Copyright MUSYN INC 1991,  All Rights Reserved
