function mod = fm_clust(mod,in,out,Par)
% FM_CLUST builds a MIMO static or dynamic fuzzy model from data
% by using product-space fuzzy clustering (Gustafson-Kessel).		
%
% M = FM_CLUST(M,PAR)
%
%	M is the fuzzy model containing the data and the information about
%	the dynamics of the system.
%
%   DAT is a structure with the following fields:
%       U,Y  ...  input and output data matrices, respectively
%       N    ...  numbers of data points per batch (vector)
%                 used only when U and Y consist of several batches
%       Ts   ...  sample time (optional, default 1)
%
%   PAR is a structure with the following fields:
%       n_rules    ...  number of clusters (thus also rules) per output
%       fuzzyness    ...  fuzziness exponent pre output (default 2)
%       tolerance  ...  termination tolerance (default 0.01)
%       seed ...  seed for initialization (default sum(100*clock))
%       rule_type ...  antecedents: x1x - product-space MFS (default)
%                              		x2x - projected MFS
%
%   DYN is a structure with the following fields:
%       Ny   ...  number of delays in y (default 0)
%       Nu   ...  numerator delays in u (default 1)
%       Nd   ...  number of transport delays (default 0)
%
%   FM is a structure containing the fuzzy model (see FMSTRUCT)
%   Mu is a cell array containing the fuzzy partition matrices
%   Z  is a cell array containing the clustered data
%
%  See also FMSIM, FMSTRUCT, GKFAST.

% (c) Robert Babuska, 1997
% modified September 24, 1998 by Antoine Duchateau


%if isfield(Dat,'U'); U = Dat.U; 
%elseif isfield(Dat,'X')'; U = Dat.X;
%elseif isfield(Dat,'x')'; U = Dat.x;
%elseif isfield(Dat,'u')'; U = Dat.u; else U = []; end;
%if isfield(Dat,'Y'); Y = Dat.Y;
%elseif isfield(Dat,'y'); Y = Dat.y; else Y = []; end;
%Ninps = size(U,2); [Nd,NO] = size(Y);
%if isfield(Dat,'Ts'); Ts = Dat.Ts; else Ts = 1; end;
%if isfield(Dat,'file'); file = Dat.file; else file = ''; end;
%if isfield(Dat,'N'); N = Dat.N; else N = Nd; end;

U=get(mod,'data_in');
Y=get(mod,'data_out');
Ninps=get(mod,'n_in');
Nd=size(Y,1);
NO=get(mod,'n_out');
Ts=1;
N=Nd;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get dimensions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Ninps = size(U,2);
%[Nd,NO] = size(Y);

Dyn=get(mod,'dynamics');

if isfield(Dyn,'ny'); ny = Dyn.ny; else ny = zeros(NO,NO); end;
if isfield(Dyn,'nu'); nu = Dyn.nu; else nu = ones(NO,Ninps); end;
if isfield(Dyn,'nd'); nd = Dyn.nd; else nd = zeros(NO,Ninps); end;

if Ninps == 0, nu = zeros(NO,1); nd = nu; end;


if isfield(Par,'n_rules'); c = Par.n_rules; else c = 2; end;
if isfield(Par,'rule_type'); ante = rem(floor(Par.rule_type/10),10); else ante = 1; end;
if isfield(Par,'fuzzyness'); m = Par.fuzzyness; else m = 2; end;
if isfield(Par,'tolerance'); tol = Par.tolerance; else tol = 0.01; end;
if isfield(Par,'seed'); seed = Par.seed; else seed = sum(100*clock); end;

if max(size(c)) == 1, c = c*ones(1,NO); end;
if max(size(m)) == 1, m = m*ones(1,NO); end;
if max(size(ante)) == 1, ante = ante*ones(1,NO); end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% define constants etc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clstep = 1;		% take each clstep-th data point for clustering
show = 0;      % show clustering progress
lstep = 1;		% take each clstep-th data point for least squares
show = 1;   	% show/don't show progress messages
MFTYPE = 2;	% type of membership functions (exponantial)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% loop through the outputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k = 1 : NO,                 % for all outputs

Alist = 1:(sum(ny(k,:))+sum(nu(k,:)));  % antecedent variable list
Clist = [Alist Alist(length(Alist))+1]; % consequent variable list

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% make regression matrix and data matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Nc = [0; cumsum(N(:))];
dat = [];
for b = 1:size(Nc,1)-1,
  if Ninps == 0,
    z = Y(Nc(b)+1:Nc(b+1),:);
  else
    z = [Y(Nc(b)+1:Nc(b+1),:) U(Nc(b)+1:Nc(b+1),:)];
  end;
  [yr,ur,y1]=regres([Y(Nc(b)+1:Nc(b+1),k) z],0,[ny(k,:) nu(k,:)],[ones(1,NO).*(ny(k,:)>0) nd(k,:)]);
  dat = [dat;[ur y1]];                    % data to cluster
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clustering
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (show ~= 0),
  fprintf(['Output ' num2str(k), ', progress: clustering data in ' num2str(c(k)) ' clusters ...\n']);
  drawnow
end;
ND = size(dat,1);
NI = size(dat,2)-1;
dat = dat(1:clstep:ND,:);
rand('seed',seed);
[mu,V,P] = gkfast(dat,c(k),m(k),tol,0);
[dum,ind] = sort(V(:,1));
mu = mu(:,ind);
V = V(ind,:);
P = P(:,:,ind);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Construct M matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% make membership functions (and rules)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mfs = [];
if ante(k) == 2,		% projected MFs
  if (show ~= 0),
    fprintf(1,['                    extracting membership functions ...\n']);
    drawnow
  end;

  range   = [min(dat(:,1:NI))'  max(dat(:,1:NI))'];
  perc = 50;
  safety = perc*0.01*(range(:,2) - range(:,1));
  rlimits = [range(:,1)-safety range(:,2)+safety];

  mfstep = round(ND/100); if mfstep < 1, mfstep = 1; end;
  OPT = foptions; OPT(14) = 1000;
  for i = 1 : NI,
    [ds,fs]=smooth(dat(1:mfstep:ND,i),mu(1:mfstep:ND,:));
    mf = mffit(ds,fs,MFTYPE,OPT,[1 0 0]);
    lim = mf(:,3) == min(mf(:,3)); mf(lim,2:3) = ones(sum(lim),1)*[rlimits(i,1) rlimits(i,1)];
    if mf(lim,4) < rlimits(i,1), mf(lim,4) = rlimits(i,1); end;
    lim = mf(:,4) == max(mf(:,4)); mf(lim,4:5) = ones(sum(lim),1)*[rlimits(i,2) rlimits(i,2)];
    if mf(lim,3) > rlimits(i,2), mf(lim,3) = rlimits(i,2); end;
    mfs{i} = mf;
  end;
end;
rls = (1:c(k))'*ones(1,NI);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute degree of fulfillment
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1 = ones(size(dat,1),1);
if (show ~= 0),
  fprintf(1,['                    computing degrees of fulfillment...\n']);
  drawnow
end;
if ante(k) == 1,              % product-space MFs
   d = [];
	for j = 1 : c(k),                        % for all clusters
   	xv = dat(:,Alist) - x1*V(j,Alist);
		if NI == 1,
	      d(:,j) = M(:,:,j)*xv.^2;            
	   else
	      d(:,j) = sum((xv*M(:,:,j).*xv)')';
	   end;
	end;
	d = (d+1e-100).^(-1/(m(k)-1));		% clustering dist
  dof = (d ./ (sum(d')'*ones(1,c(k))));
  
elseif ante(k) == 2,	% projected MFs
  dof = dofprod(rls(:,Alist),mfs,dat(:,Alist));
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LS estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (show ~= 0),
  fprintf(1,['                    estimating consequent parameters ...\n']);
  drawnow
end;
dc = [dat(:,1:NI) ones(size(dat(:,1)))];
[p,t,t,t,s] = suglms([dc(:,Clist)],dat(:,NI+1),dof,[],0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Construct the FM structure to be returned
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%FM.Ts = Ts; FM.ni = Ninps; FM.no = NO; FM.N = Nd; FM.tol = tol;
%FM.seed = seed; FM.date  = fix(clock);;
%FM.ny = ny; FM.nu = nu; FM.nd = nd;
%FM.ante = ante; FM.c = c; FM.m = m;
%if isfield(Dat,'InputName'); FM.InputName = Dat.InputName; 
%else FM.InputName = []; end;
%if isfield(Dat,'OutputName'); FM.OutputName = Dat.OutputName; 
%else FM.OutputName = []; end;

%FM.Alist{k} = Alist; FM.Clist{k} = Clist; FM.rls{k} = rls; FM.mfs{k} = mfs;
%FM.th{k} = p; FM.s{k} = s; FM.V{k} = V(:,1:end-1); FM.P{k} = P;
%FM.zmin{k} = min(dat); FM.zmax{k} = max(dat);

if ante(k)==1,
	mod=add_rules(mod,c(k),{'productspace' 'inversedist' 'linear'},k);
	set(mod,{k 'centers'},V(:,1:end-1)',{k 'm'},m(1),{k 'ivariances'},M);
	set(mod,{k 'linears'},reshape(p',[1 size(V,2) c(k)]));
	opt=struct('ClusCenters',V','ClusVars',P);
	set(mod,{k 'opt'},opt);
else
	mod=add_rules(mod,c(k),{'projected' 'inversedist' 'linear'},k);
	set(mod,{k 'mfs'},mfs);
	set(mod,{k 'rls'},rls);
	set(mod,{k 'linears'},reshape(p',[1 size(V,2) c(k)]));
	opt=struct('ClusCenters',V','ClusVars',P);
	set(mod,{k 'opt'},opt);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% return partition matrices, cell array if NO > 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargout > 1, if NO > 1, Mu{k} = mu; else Mu = mu; end; end;
if nargout > 2, if NO > 1, Z{k} = dat; else Z = dat; end; end;

end; 

if (show ~= 0),
  fprintf(1,['Done.\n']);
  drawnow
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mfs = mffit(x,f,mfini,OPT,flag)
% MFFIT find optimal parameters of membership function(s) fitted on
% fuzzy set(s) defined point-wise on the universe of discourse.
% Uses FMINS.
%
% MFS = MFFIT(X,F,MFINI,OPT,FLAG)
%
% Input:   X      ... universe of discourse (column or row vector)
%          F      ... fuzzy set(s) defined point-wise on X in columns or
%                     rows of F
%          MFINI  ... initial guess of membership function parameters or
%                     just their type(s) (scalar or column vector). In
%                     the latter case, initial guess is made automatically.
%	   OPT    ... optional parameter, gives access to the FMINS OPTIONS
%		      parameter	
%          FLAG   ... optional optimization parameter,
%                       FLAG(1) - optimize bottoms
%                       FLAG(2) - optimize shoulders
%                       FLAG(3) - optimize both
%
% Outputs: MFS    ... membership functions parameter matrix (see MGRADE)

% (c) Robert Babuska, April 1994

if nargin < 4, OPT = foptions; end;
if ~all(size(OPT)), OPT = foptions; end;
if nargin < 5, flag = [1 0 0]; end;
delta = 0.1;                                    % 1-delta and delta alpha-cuts

[sx,xi] = sort(x);                              % sort X and F accordingly
f = f(xi,:);
[m,n] = size(f);

if size(mfini,2) == 1,                            % no initial MFS supplied
  mftype = mfini.*ones(n,1);
  mfs = ones(n,1)*[sx(1) sx(1) sx(m) sx(m)];      % initialize mfs
  for i = 1 : n,                                  % detect kernels
    peak(i) = mean(find(f(:,i)==max(f(:,i))));
  end;
  for j = 1 : n;                                   % initial MF params
    for i = 1 : peak(j),
      if f(i,j) >= delta, minfl(j) = f(i,j); mfs(j,1) = sx(i); break; end;
    end;
    for i = peak(j) : -1 : 1,
      if f(i,j) <= 1-delta, mfs(j,2) = sx(i); break; end;
    end;
    for i = peak(j) : m,
      if f(i,j) <= 1-delta, mfs(j,3) = sx(i); break; end;
    end;
    for i = m : -1 : peak(j),
      if f(i,j) >= delta, minfr(j) = f(i,j); mfs(j,4) = sx(i); break; end;
    end;
    minx(j) = mfs(j,1); maxx(j) = mfs(j,4); 
    if minfl(j) ~= 1, mfs(j,1) = mfs(j,2) - (mfs(j,2) - mfs(j,1)) ./ (1 - minfl(j)); end
    if minfr(j) ~= 1, mfs(j,4) = mfs(j,3) + (mfs(j,4) - mfs(j,3)) ./ (1 - minfr(j)); end
  end;
  mfs = [mftype mfs];
else                                                    % initial MFS supplied
  mfs = mfini; mftype = mfini(:,1);
  minx = mfs(:,2); maxx = mfs(:,5); 
end;

for i = 1 : n,                                          % optimize per 1 MF
  if flag(1),
    p = fmins('mff',mfs(i,[2,5]),OPT,[],sx(sx>=minx(i) & sx<=maxx(i)),f(sx>=minx(i) & sx<=maxx(i),i),mfs(i,:),1);
    mfs(i,[2,5]) = p;
  end;
  if flag(2),
    p = fmins('mff',mfs(i,[3,4]),OPT,[],sx(sx>=minx(i) & sx<=maxx(i)),f(sx>=minx(i) & sx<=maxx(i),i),mfs(i,:),2);
    mfs(i,[3,4]) = p;
  end;
  if flag(3),
    p = fmins('mff',mfs(i,2:5),OPT,[],sx(sx>=minx(i) & sx<=maxx(i)),f(sx>=minx(i) & sx<=maxx(i),i),mfs(i,:),3);
    mfs(i,2:5) = p;
  end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [p,ym,yl,ylm,ps,S,delta] = suglms(x,y,f,def,flag)
% SUGLMS estimates the consequent parameters in the Sugeno-Takagi model
%    using least squares.
%
% [P,Ym,Yl,Ylm,Ps,S,delta] = SUGLMS(X,Y,F,DEF,FLAG)
%    Input:
%       X ..... input data matrix
%   Y ..... output data vector
%       F ..... fuzzy partition matrix (membership degrees),
%       optional, defaults to ones(size(y)) for
%       which SUGLMS is a standard linear regression
%       DEF ... default value returned when the sum of grades
%               equals to one (optional, defaults to 0)
%   FLAG .. set to 1 to get local weighted LMS estimates
%    Output:
%       P ..... consequents parameters for every cluster
%   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 with NaN's (for plots)
%   S   ... matrix S for use with SUGVAL to produce error estimates on 
%       predictions. If the errors in the data, y, are independent 
%       normal with constant variance, SUGVAL will produce error 
%       bounds which contain at least 50% of the predictions.
%       
%   delta . error estimates for the identification data, y +/- delta 
%       contains at least 50% of the predictions.
%
%    Example:
%   x = (0:0.02:1)'; y = sin(7*x);
%       f = mgrade(x',mfequ(x,2,3))';
%       [p,ym,yl,ylm] = suglms([x ones(size(x))],y,f);
%   subplot(211); plot(x,ylm,'.',x,[y ym]); title('Fitting y = sin(7*x)')
%   subplot(212); plot(x,f); title('Membership functions')

% (c) Robert Babuska, 1994-95

if nargin < 5, flag = 0; end;                           % no flag supplied
if isempty(flag), flag = 0; end;
if nargin < 4, def = 0; end;                            % no default supplied
if isempty(def), def = 0; end;
if nargin < 3, f = ones(size(y)); end;                  % no memberships supplied
if isempty(f), f = ones(size(y)); end;
[mx,nx] = size(x); [mf,nf] = size(f); p = zeros(nf,nx); ps = p;
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
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

if nf == 1, flag = 1; end;

if flag == 0, 

xx = f(:)./sumDOF(:); 
f1(:) = xx(:,ones(1,nx));                               % reshape partition matrix
x1 = f1.*x1;

if nargout > 5,
  [Q,R]=qr(x1);                                         % find LS solution
  p(:) = R\Q'*y;
else
  p(:) = x1 \ y;                                        % find LS solution
end;

if nargout > 1,                     % calculate model output
  yl = x'*p';                       % local models
  ym = x1*p(:) + def.*NoRule(:,1);                      % global model
  ylm = yl;
  mask = find(f < 0.2);                 % find membership degrees < 0.2
  ylm(mask) = NaN*ones(size(mask));             % mask them with NaN's for plots
end;

else % flag = 1

for i = 1 : nf,
  f1 = f(:,i)*ones(1,nx);
  x1 = sqrt(f1).*x';
if nargout > 5,
  [Q,R]=qr(x1);                                         % find LS solution
  p(i,:) = (R\Q'*(sqrt(f(:,i)).*y))';
else
  p(i,:) = (x1 \ (sqrt(f(:,i)).*y))';                   % find LS solution
end;

if nargout > 1,                     % calculate model output
  yl(:,i) = x'*p(i,:)';                     % local models
  ym(:,i) = yl(:,i);
  ylm(:,i) = yl(:,i);
  mask = find(f(:,i) < 0.2);                    % find membership degrees < 0.2
  ylm(mask,i) = NaN*ones(size(mask));           % mask them with NaN's for plots
end;

if nargout > 4,                                         % STD of parameters
  df = mx - nf*nx;                                      % degrees of freedom
  Rr = x1'*x1;
  r = y - x1*p(i,:)';                                      % residuals
  V=r'*r/df;
  M = V*inv(Rr);
  ps(i,:) = sqrt(diag(M))';
end;
end;
end;

if nargout > 5,                                         % make S for error bounds
  S = [R(1:nf*nx,:); ...
      [df zeros(1,nf*nx-1)]; ...
      [norm(r) zeros(1,nf*nx-1)]];                      % construct S as in polyfit
end;

if nargout > 6,                                         % evaluate error bounds
  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 = norm(r)/sqrt(df)*e;
  end
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [sx,fm] = smooth(x,f)
% SMOOTH smooth a fuzzy set defined point-wise on the universe of
% discourse. Can be used e.g. for obtaining membership functions from
% multidimensional fuzzy clusters projected on the individual dimensions.
%
% [SX,FM] = SMOOTH(X,F,CUTOFF)
%
% Input:   X      ... universe of discourse (column vector)
%          F      ... fuzzy partition matrix N x C, N is number of data
%                     points, C is number of clusters
% Outputs: SX     ... sorted universe of discourse
%          FM     ... smoothed fuzzy partition matrix

% (c) Robert Babuska, 1993-94

[sx,xi] = sort(x);                              % sort X and F accordingly
f = f(xi,:);
[m,n] = size(f);

b = [0.1367 0.1367];                            % 1st-order filter
a = [1 -0.7265];

for i = 1 : n,                                  % filter all columns of F
  fi1 = filter(b,a,f(:,i),f(1,i));              % top-bottom
  fi2 = filter(b,a,f(m:-1:1,i),f(m,i));         % bottom-top  
  fi(:,i) = (fi1 + fi2(m:-1:1)) / 2;    			% average
  fi(:,i) = fi(:,i)/max(fi(:,i));    				% normalize
  fi(1,i) = f(1,i);  fi(m,i) = f(m,i);          % remove initial condition
end;

lf = zeros(1,n); rf = lf;
left = zeros(size(f)); right = left;
for i = 1 : m,                                  % peak detector, i.e.
  lf = max(lf,fi(i,:));                         % make F convex by
  rf = max(rf,fi(m-i+1,:));                     % taking its envelope
  right(m-i+1,:) = rf;
  left(i,:) = lf;
end;
fm = min(left,right);

if nargout == 0, plot(sx,f,'o',sx,fm); end;     % plot result

