function [DAE,q0,dq0,d2q0] = findCIC(M,C,K,b,F,G,q0,dq0,h)
% Checks for DAE and computes matching consistent initial condition.
% Note: Use "minimal" sparss realization to avoid index-2 issues when M
%       is singular.

%  Copyright 2020 The MathWorks, Inc.

nq = size(K,1);
[A,b,~,E,iNZ] = ltipack.mechdata.dss(M,C,K,b,F,G);
ndq = numel(iNZ);
if isempty(E)
   E = speye(nq+ndq);
end
if nargout<4 && ndq==nq
   [DAE,X] = ltipack.util.findCIC(A,E,b,[q0;dq0],h);
   q0 = X(1:nq,:);
   dq0 = X(nq+1:2*nq,:);
else
   % Note: q'(iZ) is in X'(0) when X(0) only contains q'(iNZ)
   [DAE,X,Xp] = ltipack.util.findCIC(A,E,b,[q0;dq0(iNZ)],h);
   q0 = X(1:nq,:);
   % Note: Xp(iNZ) and X(nq+1:nq+ndq,:) should be equal when consistent
   dq0 = Xp(1:nq,:);  dq0(iNZ) = X(nq+1:nq+ndq,:);
   d2q0 = zeros(nq,1);  d2q0(iNZ) = Xp(nq+1:nq+ndq,:);
end