function W = emgr(f,g,s,t,w,pr,nf,ut,us,xs,um,xm,dp)
%% emgr - EMpirical GRamian Framework
%
%  project: emgr ( https://gramian.de )
%  version: 5.99 (2022-04-13)
%  authors: C. Himpe (0000-0003-2194-6754)
%  license: BSD-2-Clause (opensource.org/licenses/BSD-2-Clause)
%  summary: Empirical system Gramians for (nonlinear) input-output systems.
%
% DESCRIPTION:
%
%  Empirical gramian matrix and empirical covariance matrix computation
%  for model reduction, decentralized control, nonlinearity quantification,
%  sensitivity analysis, parameter identification, uncertainty quantification &
%  combined state and parameter reduction of large-scale input-output systems.
%  Data-driven analysis of input-output coherence and system-gramian-based
%  nonlinear model order reduction. Compatible with OCTAVE and MATLAB.
%
% BRIEF:
%
%  Unsupervised learning of I/O system properties for data-driven control.
%
% ALGORITHM:
%
%  C. Himpe (2018). emgr - The Empirical Gramian Framework. Algorithms 11(7):91
%  <https://doi.org/10.3390/a11070091 doi:10.3390/a11070091>
%
% USAGE:
%
%  W = emgr(f,g,s,t,w,[pr],[nf],[ut],[us],[xs],[um],[xm],[dp])
%
% MANDATORY ARGUMENTS:
%
%   f {handle} vector field: x' = f(x,u,p,t)
%   g {handle} output functional: y = g(x,u,p,t)
%   s {vector} system dimensions: [inputs, states, outputs]
%   t {vector} time discretization: [time-step, time-horizon]
%   w  {char}  empirical gramian type:
%    * 'c' empirical controllability gramian (Wc)
%    * 'o' empirical observability gramian (Wo)
%    * 'x' empirical cross gramian (Wx aka Wco)
%    * 'y' empirical linear cross gramian (Wy)
%    * 's' empirical sensitivity gramian (Ws)
%    * 'i' empirical identifiability gramian (Wi)
%    * 'j' empirical joint gramian (Wj)
%
% OPTIONAL ARGUMENTS:
%
%  pr {matrix|0} parameter vector(s), each column is one parameter sample
%  nf {vector|0} option flags, thirteen component vector, default all zero:
%    * centering: none(0), steady(1), last(2), mean(3), rms(4), midrange(5)
%    * input scales: single(0), linear(1), geometric(2), log(3), sparse(4)
%    * state scales: single(0), linear(1), geometric(2), log(3), sparse(4)
%    * input rotations: unit(0), single(1)
%    * state rotations: unit(0), single(1)
%    * normalization (only: Wc, Wo, Wx, Wy): none(0), steady(1), Jacobi(2)
%    * state gramian variant:
%      * controllability gramian type (only: Wc, Ws): regular(0), output(1)
%      * observability gramian type (only: Wo, Wi): regular(0), averaged(1)
%      * cross gramian type (only: Wx, Wy, Wj): regular(0), non-symmetric(1)
%    * extra input (only: Wo, Wx, Ws, Wi, Wj): no(0), yes(1)
%    * parameter centering (only: Ws, Wi, Wj): none(0), lin(1), log(2), nom(3)
%    * parameter gramian variant:
%      * averaging type (only: Ws): input-state(0), input-output(1)
%      * Schur-complement (only: Wi, Wj): approx(0), coarse(1), exact(2)
%    * cross gramian partition size (only: Wx, Wj): full(0), partitioned(<N)
%    * cross gramian partition index (only: Wx, Wj): partition(>0)
%    * weighting: none(0), linear(1), squared(2), state(3), scale(4), rsqrt(5)
%  ut {handle|'i'} input function: u_t = ut(t) or character:
%    * 'i' delta impulse input
%    * 's' step input / load vector / source term
%    * 'h' havercosine decaying exponential chirp input
%    * 'a' sinc (cardinal sine) input
%    * 'r' pseudo-random binary input
%  us {vector|0} steady-state input (1 or #inputs rows)
%  xs {vector|0} steady-state and nominal initial state x_0 (1 or #states rows)
%  um {matrix|1} input scales (1 or #inputs rows)
%  xm {matrix|1} initial-state scales (1 or #states rows)
%  dp {handle|@mtimes} inner product or kernel: xy = dp(x,y)
%
% RETURNS:
%
%  W {matrix} State-space system Gramian Matrix (for: Wc, Wo, Wx, Wy)
%  W  {cell}  {State, Parameter}-space system Gramian (for: Ws, Wi, Wj)
%
% CITE AS:
%
%  C. Himpe (2022). emgr - EMpirical GRamian Framework (Version 5.99)
%  [Software]. Available from https://gramian.de . doi:10.5281/zenodo.6457616
%
% KEYWORDS:
%
%  model reduction, system gramians, empirical gramians, cross gramian, MOR
%
% SEE ALSO: gram (Control System Toolbox)
%
% COPYRIGHT: Christian Himpe
%
% For more information, see: <https://gramian.de>

  % Set Integrator Handle (i.e. for custom solvers)
  global ODE;
  if not(isa(ODE,'function_handle')), ODE = @ssp2; end%if

  % Version Info (and export default local integratorvia ODE)
  if isequal(f,'version'), W = 5.99; return; else, fState = f; end%if

  % Default Arguments
  if (nargin <  6) || isempty(pr), pr = 0.0; end%if
  if (nargin <  7) || isempty(nf), nf = 0;   end%if
  if (nargin <  8) || isempty(ut), ut = 'i'; end%if
  if (nargin <  9) || isempty(us), us = 0.0; end%if
  if (nargin < 10) || isempty(xs), xs = 0.0; end%if
  if (nargin < 11) || isempty(um), um = 1.0; end%if
  if (nargin < 12) || isempty(xm), xm = 1.0; end%if
  if (nargin < 13) || isempty(dp), dp = @mtimes; end%if

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% SETUP

  % System Dimensions
  nInputs = s(1);   % Number of system inputs / controls
  nStates = s(2);   % Number of system states / degrees of freedom (DoF)
  nOutputs = s(3);  % Number of system outputs / quantities of interest (QoI)

  % Parameter Dimensions
  nParams = size(pr,1);        % Number of parameters / parameter dimension
  nParamSamples = size(pr,2);  % Number of parameter samples

  % Time Discretization
  tStep = t(1);                        % Time-step width
  tFinal = t(2);                       % Time horizon
  nSteps = floor(tFinal / tStep) + 1;  % Number of time-steps

  % Gramian Type
  gramianType = lower(w);

  % Flag Vector
  flags = [nf(:)',zeros(1,max(13,13-numel(nf)))];

  % Built-in Input Functions
  if not(isa(ut,'function_handle'))

    a0 = (pi / (2.0 * tStep)) * tFinal / log(4.0 * (tStep / tFinal));
    b0 = (4.0 * (tStep / tFinal)) ^ (1.0 / tFinal);

    switch lower(ut)
      case 'i',  fExcite = @(t) (t <= tStep) / tStep;				% Impulse Input
      case 's',  fExcite = @(t) 1.0;						% Step Input
      case 'h',  fExcite = @(t) 0.5 * cos(a0 * (b0 ^ t - 1.0)) + 0.5;		% Havercosine Chirp Input
      case 'a',  fExcite = @(t) sin(t / tStep) / ((t / tStep) + (t == 0));	% Sinc Input
      case 'r',  fExcite = @(t) randi([0,1],1,1);				% Pseudo-Random Binary Input
      otherwise, error(' emgr: Unknown input ut!');
    end%switch
  else
    fExcite = ut;
  end%if

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CONFIGURATION

  % Output Function
  if (isnumeric(g) && (1 == g)) || ...
     (strcmp(gramianType,'c') && not(flags(7))) || strcmp(gramianType,'y')
    fOutput = @id;
    fAdjoint = g;
  else
    fOutput = g;
  end%if

  % Trajectory Weighting
  tInstances = [0.5*tStep,tStep:tStep:tFinal];
  switch flags(13)
    case 1,    fWeight = @(traj) traj .* sqrt(tInstances);			% Linear time-weighting
    case 2,    fWeight = @(traj) traj .* (tInstances ./ sqrt(2.0));		% Quadratic time-weighting
    case 3,    fWeight = @(traj) traj ./ max(sqrt(eps),vecnorm(traj,2,1));	% State-weighting
    case 4,    fWeight = @(traj) traj ./ max(sqrt(eps),vecnorm(traj,Inf,2));	% Scale-weighting
    case 5,    fWeight = @(traj) traj ./ (pi*tInstances).^0.25;		% Reciprocal square-root time-weighting
    otherwise, fWeight = @(traj) traj;
  end%switch

  % Trajectory Centering
  switch flags(1)
    case 1,    fCenter = @(traj,xs) traj - xs;					% Steady state / output
    case 2,    fCenter = @(traj,xs) traj - traj(:,end);			% Final state / output
    case 3,    fCenter = @(traj,xs) traj - mean(traj,2);			% Temporal mean of state / output
    case 4,    fCenter = @(traj,xs) traj - sqrt(mean(traj .* traj,2));	% Temporal root-mean-square of state / output
    case 5,    fCenter = @(traj,xs) traj - 0.5*(max(traj,[],2)+min(traj,[],2));% Temporal mid-range of state / output
    otherwise, fCenter = @(traj,xs) traj;
  end%switch

  % Steady State
  vSteadyInput = repmat(us,iif(isscalar(us),nInputs,1),1);
  vSteadyState = repmat(xs,iif(isscalar(xs),nStates,1),1);

  % Gramian Normalization
  if ismember(flags(6),[1,2]) && ismember(gramianType,{'c','o','x','y'})

    if 2 == flags(6)  % Jacobi-type preconditioner
        NF = nf;
        NF(6) = 0;
        if isequal(w,'c'), NF(7) = 0; end%if
        PR = mean(pr,2);
        DP = @(x,y) sum(x(1:nStates,:) .* y(:,1:nStates)',2);  % Diagonal-only pseudo-kernel
        TX = sqrt(abs(emgr(f,g,s,t,w,PR,NF,ut,us,xs,um,xm,DP)));
    else              % Steady-state preconditioner
        TX = vSteadyState;
    end%if

    TX(abs(TX)<sqrt(eps)) = 1.0;

    vSteadyState = vSteadyState ./ TX;
    fState = @(x,u,p,t) f(TX .* x,u,p,t) ./ TX;
    fAdjoint = @(x,u,p,t) g(TX .* x,u,p,t) ./ TX;
    fOutput = @(x,u,p,t) g(TX .* x,u,p,t);
  end%if

  % Output Averaging
  nPages = iif(flags(7),1,nOutputs);

  % Extra Input (for control explicit observability)
  fSteady = iif(flags(8),@(t) vSteadyInput + fExcite(t),@(t) vSteadyInput);

  % Perturbation Scales
  vInputMax = repmat(um,iif(isscalar(um),nInputs,1),1);
  vStateMax = repmat(xm,iif(isscalar(xm),nStates,1),1);
  vOutputMax = repmat(xm,iif(isscalar(xm),nOutputs,1),1);

  mInputScales = vInputMax * iif(1 == size(um,2),scales(flags(2),flags(4)),1);
  mStateScales = vStateMax * iif(1 == size(xm,2),scales(flags(3),flags(5)),1);
  mOutputScales = vOutputMax * iif(1 == size(xm,2),scales(flags(2),flags(4)),1);

  nInputScales = size(mInputScales,2);
  nStateScales = size(mStateScales,2);
  nOutputScales = size(mOutputScales,2);

  nTotalStates = size(mStateScales,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% GRAMIAN COMPUTATION

  W = 0.0;

  switch gramianType

    % Common Layout:
    %   For each {parameter, scale, input/state/parameter component}:
    %     Perturb, simulate, (weight, center), normalize, accumulate
    %   Output and adjoint trajectories are cached to prevent recomputation
    %   Parameter gramians 's', 'i', 'j' call state gramians 'c', 'o', 'x'

%% Empirical Controllability Gramian

    case 'c'

      for k = 1:nParamSamples
        vParam = pr(:,k);
        vSteadyOutput = fOutput(vSteadyState,vSteadyInput,vParam,0);
        for c = 1:nInputScales
          for m = 1:nInputs  % (parallelizable with `parfor`)
            sPerturb = mInputScales(m,c);
            if not(0 == sPerturb)
              vUnit = sparse(m,1,sPerturb,nInputs,1);
              fInput = @(t) vSteadyInput + vUnit * fExcite(t);
              mTraj = ODE(fState,fOutput,t,vSteadyState,fInput,vParam);
              mTraj = fWeight(fCenter(mTraj,vSteadyOutput)) ./ sPerturb;
              W = W + dp(mTraj,mTraj');
            end%if
          end%for
        end%for
      end%for
      W = W * (tStep / (nInputScales * nParamSamples));

%% Empirical Observability Gramian

    case 'o'

      obsCache = zeros(nPages*nSteps,nTotalStates);
      for k = 1:nParamSamples
        vParam = pr(:,k);
        for d = 1:nStateScales
          for n = 1:nTotalStates  % (parallelizable with `parfor`)
            sPerturb = mStateScales(n,d);
            if not(0 == sPerturb)
              vUnit = sparse(n,1,sPerturb,nTotalStates,1);
              vInit = vSteadyState + vUnit(1:nStates);
              vParamInit = vParam;
              if nTotalStates > nStates
                vParamInit = vParamInit + vUnit(nStates+1:end);
              end%if
              vSteadyOutput = fOutput(vSteadyState,vSteadyInput,vParamInit,0);
              mTraj = ODE(fState,fOutput,t,vInit,fSteady,vParamInit);
              mTraj = fWeight(fCenter(mTraj,vSteadyOutput)) ./ sPerturb;
              if flags(7)
                obsCache(:,n) = sum(mTraj,1)';
              else
                obsCache(:,n) = reshape(mTraj',[],1);
              end%if
            end%if
          end%for
          W = W + dp(obsCache',obsCache);
        end%for
      end%for
      W = W * (tStep / (nStateScales * nParamSamples));

%% Empirical Cross Gramian

    case 'x'

      assert((nInputs == nOutputs) || flags(7),' emgr: non-square system!');

      colFirst = 1;            % Start partition column index
      colLast = nTotalStates;  % Final partition column index

      % Partitioned Cross Gramian
      if flags(11) > 0
        parSize = round(flags(11));   % Partition size
        parIndex = round(flags(12));  % Partition index
        colFirst = colFirst + (parIndex - 1) * parSize;
        colLast = min(colFirst + (parSize - 1),nStates);
        if colFirst > nStates
          colFirst = colFirst - (ceil(nStates / parSize) * parSize - nStates);
          colLast = min(colFirst + parSize - 1,nTotalStates);
        end%if

        if (parIndex < 1) || (colFirst > colLast) || (colFirst < 0)
          return;
        end%if
      end%if

      obsCache = zeros(nSteps*nPages,colLast-colFirst+1);
      for k = 1:nParamSamples
        vParam = pr(:,k);
        for d = 1:nStateScales
          for n = 1:(colLast-colFirst+1)  % (parallelizable with `parfor`)
            sPerturb = mStateScales(colFirst+n-1,d);
            if not(0 == sPerturb)
              vUnit = sparse(colFirst+n-1,1,sPerturb,nTotalStates,1);
              vInit = vSteadyState + vUnit(1:nStates);
              vParamInit = vParam;
              if nTotalStates > nStates
                vParamInit = vParamInit + vUnit(nStates+1:end);
              end%if
              vSteadyOutput = fOutput(vSteadyState,vSteadyInput,vParamInit,0);
              mTraj = ODE(fState,fOutput,t,vInit,fSteady,vParamInit);
              mTraj = fWeight(fCenter(mTraj,vSteadyOutput)) ./ sPerturb;
              if flags(7)
                obsCache(:,n) = sum(mTraj,1)';
              else
                obsCache(:,n) = reshape(mTraj',[],1);
              end%if
            end%if
          end%for
          for c = 1:nInputScales  % (parallelizable with `parfor`)
            for m = 1:nInputs
              sPerturb = mInputScales(m,c);
              if not(0 == sPerturb)
                vUnit = sparse(m,1,sPerturb,nInputs,1);
                fInput = @(t) vSteadyInput + vUnit * fExcite(t);
                mTraj = ODE(fState,@id,t,vSteadyState,fInput,vParam);
                mTraj = fWeight(fCenter(mTraj,vSteadyInput)) ./ sPerturb;
                nBlock = iif(flags(7),0,(m - 1) * nSteps);
                W = W + dp(mTraj,obsCache(nBlock+1:nBlock+nSteps,:));
              end%if
            end%for
          end%for
        end%for
      end%for
      W = W * (tStep / (nInputScales * nStateScales * nParamSamples));

%% Empirical Linear Cross Gramian

    case 'y'

      assert((nInputs == nOutputs) || flags(7),' emgr: non-square system!');
      assert(nInputScales == nOutputScales,' emgr: scale count mismatch!');

      adjCache = zeros(nSteps,nStates,nPages);
      for k = 1:nParamSamples
        vParam = pr(:,k);
        for c = 1:nInputScales
          for q = 1:nOutputs  % (parallelizable with `parfor`)
            sPerturb = mOutputScales(q,c);
            if not(0 == sPerturb)
              vUnit = sparse(q,1,sPerturb,nOutputs,1);
              fInput = @(t) vSteadyInput + vUnit * fExcite(t);
              mTraj = ODE(fAdjoint,@id,t,vSteadyState,fInput,vParam);
              mTraj = fWeight(fCenter(mTraj,vSteadyInput)) ./ sPerturb;
              adjCache(:,:,q) = mTraj';
            end%if
          end%for
          if flags(7)
            adjCache(:,:,1) = sum(adjCache,3);
          end%if
          for m = 1:nInputs  % (parallelizable with `parfor`)
            sPerturb = mInputScales(m,c);
            if not(0 == sPerturb)
              vUnit = sparse(m,1,sPerturb,nInputs,1);
              fInput = @(t) vSteadyInput + vUnit * fExcite(t);
              mTraj = ODE(fState,@id,t,vSteadyState,fInput,vParam);
              mTraj = fWeight(fCenter(mTraj,vSteadyInput)) ./ sPerturb;
              W = W + dp(mTraj,adjCache(:,:,iif(flags(7),1,m)));
            end%if
          end%for
        end%for
      end%for
      W = W * (tStep / (nInputScales * nParamSamples));

%% Empirical Sensitivity Gramian

    case 's'

      % Controllability Gramian
      [pr,mParamScales] = paramScales(pr,flags(9),nInputScales);
      WC = emgr(f,g,s,t,'c',pr,flags,ut,us,xs,um,xm,dp);

      if not(flags(10))  % Input-state sensitivity gramian
        DP = @(x,y) sum(sum(x .* y'));       % Trace pseudo-kernel
      else               % Input-output sensitivity gramian
        DP = @(x,y) y;                       % Custom pseudo-kernel
        flags(7) = 1;
        Y = emgr(f,g,s,t,'o',pr,flags,ut,us,xs,um,xm,DP);
        flags(7) = 0;
        DP = @(x,y) abs(sum(y(:) .* Y(:)));  % Custom pseudo-kernel
      end%if

      % (Diagonal) Sensitivity Gramian
      WS = zeros(nParams,1);
      for p = 1:nParams  % (parallelizable with `parfor`)
        paramSamples = repmat(pr,[1,size(mParamScales,2)]);
        paramSamples(p,:) = paramSamples(p,:) + mParamScales(p,:);
        WS(p) = emgr(f,g,s,t,'c',paramSamples,flags,ut,us,xs,um,xm,DP);
      end%for

      W = {WC,WS};

%% Empirical Identifiability Gramian

    case 'i'

      % Augmented Observability Gramian
      [pr,mParamScales] = paramScales(pr,flags(9),nStateScales);
      V = emgr(f,g,s,t,'o',pr,flags,ut,us,xs,um,[mStateScales;mParamScales],dp);

      % Return Augmented Observability Gramian
      if flags(11), W = V; return; end%if

      WO = V(1:nStates, 1:nStates);          % Observability gramian
      WM = V(1:nStates, nStates+1:end);      % Mixed block
      WI = V(nStates+1:end, nStates+1:end);  % Parameter gramian

      % Identifiability Gramian via Schur Complement
      switch flags(10)
        case 0,    WI = WI - (WM' * ainv(WO) * WM);
        case 2,    WI = WI - (WM' * pinv(WO) * WM);
      end%switch

      W = {WO,WI};

%% Empirical Joint Gramian

    case 'j'

      % Joint Gramian
      [pr,mParamScales] = paramScales(pr,flags(9),nStateScales);
      V = emgr(f,g,s,t,'x',pr,flags,ut,us,xs,um,[mStateScales;mParamScales],dp);

      % Return Joint Gramian (Partition)
      if flags(11), W = V; return; end%if

      WX = V(1:nStates, 1:nStates);      % Cross gramian
      WM = V(1:nStates, nStates+1:end);  % Mixed block

      % Cross-Identifiability Gramian via Schur Complement
      switch flags(10)
        case 1,    WI = 0.5 * (WM' * WM);
        case 2,    WI = 0.5 * (WM' * pinv(WX + WX') * WM);
        otherwise, WI = 0.5 * (WM' * ainv(WX + WX') * WM);
      end%switch

      W = {WX,WI};

    otherwise

      error(' emgr: unknown empirical gramian type!');
  end%switch
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: iif

function r = iif(pre,con,alt)
% summary: inline if

  if pre, r = con; else, r = alt; end%if
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: id

function x = id(x,u,p,t)
% summary: (Output) identity functional

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: stateScales

function mScales = scales(flScales,flRot)
% summary: Input and initial state perturbation scales

  switch flScales
    case 1,    mScales = [0.25, 0.50, 0.75, 1.0];  % Linear
    case 2,    mScales = [0.125, 0.25, 0.5, 1.0];  % Geometric
    case 3,    mScales = [0.001, 0.01, 0.1, 1.0];  % Logarithmic
    case 4,    mScales = [0.01, 0.50, 0.99, 1.0];  % Sparse
    otherwise, mScales = 1.0;                      % One
  end%switch

  if 0 == flRot, mScales = [-mScales,mScales]; end%if
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: paramScales

function [vParamSteady,mParamScales] = paramScales(p,flScales,nParamScales)
% summary: Parameter perturbation scales

  [vParamMin,vParamMax] = bounds(p,2);

  switch flScales
    case 1     % Linear centering and scaling
      assert(size(p,2) >= 2,' emgr: min and max parameter required!');
      vParamSteady = 0.5 * (vParamMax + vParamMin);
      vScales = linspace(0.0,1.0,nParamScales);
    case 2     % Logarithmic centering and scaling
      assert(size(p,2) >= 2,' emgr: min and max parameter required!');
      vParamSteady = sqrt(vParamMax .* vParamMin);
      vParamMin = log(vParamMin);
      vParamMax = log(vParamMax);
      vScales = linspace(0.0,1.0,nParamScales);
    case 3     % Nominal centering and scaling
      assert(size(p,2) == 3,' emgr: min, nom, max parameter required!');
      vParamSteady = p(:,2);
      vParamMin = p(:,1);
      vParamMax = p(:,3);
      vScales = linspace(0.0,1.0,nParamScales);
    otherwise  % No centering and linear scaling
      assert(size(p,2) >= 2,' emgr: min and max parameter required!');
      vParamSteady = vParamMin;
      vParamMin = ones(size(p,1),1)./nParamScales;
      vScales = linspace(1.0/nParamScales,1.0,nParamScales);
  end%switch

  mParamScales = (vParamMax - vParamMin) * vScales + vParamMin;
  if 2 == flScales, mParamScales = exp(mParamScales); end%if
  mParamScales = mParamScales - vParamSteady;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: ainv

function x = ainv(m)
% summary: Quadratic complexity approximate inverse matrix

  % Based on truncated Neumann series: X = D^-1 - (D^-1 (M - D) D^-1)
  D = diag(m);
  k = find(abs(D) > sqrt(eps));
  D(k) = 1.0 ./ D(k);
  x = (m .* (-D)) .* D';
  x(1:numel(D) + 1:end) = D;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LOCAL FUNCTION: ssp2

function y = ssp2(f,g,t,x0,u,p)
% summary: Low-Storage Strong-Stability-Preserving Second-Order Runge-Kutta

  global STAGES;  % Configurable number of stages for enhanced stability
  if not(isscalar(STAGES)), nStages = 3; else, nStages = STAGES; end%if

  tStep = t(1);
  nSteps = floor(t(2) / tStep) + 1;
  y = g(x0,u(0),p,0);
  y(:,nSteps) = 0.0;  % Pre-allocate trajectory

  xk1 = x0;
  for k = 2:nSteps
    xk2 = xk1;
    tCurr = (k - 1.5) * tStep;
    uCurr = u(tCurr);
    for s = 2:nStages
      xk1 = xk1 + (tStep / (nStages - 1)) * f(xk1,uCurr,p,tCurr);
    end%for
    xk1 = (xk1 * (nStages - 1) + xk2 + tStep * f(xk1,uCurr,p,tCurr)) / nStages;
    y(:,k) = g(xk1,uCurr,p,tCurr);
  end%for
end