function [out,out1]=fe_mpc(varargin)

%FE_MPC	generic handling of multiple point constraints
%	rigid links
%
%	Syntax : [Case,mdof] = fe_mpc(model,Case,mdof);
%
%       Multiple point constraints are linear constraints between
%        DOFs of a model.
%
%
%	See also help rigid, celas, bar1, beam1, ...
%                doc  eltfun, elem0

%       Etienne Balmes
%       Copyright (c) 1990-2003 by SDTools, All Rights Reserved.

if comstr(varargin{1},'cvs')
 out='$Revision: 1.25 $  $Date: 2006/05/10 08:33:36 $'; return;
end

% Get the model, figure out true node locations, ...

model=varargin{1};carg=2;
if ~isfield(model,'Node') error('model must contain nodes'); end  
if isfield(model,'bas')&~isempty(model.bas)
  [node,bas]=basis(model.Node,model.bas);
  [cGL,mech_dof]=basis('trans l',bas,model.Node);
else 
  i1=[1:6]/100;
  mech_dof=i1(ones(size(model.Node,1),1),:)+model.Node(:,ones(6,1));
  mech_dof=mech_dof';mech_dof=mech_dof(:);
  node=model.Node; cGL=[];
end

% Get the Case

Case=[]; if carg<=nargin Case=varargin{carg};carg=carg+1; end
if isempty(Case)|~isfield(Case,'Stack')
 eval('Case=fe_case(model,''getcase'');')
end
if isempty(Case)|~isfield(Case,'Stack')
 Case=struct('Stack',[]); Case.Stack=cell(0,3);
end

% Get the active DOFs if any and append any non mech_dof to mech_dof

if carg<=nargin mdof=varargin{carg};carg=carg+1; 
elseif isfield(model,'DOF')&~isempty(model.DOF)
 mdof=model.DOF;
else
 mdof=feutil('getdof',model);
end
if carg<=nargin; RunOpt=varargin{carg};carg=carg+1; 
else; RunOpt=struct('fixdof',[]);
end

i1=fe_c(mdof,[13:99]'/100,'ind');
if ~isempty(i1) mech_dof=[mech_dof;mdof(i1)]; end

% Allow rigid in Elt rather than case
if isfield(model,'Elt') 
   [i2,el1]=feutil('findelt egid>=0 & eltname rigid ',model);
   if ~isempty(el1)
    Case=stack_rm(Case,'rigid','rigid in .Elt');
    Case.Stack(end+1,1:3)={'rigid','rigid in .Elt',el1};
   end
else el1=[]; end

% join all keepdof entries
if ~isempty(Case.Stack) ind=strmatch('keepdof',comstr(Case.Stack(:,1),-27));
else ind=[];end
if ~isempty(ind)
   i1=[]; for j1=ind(:)'
         r1=Case.Stack{j1,3};  
         if isnumeric(r1) 
         elseif isfield(r1,'data') r1=r1.data;
         elseif ischar(r1) r1=feutil(['findnode' r1(:)'],model);
         else error('Not supported KeepDof format');   end
         i1=[i1;r1(:)];
   end
   ikeep=fe_c(mdof,i1,'ind');
else; ikeep=[]; 
end

NNode=sparse(node(:,1),1,1:size(node,1));

nd=sparse(round(rem(mech_dof,1)*100),fix(mech_dof),1:length(mech_dof),100, ...
    max([fix(max(mech_dof))+1 max(node(:,1))]));
N=full(max(max(nd)));
slave=sparse(size(nd,1),size(nd,2));
II=[]; JJ=[]; TT=[];  % contains rows columns and values of constraint matrix


for j0=1:size(Case.Stack,1)

switch comstr(Case.Stack{j0,1},-27)

case 'rigid' % deal with rigid element - - - - - - - - - - - - - - - - - - - 

% Increment slave DOF list

elt=Case.Stack{j0,3};cEGI=find(finite(elt(:,1)));

i1=abs(num2str(elt(cEGI,3)))-48;
i1=(i1/100+elt(cEGI,2*ones(size(i1,2),1))).*sign(i1);


slave=AddSlave(slave,nd,i1);

i0=find(elt(cEGI,3)>0);
if ~isempty(i0) % standard rigid links
  % tx_slave = tx_master + x(3) * ry_master - x(2)* rz_master
  % ty_slave = ty_master - x(3) * rx_master + x(1)* rz_master
  % tz_slave = tz_master + x(2) * rx_master - x(1)* ry_master

  i1=[elt(cEGI,1)];
  i1=[i1+.01 i1+.02 i1+.03 i1+.04 i1+.05 i1+.06 ... % diagonal terms
    i1+.06 i1+.04 i1+.05 ... % terms in +x
    i1+.05 i1+.06 i1+.04 ... % terms in -x
  ];
  i2=[elt(cEGI,2)];i2=[i2+.01 i2+.02 i2+.03 i2+.04 i2+.05 i2+.06 ... % diagonal
   i2+.02 i2+.03 i2+.01 ... % terms in +x
   i2+.03 i2+.01 i2+.02 ... % terms in -x
  ];
  x = node(NNode(elt(cEGI,2)),5:7)-node(NNode(elt(cEGI,1)),5:7);
  i2=full(slave(round(i2(:)*100)-100));
  r1=[ones(length(cEGI)*6,1);x(:);-x(:)];
  i3=find(i2&r1);

  II=[II;i2(i3)];TT=[TT;r1(i3)];
  i4=full(nd(round(i1(i3)*100)-100));
  JJ=[JJ;i4(:)];

end
i0=find(elt(cEGI,3)<0);
if ~isempty(i0) % DOF equality
  error('DOF equality not reimplemented'); % xxx

  i1=[elt(cEGI,1)];
  i1=[i1+.01 i1+.02 i1+.03 i1+.04 i1+.05 i1+.06 ... % diagonal terms
    i1+.06 i1+.04 i1+.05 ... % terms in +x
    i1+.05 i1+.06 i1+.04 ... % terms in -x
  ];
  i2=[elt(cEGI,2)];i2=[i2+.01 i2+.02 i2+.03 i2+.04 i2+.05 i2+.06 ... % diagonal
   i2+.02 i2+.03 i2+.01 ... % terms in +x
   i2+.03 i2+.01 i2+.02 ... % terms in -x
  ];
  x = node(NNode(elt(cEGI,2)),5:7)-node(NNode(elt(cEGI,1)),5:7);
  i2=full(slave(round(i2(:)*100)-100));i3=find(i2);
  r1=[ones(length(cEGI)*6,1);x(:);-x(:)];

  II=[II;i2(i3)];JJ=[JJ;full(nd(round(i1(i3)*100)-100))];TT=[TT;r1(i3)];
end

% Multiple point constraints
case 'mpc';    

     r1=Case.Stack{j0,3}; 
     if ~isfield(r1,'c')& isfield(r1,'DOF')
       error('Not a proper MPC format');
       % implementation of NASTRAN rbe3
       %elseif ~isempty(findstr(lower(Case.Stack{j0,2}),'rbe3'))
       %  warning('RBE3 currently ignored');
     else % other case of MPC
       i1=[]; 
       if ~isfield(r1,'slave')|isempty(r1.slave);
          [r1.c,r1.slave]=feutil('fixMpcMaster',r1.c);
       else;[r1.c,r1.slave]=feutil('fixMpcMaster',r1.c,r1.slave);
       end
       try; if ~isempty(RunOpt.fixdof)
        i2=fe_c(r1.DOF(r1.slave),RunOpt.fixdof,'ind');
        if ~isempty(i2)
         i2=r1.slave(i2); r2=fe_c(r1.DOF,r1.DOF(i2));
         r1.c=r1.c-r1.c(:,i2)*r2; % remove component on fixed slave
         r1.slave=[]; [r1.c,r1.slave]=feutil('fixMpcMaster',r1.c);
        end
       end;end

       i1=1:size(r1.c,1);i2=r1.slave;
       if norm(r1.c(:,i2)-speye(size(r1.c,1)),'inf')>1e-10
          error('Slave DOFs are not independent');
       end
       i3=[];i3(flipud(i1(:)))=flipud(i2(:));

       r2=sparse(i3,1:length(i3),1,size(r1.c,2),size(r1.c,1))-r1.c';

       slave=AddSlave(slave,nd,r1.DOF(i3));
       [i2,i1,r2]=find(r2);
       i2=full(nd(round(r1.DOF(i2)*100)-100)); % all dofs
       i3=full(nd(round(r1.DOF(i3)*100)-100)); % slave
       i1=i3(i1); 
       II=[II;i1];JJ=[JJ;i2];TT=[TT;r2];
     end

case 'rbe3'

     r1=Case.Stack{j0,3};
     if isfield(r1,'data'); r1=r1.data; end

try;
     for j1=1:size(r1,1)

       % THIS NEEDS DOCUMENTATION/FORMALIZATION XXX
       % i2(:,1) slave [Id;NodeId;Dof], i2(:,2:end) =[weight;DOF;Node] 
       try;
        i2=r1(j1,:);if rem(length(i2),3)~=0;i2(1,ceil(length(i2)/3)*3)=0;end
        i2=reshape(i2,3,length(i2)/3);i2=i2(:,find(any(i2,1)));
        
        if ~isempty(setdiff(abs(sprintf('%i',i2(2,2:end)))-48,1:6))
         % format is alternate
         i2=r1(j1,6:end);i2=i2(find(i2));
         i2=[r1(j1,1:3)' [[r1(j1,4:5)'*ones(size(i2))];i2]];
         if any(rem(i2(3,:),1));
          error('Not proper formatting of rbe3 entry');% just one weight
         end
        end
       catch
        error('RBE3 format is unknown');
       end
       if size(i2,2)==1; 
        sdtw('_nb','%i %i %i : single node RBE3',i2);continue
       end
       i1=NNode(i2(3,2:end)); 
       if any(i1==0); 
        fprintf('Nodes %s not defined\n',sprintf(' %i',i2(3,find(i1==0)+1)));
        error(sprintf('Error in mpc %i',j1));
       end
       Lx=node(NNode(i2(3,2:end)),5:7)- ...
         node(NNode(i2(2))*ones(size(i2,2)-1,1),5:7);
       Lc2=mean(sqrt(sum(Lx.^2,2)))^2;

       W=[1 1 1 Lc2 Lc2 Lc2];
       Kci=zeros(6,6*size(Lx,1)); Kcc=zeros(6);ind=zeros(1,size(Kci,2));
       CDOF=ones(6,1)*[i2(2,1) i2(3,2:end)]+ ...
         [.01;.02;.03;.04;.05;.06]*ones(1,size(i2,2));
       CDOF=CDOF(:); Sci=[eye(6) zeros(6,size(Kci,2))];
       % k=k=[Sc' -I];k=k'*k;k-[Sc*Sc' -Sc;-Sc' eye(6)]

       for j2=1:size(Lx,1)
        i3=(j2-1)*6; 
        Sc=eye(6); Sc([17 6 10])=-Lx(j2,:); Sc([12 16 5])=Lx(j2,:);
        Wi=eye(6); %Wi=diag(W*i2(1,j2+1));
        Kcc=Kcc+Sc*Wi*Sc';
        Kci(:,i3+[1:6])=-Wi*Sc; Sci(:,i3+[7:12])=Sc;
        ind(i3+abs(sprintf('%i',i2(2,j2+1)))-48)=1;
       end

       % condense on master DOFs
       
       Kci=sparse(Kci);Kcc=sparse(Kcc); K=[Kcc Kci;Kci' speye(length(Kci))];
       in1=[1:6 find(~ind)+6]; K1=K(in1,in1);
       r2=null(K1,'r'); 
       if ~isempty(r2); in1=setdiff(in1,find(any(abs(r2(4:6)),2))+3);end
       in2=1:size(K,1); in2(in1)=0; in2=find(in2);
       Q=-K(in1,in1)\K(in1,in2);

       [in1,in3]=intersect(abs(sprintf('%i',i2(3,1)))'-48,in1);
       slave=AddSlave(slave,nd,CDOF(in1));

       [i1,i2,r2]=find(Q(1:length(in1),:));
       i3=full(nd(round(CDOF(in1)*100)-100));i1=i3(i1);    % slave
       i3=full(nd(round(CDOF(in2)*100)-100)); i2=i3(i2); % master

       if isempty(II); II=i1;JJ=i2;TT=r2;
       else; II=[II;i1];JJ=[JJ;i2];TT=[TT;r2];
       end
     end
catch;
  if sdtdef('diag')>10;error(lasterr);end
  sdtw('_nb','RBE3 (%s) computation failed',Case.Stack{j0,2});
end 
case {'keepdof','par','dofload','fsurf','fvol','sensdof','dofset', ...
          'cyclic','fixdof','nastran','sensstrain','info'}

otherwise
 sdtw('_nb','''%s'' not yet supported by fe_mpc',Case.Stack{j0,1});
end



end % loop on stack

% add masters to the II,JJ,TT list - - - - - - - - - - - - - - - - - - - - - -
% one adds masters used to define constraints and masters in mdof


i0=[1:N]'; 
i0(fe_c(mech_dof,mdof,'ind',2))=0; % eliminate masters not in mdof
i1=find(sparse(JJ,1,1));i0(i1)=i1; % reintroduce masters used for constraints
[i1,i2,i3]=find(slave);i0(i3)=0; % get rid of slaves
i1=find(i0);II=[II;i1]; JJ=[JJ;i1];% keep the remaining DOFs
TT=[TT;ones(size(i1))];

% close things up ---------------------------------------------------------

T=sparse(II,JJ,TT,N,N);

% recurse while some masters are slaves

[i1,i2,islave]=find(slave);
i1=mech_dof(find(any(T))); 
i2=full(slave(round(i1*100)-100));i2=i2(find(i2)); i3=length(i2);

while length(i2)>0
 % slave masters
 T1t=T(i2,:);
 T=T+T(:,i2)*(T1t-sparse(1:length(i2),i2,1,length(i2),N));

 i1=mech_dof(find(any(T))); 
 i2=full(slave(round(i1*100)-100));i2=i2(find(i2)); 
 if length(i2)==i3
  disp(fe_c(mech_dof(i2)))
  error('The above slave masters could not be eliminated');
 else i3=length(i2);end
end



if ~isempty(ikeep) % some keepdof
  if ~isempty(cGL)  % allow for non mechanical DOFs in T
   if size(cGL,1)==size(T,2) T=cGL'*T*cGL; 
    else error('This is not yet supported');
   end
  end
  i1=sum(fe_c(mech_dof,mdof(ikeep)));
  i1=find(any(T).*i1);T=T(:,i1);
else
  if ~isempty(cGL)  % allow for non mechanical DOFs in T
   i1=find(any(T));
   if size(cGL,1)==size(T,2) T=cGL'*T(:,i1); 
   else error('This is not yet supported');
   end
  else
   i1=find(any(T));T=T(:,i1);
  end

end

i2=fe_c(mech_dof,mdof,'ind');
i3=fe_c(mdof,mech_dof,'ind',2);
if isempty(i3)
 T=T';T=T(:,i2);Case.T=T';
 %Case.T=T(i2,i1); 
 Case.DOF=mech_dof(i1);
else
 Case.DOF=[mech_dof(i1);mdof(i3)];
 Case.T=[T(i2,:) spalloc(length(i2),length(i3),0);
         spalloc(length(i3),length(i1),0) speye(length(i3))];
end
out=Case; out1=mdof;


% -----------------------------------------------------------------------
function slave=AddSlave(slave,nd,i1);

i1=i1(find(i1>0));
if length(unique(round(i1*100)))~=length(i1) 
  disp(sdtw('_clip 60 1','%s ',fe_c(find(sparse(round(i1*100),1,1)>1)/100)));
  error('Same DOF is slave more than once');
end

i2=full(nd(round(i1*100)-100));

if any(slave(round(i1*100)-100))
  fprintf('\n%s\n\n', ...
   sdtw('_clip 80 1','%s ',fe_c(fix(i1)+round(rem(i1,1)*100)/100)));
  error('Some slave DOFs are already slaves'); 
else
 slave=slave+sparse(round(rem(i1,1)*100),fix(i1),i2,100, ...
    size(slave,2));
end



