function D = interface(D,C1,iC1,C2,iC2,KI,CI,METHOD)
% Creates physical coupling interface between two MECHSS models.

%   Copyright 2020 The MathWorks, Inc.

% Sizes
nc = numel(iC1);
nq = size(D.k,1);
[ny,nu] = iosize(D);
RIGID = (isempty(KI) && isempty(CI));

% Get absolute index of shared DOFs into global DOF vector q
StateInfo = D.StateInfo;
ixN1 = getAbsIndex(StateInfo,C1,iC1);
if ~isempty(C2)
   ixN2 = getAbsIndex(StateInfo,C2,iC2);
end


switch METHOD
   case 'primal'
      % Primal assembly
      if RIGID
         if isempty(C2)
            % Eliminate q(ixN1)
            qkeep = true(1,nq);
            qkeep(:,ixN1) = false;
            L = sparse(find(qkeep),1:nq-nc,1,nq,nq-nc);
         else
            % Eliminate q(ixN2) = q(ixN1).
            % L is the identity matrix where block (ixN2,ixN2) is moved to
            % (ixN2,ixN1) position and columns ixN2 are subsequently deleted
            % Note: Make sure that the shared DoFs are q=q(ixN2)=q(ixN1).
            j = 1:nq;
            s = ones(1,nq);
            j(ixN2) = ixN1;
            % Delete zero columns ixN2
            qkeep = true(1,nq);
            qkeep(:,ixN2) = false;
            offsets = cumsum(~qkeep);
            j = j-offsets(j);
            L = sparse(1:nq,j,s,nq,nq-nc);
         end

         if isempty(D.m)
            D.m = L'*L;
         else
            m = L'*D.m*L;
            if ishermitian(D.m)
               D.m = (m+m')/2;
            else
               D.m = m;
            end
         end
         k = L'*D.k*L;
         if ishermitian(D.k)
            D.k = (k+k')/2;
         else
            D.k = k;
         end
         c = L'*D.c*L;
         if ishermitian(D.c)
            D.c = (c+c')/2;
         else
            D.c = c;
         end
         D.b = L'*D.b;
         D.f = D.f*L;
         D.g = D.g*L;
         D.StateInfo = ltipack.util.selectStateInfo(StateInfo,qkeep);
      else
         % Non-rigid
         if isempty(KI)
            KI = sparse(nc,nc);
         end
         if isempty(CI)
            CI = sparse(nc,nc);
         end
         if isempty(C2)
            % Interface with ground
            H = sparse(1:nc,ixN1,ones(nc,1),nc,nq);
         else
            % Interface between pair of components
            ir = 1:nc;
            H = sparse([ir ir],[ixN1 ixN2],[ones(nc,1);-ones(nc,1)],nc,nq);
         end
         k = D.k + H'*KI*H;
         if ishermitian(D.k)
            D.k = (k+k')/2;
         else
            D.k = k;
         end
         c = D.c + H'*CI*H;
         if ishermitian(D.c)
            D.c = (c+c')/2;
         else
            D.c = c;
         end
      end

   case 'dual'
      % Dual assembly
      if RIGID
         nav = nc;   % additional states: lambda
      else
         nav = 2*nc; % additional states: delta,lambda
      end

      % Get absolute index of shared DOFs into global DOF vector q
      if isempty(C2)
         % Interface with ground
         H = sparse(1:nc,ixN1,ones(nc,1),nc,nq);
         D.StateInfo = [StateInfo ; struct('Type',2,'Name',C1+"-Ground",'Size',nav)];
      else
         % Interface between pair of components
         ir = 1:nc;
         H = sparse([ir ir],[ixN1 ixN2],[ones(nc,1);-ones(nc,1)],nc,nq);
         D.StateInfo = [StateInfo ; struct('Type',2,'Name',C1+"-"+C2,'Size',nav)];
      end

      if isempty(D.m)
         ij = 1:nq;
         D.m = sparse(ij,ij,ones(nq,1),nq+nav,nq+nav);
      else
         D.m = ltipack.util.blkdiag(D.m,sparse(nav,nav));
      end
      if RIGID
         % Mq''+Cq'+Kq = Bu+g,  g = -H'*lambda,  H*q = 0
         D.k = [D.k H';H sparse(nc,nc)];
         D.c = ltipack.util.blkdiag(D.c,sparse(nc,nc));
      else
         % Non-rigid connection
         % Mq''+Cq'+Kq = Bu+g,  g = -H'*(KI*delta + CI*delta'),  delta = H*q.
         % Note: Use DAE formulation that preserves symmetry
         if isempty(KI)
            KI = sparse(nc,nc);
         end
         if isempty(CI)
            CI = sparse(nc,nc);
         end
         D.c = ltipack.util.blkdiag(D.c,ltipack.util.blkdiag(CI,sparse(nc,nc)));
         D.k = [D.k sparse(nq,nc) H';sparse(nc,nq) KI -speye(nc);H -speye(nc) sparse(nc,nc)];
      end
      D.b = [D.b ; sparse(nav,nu)];
      D.f = [D.f , sparse(ny,nav)];
      D.g = [D.g , sparse(ny,nav)];
end


%-------------------
function ixN = getAbsIndex(S,C,iN)
% Turn relative indices IN (relative to component's DOFs) into absolute 
% indices IXN into global q vector.
ic = find(strcmp(cat(1,S.Name),C));
nic = numel(ic);
if nic==0
   error(message('Control:combination:interface1',C))
elseif nic>1
   error(message('Control:combination:interface2',C))
end
cs = S(ic).Size; % component size
if min(iN)<1 || max(iN)>cs
   error(message('Control:combination:interface3',C))
end
is = sum([S(1:ic-1).Size]);
ixC = is+1:is+cs;
ixN = ixC(iN);
   