function [D,x0,Dq] = scale(D,x0,MREF)
% Scales model data based on MREF with the goal of improving the 
% conditioning of the linear systems in time and frequency response 
% computations. The scaling transforms q to qs such that q = Dq * qs.
% Note: x0 = [q(0);q'(0)] when nonempty.

%   Copyright 2020 The MathWorks, Inc.
k = D.k;
c = D.c;
m = D.m;
nq = size(k,1);
if isempty(m)
   m = speye(nq);
end

if ishermitian(k) && ishermitian(c) && ishermitian(m)
   % Use SYMSCALE
   s = ltipack.util.symscale(MREF);
   ix = 1:nq;
   DS = sparse(ix,ix,s);
   D.m = DS * m * DS;
   D.c = DS * c * DS;
   D.k = DS * k * DS;
   D.b = DS * D.b;
   D.f = D.f * DS;
   D.g = D.g * DS;
   if ~isempty(x0)
      % New displacement vector is DS\q
      x0 = [x0(1:nq,:)./s ; x0(nq+1:2*nq,:)./s];
   end
   Dq = DS;
else
   % Use equilibration
   try
      [P,DR,DC] = equilibrate(MREF,'matrix');
      D.m = DR * P * m * DC;
      D.c = DR * P * c * DC;
      D.k = DR * P * k * DC;
      D.b = DR * P * D.b;
      D.f = D.f * DC;
      D.g = D.g * DC;
      if ~isempty(x0)
         % New displacement vector is DC\q
         [~,~,s] = find(DC);
         x0 = [x0(1:nq,:)./s ; x0(nq+1:2*nq,:)./s];
      end
      Dq = DC;
   catch
      Dq = speye(nq);
   end
end