function [Ym,q,DOF,Yl,Ylm] = fmsim(U,Y,FM,Ymin,Ymax,show,H)
% FMSIM simulate a MIMO fuzzy model.
%
% [Ym,q,DOF,Yl,Ylm] = FMSIM(U,Y,FM,Ymin,Ymax,show,H)
%
%       U,Y  ... input and output data matrices, respectively
%                (output data is needed for initialization
%                and for comparison)
%       FM   ... structure containing fuzzy model parameters
%       Ymin ... lower bound constraint on Y (optional)
%       Ymax ... upper bound constraint on Y (optional)
%       show ... set to 1 for on-line graphics, 2 for final plot only,
%		 and 0 for no graphics (optional, default 1)
%       H    ... prediction horizon (optional, by default simulation)
%
%       Ym  ... simulated output of the model
%       q   ... performance index of the model computed as VAF
%               (variance accounted for)
%       DOF ... degrees of fulfillment of the rules (all outputs
%               appended)
%       Yl .... output of the individual rule's consequents
%       Ylm ... output of the individual rule's consequents
%               (data with DOF < 0.5 are masked by NaN)
%
%  See also FMCLUST, FMSTRUCT, GKFAST, VAF.

% (c) Robert Babuska, 1997

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% extract data from the FM structure (much faster!)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ts = FM.Ts;
Ninps = FM.ni;
NO = FM.no;
ny = FM.ny;
nu = FM.nu;
nd = FM.nd;
atype = FM.ante;
c = FM.c;
m = FM.m;
for k = 1 : NO,
  Nr(k) = size(FM.rls{k},1);
  Nm(k) = size(FM.mfs{k},1);
  Alist = FM.Alist{k};
  NI = length(Alist);
  P = FM.P{k}; M = zeros(NI,NI,c(k));
  for j = 1 : c(k),
     %  M(:,:,j) = det(P(:,:,j)).^(1/(NI+1))*inv(P(Alist,Alist,j));
     M(:,:,j) = det(P(Alist,Alist,j)).^(1/length(Alist))*inv(P(Alist,Alist,j));
     %  M(:,:,j) = inv(P(Alist,Alist,j));
  end;
  FM.M{k} = M;
end;
NI = sum([ny'; zeros(1,NO)]) + sum([nu'; zeros(1,NO)]);

if isempty(Y), Y = zeros(size(U,1),NO); end;
if nargin < 4, Ymin = -inf*ones(1,NO); elseif isempty(Ymin), Ymin = -inf*ones(1,NO); end;
if nargin < 5, Ymax =  inf*ones(1,NO); elseif isempty(Ymax), Ymax =  inf*ones(1,NO); end;
if nargin < 6, show = 1; elseif isempty(show), show =  1; end;
if nargin < 7, H = 0; elseif isempty(H), H =  0; end;

DOF = zeros(size(U,1),sum(Nr));
Yl  = DOF; Ylm  = Yl;
cind = [0 cumsum(c)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% intialize graphics
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if show == 1,
  clf;
  if ~(max(max(ny)) == 0 | NI == 1 | H ~= 0),
    for i = 1:NO,
      subplot(NO,1,i); 
      plot(Ts*(1:length(Y(:,i))),Y(:,i));
      set(gca,'xlim',Ts*[1 size(Y,1)]);
      xlabel('Time [s]'); ylabel(['Output' num2str(i)]); drawnow; hold on; 
    end;
  end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% simulation loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if max(max(ny)) == 0,	% static model

for kk = 1 : NO,

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% extract individual MISO models from the FM structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  Alist = FM.Alist{kk};
  Clist = FM.Clist{kk};
  p     = FM.th{kk};
  V     = FM.V{kk};
  M     = FM.M{kk};
  rls   = FM.rls{kk};
  mfs   = FM.mfs{kk};

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% constants
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  M1 = zeros(NI(kk)); 
  V1 = ones(c(kk),1);
  x1 = ones(size(U,1),1);

  ante = U; cons = [ante ones(size(ante,1),1)];
  
  if atype(kk) == 1,		% product-space MFs
  for j = 1 : c(kk),                        % for all clusters
    xv = ante(:,Alist) - x1*V(j,Alist);
    M1 = M(:,:,j);			% this won't work for general Alist
    if NI == 1,
      d(:,j) = M1*xv.^2;
    else
      d(:,j) = sum((xv*M1.*xv)')';
    end;
  end;
  d = (d+1e-100).^(-1/(m(kk)-1));		% clustering dist
  dofe = (d ./ (sum(d')'*ones(1,c(kk))));
% dofe = 1./(1+d);				% similarity
  elseif atype(kk) == 2,	% projected MFs
    dofe = dofprod(rls(:,Alist),mfs,ante(:,Alist));
  end;
  
  [Ym(:,kk),Yl(:,cind(kk)+(1:c(kk))),Ylm(:,cind(kk)+(1:c(kk)))] = sugval(p(:,Clist),cons(:,Clist),dofe);
  DOF(:,cind(kk)+(1:c(kk))) = dofe;

end;

elseif H > 0,			% dynamic model, H-step-ahead simulation

N = size(Y,1);
Ym = Y;

for kk = 1 : NO,                 % for all outputs

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% extract individual MISO models from the FM structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  Alist = FM.Alist{kk};
  Clist = FM.Clist{kk};
  p     = FM.th{kk};
  V     = FM.V{kk};
  M     = FM.M{kk};
  rls   = FM.rls{kk};
  mfs   = FM.mfs{kk};

  if Ninps == 0,
    z = Y;
  else
    z = [Y U];
  end;
  [yr,ur,y1]=regres([Y(:,kk) z],0,[ny(kk,:) nu(kk,:)],[ones(1,NO).*(ny(kk,:)>0) nd(kk,:)]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% constants
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  M1 = zeros(NI(kk)); 
  V1 = ones(c(kk),1);
  x1 = ones(size(ur,1),1);

  ante = ur; cons = [ante ones(size(ante,1),1)];

  if atype(kk) == 1,		% product-space MFs

  d = [];
  for j = 1 : c(kk),                        % for all clusters
    xv = ante(:,Alist) - x1*V(j,Alist);

    M1 = M(:,:,j);			% this won't work for general Alist
    if NI == 1,
      d(:,j) = M1*xv.^2;
    else
      d(:,j) = sum((xv*M1.*xv)')';
    end;
  end;

  d = (d+1e-100).^(-1/(m(kk)-1));		% clustering dist
  dofe = (d ./ (sum(d')'*ones(1,c(kk))));
%  dofe = 1./(1+d);				% similarity
  elseif atype(kk) == 2,	% projected MFs
    dofe = dofprod(rls(:,Alist),mfs,ante(:,Alist));
  end;

  s1 = size(dofe,1);
  [Ym(N-s1+1:N,kk),Yl(N-s1+1:N,cind(kk)+(1:c(kk))),Ylm(N-s1+1:N,cind(kk)+(1:c(kk)))] = sugval(p(:,Clist),cons(:,Clist),dofe);
  DOF(N-s1+1:N,cind(kk)+(1:c(kk))) = dofe;

end;

else 				% dynamic model, simulation from input

if Ninps > 0, 
  k0 = max(max(max(max(ny))+1,max(max(nu))+max(max(nd))),2);
  if size(U,1) < k0, error(['Supply at least ' int2str(k0) ' data samples.']); end;
else
  k0 = max(max(max(ny))+1,2);
end;

if Ninps > 0, kmax = size(U,1); else kmax = size(Y,1); end;

Ym = Y;
for k = k0 : kmax,

for kk = 1 : NO,

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% extract individual MISO models from the FM structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  Alist = FM.Alist{kk};
  Clist = FM.Clist{kk};
  p     = FM.th{kk};
  V     = FM.V{kk};
  M     = FM.M{kk};
  rls   = FM.rls{kk};
  mfs   = FM.mfs{kk};

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% constants
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
M1 = zeros(NI(kk)); 
V1 = ones(c(kk),1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% construct regressors
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  ante = [];
  for j = 1 : NO,
    ante = [ante Ym(k-1:-1:k-ny(kk,j),j)'];
  end;
  for j = 1 : Ninps,
    ante = [ante U(k-nd(kk,j):-1:k-nu(kk,j)-nd(kk,j)+1,j)'];
  end;
  cons = [ante 1];
  if atype(kk) == 1,		% product-space MFs
    xv = V1*ante(Alist) - V(:,Alist);
    d = [];
    for j = 1 : c(kk),
    	M1 = M(:,:,j);			% this won't work for general Alist
      d(j) = xv(j,:)*M1*xv(j,:)';
    end;
  d = (d+1e-100).^(-1/(m(kk)-1));		% clustering distance
  dofe = (d ./ (sum(d')'*ones(1,c(kk))));
%    dofe = 1./(1+d);				% similarity

  elseif atype(kk) == 2,	% projected MFs
    dofe = dofprod(rls(:,Alist),mfs,ante(Alist));
 end;
 
 ds = sum(dofe);
  NoRule = ds == 0;          	% no rule applicable
  ds = ds + NoRule;             % set zeros to one
  yl = cons(Clist)*p(:,Clist)';	% local models
  ylm = yl;
  mask = find(dofe < max(dofe)*ones(1,Nr(kk)));   % find the largest membership degree
  ylm(mask) = NaN*ones(size(mask));		% mask with NaN's for plots

  Ym(k,kk) = yl*(dofe/ds)' + Ym(k-1,kk)*NoRule; % global model
  Yl(k,cind(kk)+(1:c(kk))) = yl;
  Ylm(k,cind(kk)+(1:c(kk))) = ylm;
  DOF(k-1,cind(kk)+(1:c(kk))) = dofe;

  if Ym(k,kk) < Ymin(kk), Ym(k,kk) = Ymin(kk); end;
  if Ym(k,kk) > Ymax(kk), Ym(k,kk) = Ymax(kk); end;

  if show == 1,
    subplot(NO,1,kk);
    plot(Ts*[k-1 k],[Ym(k-1,kk) Ym(k,kk)],'m-','EraseMode','none'); drawnow;
  end;
end;
end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% final plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if show,
clf; hold off;
for i = 1:NO,
  subplot(NO,1,i); 
if max(max(ny)) == 0 & NI == 1, 	% static SISO model
  plot(U,Y(:,i),'b-',U,Ym(:,i),'m-.');
  xlabel('Input'); ylabel(['Output' num2str(i)]); drawnow;
else					% dynamic model
  plot(Ts*(1:length(Y(:,i))),Y(:,i),'b-',Ts*(1:length(Ym(:,i))),Ym(:,i),'m-.');
  xlabel('Time [s]'); ylabel(['Output' num2str(i)]); drawnow;
end;
end;
end;

if size(Y,1)==size(Ym,1) & size(Ym,1)>1, q = vaf(Y,Ym); end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [ym,yl,ylm,delta] = sugval(p,x,f,def,S)
% SUGVAL calculates the output of the Sugeno-Takagi model.
%
% [Ym,Yl,Ylm] = SUGVAL(P,X,F)
%    Input:
%       P .....	consequents parameters for every cluster
%               (obtained for instance with SUGLMS).
%       X ..... input data matrix.
%       F ..... fuzzy partition matrix (membership degrees),
%		optional, defaults to ones(size(x,1),1) for
%		which SUGVAL is a standard linear model
%       DEF ... default value returned when the sum of grades
%               equals to one (optional, defaults to 0)
%	S   ... optional matrix generated by SUGLMS to produce error 
%		estimates on  predictions, see SUGLMS for details. 
%
%    Output:
%	Ym ....	global model output for the given input data
%	Yl ....	output of local submodels (corresponding to clusters)
%	Ylm ...	output of local submodels with data corresponding to
%		degrees < 0.2 masked (for plots)
%	delta . error estimates, y +/- delta contains at least 50% 
%		of the predictions.

% (c) Robert Babuska, January 1994

if nargin < 4, def = 0; end;              	% no default supplied
if isempty(def), def = 0; end;
if nargin < 3, f = ones(size(x,1),size(p,1)); end;          % no memberships supplied
if isempty(f), f = ones(size(x,1),size(p,1)); end;
[mx,nx] = size(x); [mf,nf] = size(f);
sumDOF = sum([zeros(1,mf);f'])';		% sum of DOFs
sumDOF = sumDOF(:,ones(1,nf));
NoRule = sumDOF == 0;                           % no rule applicable
sumDOF = sumDOF + NoRule;                       % set zeros to one
yl = x*p';                                      % local models
ym = sum([zeros(1,mx); ...
         (yl.*f./sumDOF)'])' + def.*NoRule(:,1); % global model
ylm = yl;
mask = find(f < max(f')'*ones(1,nf));   % find the largest membership degree
ylm(mask) = NaN*ones(size(mask));		% mask with NaN's for plots

if nargin > 4 & nargout > 3,
    nc = nf*nx;
    [ms,ns] = size(S);
    if (ms ~= ns+2) | (nc ~= ns)
        error('S matrix must be n+2-by-n where n = length(p(:))')
    end
    R = S(1:nc,1:nc);
    df = S(nc+1,1);
    normr = S(nc+2,1);

    x1 = zeros(mx,nf*nx); 	 			% auxilliary variables
    xx = zeros(nx,nf*mx);                   		% auxilliary variable
    x = x'; f1 = x(:); xx(:) = f1(:,ones(1,nf));
    f1 = x1;
    x1(:) = xx';                                        % reshape data matrix
    xx = f(:)./sumDOF(:);
    f1(:) = xx(:,ones(1,nx));                           % reshape partition matrix
    x1 = f1.*x1;

    E = x1/R;
    e = sqrt(1+sum((E.*E)')');
    if df == 0
        disp('Warning: zero degrees of freedom implies infinite error bounds.')
        delta = Inf*e;
    else
        delta = normr/sqrt(df)*e;
    end
end;

