%
% Code to simulate staircase input
% 
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function [y,t,x] = vansim(sys,A,T,x0,N)
% vansim simulates the time response of a LTI system to a series of 
% step inputs whose amplitudes are given by the vector A and the time
% of application by the vector T

ni = nargin;
no = nargout;
error(nargchk(4,7,ni));

[Asys,Bsys,Csys,Dsys] = ssdata(sys);
%keyboard
[nr,nc] = size(Bsys);
P = [Asys Bsys;zeros(nc,length(Asys)+nc)];

% Generate the time vector for each interval
for indx = 1:length(T)-1,
    tmat(:,indx) = linspace(T(indx),T(indx+1),501)';
end;

% Generate the system resonse to the multi-step input
timeindx = 1;
for indx = 1:length(A)-1,
    Asum = sum(A(:,1:indx));
    for indt = 1:length(tmat(:,indx))
        M = expm(P*(tmat(indt,indx)-T(indx)));
        %keyboard
        tvec(timeindx) = tmat(indt,indx);
        xmat(timeindx,:) = (M(1:length(Asys),1:length(Asys))*x0 + M(1:length(Asys),length(Asys)+1:end)*Asum)';
        ymat(timeindx,:) = Csys*xmat(timeindx,:)'+Dsys*Asum;
        timeindx = timeindx+1;
    end;
    x0 = xmat(timeindx-1,:)';
end;

if exist('N') ~=0
    tvec = tvec(:);
    indrep = find(diff(tvec)~=0);
    t = linspace(0,T(1,end),N)';
    x = interp1(tvec(indrep),xmat(indrep,:),t);
    y = interp1(tvec(indrep),ymat(indrep,:),t);
else
    t = tvec(:);
    x = xmat;
    y = ymat;
end;

