function [t,y] = heunsc(fode,tspan,y0,options)
%
%   HEUNSC  Solve large systems of stiff ordinary differential equations
%
%   [T,Y] = HEUNSC(ODEFUN,TSPAN,Y0) with TSPAN = [T0 TFINAL] integrates the
%   system of differential equations y' = f(t,y) from time T0 to TFINAL with
%   initial conditions Y0. Function ODEFUN(T,Y) must return a column vector
%   corresponding to f(t,y). Each row in the solution array Y corresponds to
%   a time returned in the column vector T.
%
%   [T,Y] = HEUNSC(ODEFUN,TSPAN,Y0,OPTIONS) solves as above with default
%   integration properties replaced by values in OPTIONS, an argument created
%   with the ODESET function. See ODESET for details. HEUNSC supports the
%   most important features of the matlab ODE solver. Moreover in this code
%   there are a few other options available:
%
%       'IncDecSc' - positive scalar - default: 0.05
%                  Relative factor for increasing/decreasing the scaling.
%                  Commonly used values are in the range (0,0.2). See [3]
%                  for details.
%       'PredCont' - 'on'/'off' - default: 'on'
%                  Enable the stepsize selection based on the 
%                  the predictive controller method. Setting 'off', the
%                  classical controller is used (see e.g. [1]).
%       'PredContValues' - 2 element vector - default: [0.3 0.6]
%                  Parameters for the stepsize selection based on the 
%                  the predictive controller method. The values are in the 
%                  range [0,1]. By default the code uses the controller PC.3.6.
%                  See [2] for details.
%
%   HEUNSC is an implementation of the "Scaled Heun method", that is
%   a modified version of the well known Heun method of order 2. The stepsize
%   control procedure is based on Richardson extrapolation. Deatails can be found in [3].
%
%   REMARKS:
%
%   1. the code is particularly efficient for solving stiff problems
%   arising from the semidiscretization of parabolic PDE, where the solution
%   becomes smooth after a brief transient. For problems in which the solution
%   is never smooth, such as the Van der Pol equation, the method is always much
%   better than any explicit Runge-Kutta method but is not competitive with the
%   standard implicit stiff solvers.
%
%   2. The performances of the code are highly sensitive to the choice of the parameters 
%   'IncDecSc', 'PredCont' and 'PredContValues'. Actually this is a drawback that 
%   we are trying to overcome in order to achive a much more robust code. 
%   This is a first but encouraging version.
%
%   BIBLIOGRAPHY:
%   
%   [1] Hairer, E., Norsett, S.P., Wanner, G., Solving ordinary differential equations. 
%       I: Nonstiff problems. Springer Series in Computational Mathematics, 8. Berlin: Springer 1987.
%   [2] Soderlind, G., Automatic control and adaptive time-stepping, 
%       Numerical Algorithms 31 (2002), pp.281-310.
%   [3] Novati, P., A class of explicit one-step methods of order 2 for stiff problems,
%       preprint available at http://univaq.it/~novati .
%
%
%   Paolo Novati, 13-Mar-2005.


true = 1; false = ~true;

% Initialization

if nargin < 4
    options = [];
    if nargin < 3
        error('not enough input arguments')
    end
end

rtol = odeget(options,'RelTol',1e-3);
atol = odeget(options,'AbsTol',1e-6);
nctr = odeget(options,'NormControl','off');
statson = odeget(options,'Stats','off');
instp = odeget(options,'InitialStep',[]);
hmax = odeget(options,'MaxStep',[]);

if statson
    stats = struct(...
        'nfevals',0,...
        'nsteps',0,...
        'nrejected',0);
end
if isfield(options,'IncDecSc')
    idm = options.IncDecSc;
else
    idm = 0.05;
end
if isfield(options,'PredCont')
    pd = options.PredCont;
else
    pd = 'on';
end
if isfield(options,'PredContValues')
    gf1 = options.PredContValues(1);
    gf2 = options.PredContValues(2);
else
    gf1 = 0.5; gf2 = 0.8;  
end

neq = length(y0);
scv = ones(neq,1); 
threshold = atol/rtol;

t0 = tspan(1); tf = tspan(2); 
T = t0; t(1) = t0; y(:,1) = y0; 
htspan = tf - t0;
hmin = 16 * eps * abs(t0);
if isempty(hmax)
    hmax = htspan/10;
end

if isempty(instp)
    % Compute an initial step using y'(t)
    h = min(hmax, htspan);
    f0 = feval(fode,t0,y0);
    stats.nfevals = stats.nfevals + 1;
    if strcmp(nctr,'on')
        rh = (norm(f0) / max(norm(y0),threshold)) / (0.9 * rtol^(1/3));
    else
        rh = norm(f0 ./ max(abs(y0),threshold),inf) / (0.9 * rtol^(1/3));
    end
    if h * rh > 1
        h = 1 / rh;
    end
    h = max(h, hmin);
else
    h = min(hmax, max(hmin, instp));
end

% The main loop

n = 1; lev_in = [];
done = false;
while ~done
    
    hmin = 16 * eps * abs(T);
    h = max(hmin, h); 
    
    % Stretch the step if within 10% of tfinal-t.
    if 1.1 * h >= abs(tf - T)
        h = tf - T;
        done = true;
    end

    % Loop for advancing one step  
    nofailed = false;
    while ~nofailed

        if done
            [c1,c2] = coeff(h,scv); 
            z1 = feval(fode,T,y(:,n));
            z2 = feval(fode,T + h,y(:,n) + h * z1);
            y(:,n + 1) = y(:,n) + h * (c1 .* z1 + c2 .* z2); t(n + 1) = tf;
            stats.nfevals = stats.nfevals + 2;
            break
        end
        
        nscv(:,1) = scv * (1 + idm); nscv(:,2) = scv * (1 - idm); nscv((find(nscv(:,2) < 1)),2) = 1;
        [y1(:,1),y2(:,1),lev_out] = RK(fode,T,y(:,n),h,nscv(:,1),[]);
        [y1(:,2),y2(:,2),lev_out] = RK(fode,T,y(:,n),h,nscv(:,2),lev_out);
        stats.nfevals = stats.nfevals + 7;
        
        % Weighted error estimate
        corr = sc_fact(h,scv) .^ (-1);
        nrm = (y1 - y2) .* [corr corr];
        if strcmp(nctr,'on')
            err(1) = (1/3) * norm(nrm(:,1)) / max(max(norm(y(:,n)),norm(y2(:,1))),threshold);
            err(2) = (1/3) * norm(nrm(:,2)) / max(max(norm(y(:,n)),norm(y2(:,2))),threshold);
        else
            err(1) = (1/3) * norm(nrm(:,1) ./ max(max(abs(y(:,n)),abs(y2(:,1))),threshold),inf); 
            err(2) = (1/3) * norm(nrm(:,2) ./ max(max(abs(y(:,n)),abs(y2(:,2))),threshold),inf);
        end
        [err,j] = min(err);
        if n == 1
            olderr = err;
            oldh = h;
        end
        
        if err <= rtol
            nofailed = true;
            T = T + h; t(n + 1) = T; y(:,n + 1) = y2(:,j);
            stats.nsteps = stats.nsteps + 1;
            if strcmp(pd,'on')
                fac = gust_fact(gf1/3,gf2/3,rtol,err,olderr,h,oldh);
            else
                fac = cla_fact(rtol,err);
            end
            h = min(h * min(2, max(0.5 , 0.8 * fac)), hmax); 
            scv = new_scv(nscv,abs(nrm));
            olderr = err; oldh = h;
            n = n + 1;
        else
            if strcmp(pd,'on')
                fac = gust_fact(gf1/3,gf2/3,rtol,err,olderr,h,oldh);
            else
                fac = cla_fact(rtol,err);
            end
            h = min(h * min(2, max(0.5 , 0.8 * fac)), hmax);
            stats.nrejected = stats.nrejected + 1;
        end
    end

end

y = y.';

if statson                   % print cost statistics
    fprintf('%g successful steps\n', stats.nsteps);
    fprintf('%g failed attempts\n', stats.nrejected);
    fprintf('%g function evaluations\n', stats.nfevals + 3);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Internal Functions %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Advancing with h and h/2 %%%%%%%%%%%%%%%%%%%%%

function [y1,y2,lev_out] = RK(fode,T,y,h,scv,lev);

if isempty(lev)
    lev(:,1) = feval(fode,T,y); 
    lev(:,2) = feval(fode,T + h,y + h * lev(:,1));
    lev(:,3) = feval(fode,T + h/2,y + h/2 * lev(:,1));
end

[c1,c2] = coeff(h,scv); 
y1 = y + h * (c1 .* lev(:,1) + c2 .* lev(:,2));
[c1,c2] = coeff(h/2,scv);
ym = y + h/2 * (c1 .* lev(:,1) + c2 .* lev(:,3));

lev(:,4) = feval(fode,T + h/2,ym); 
lev(:,5) = feval(fode,T + h,ym + h/2 * lev(:,4));
y2 = ym + h/2 * (c1 .* lev(:,4) + c2 .* lev(:,5));

lev_out = lev(:,1:3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Update scaling vector %%%%%%%%%%%%%%%%%%%%%

function scv = new_scv(nscv,nrm);

[nrm,pos] = min(nrm,[],2);
scv = diag(nscv(:,pos));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Method coefficients %%%%%%%%%%%%%%%%%%%%%%

function [c1,c2] = coeff(h,scv);

fac = sc_fact(h,scv);
c2 = fac.^2/2; c1 = fac - c2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gustafsson factor %%%%%%%%%%%%%%%%%%%%%%%

function fac = gust_fact(KI,KP,rtol,err,olderr,h,oldh);

fac = (rtol/err)^KI * (olderr/err)^KP * h/oldh; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  Classical factor %%%%%%%%%%%%%%%%%%%%%%%%

function fac = cla_fact(rtol,err);

fac = (rtol/err)^(1/3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  Scaling factor %%%%%%%%%%%%%%%%%%%%%%%%%%

function sf = sc_fact(h,scv);

sf = (1 + (h^2 * scv)) ./ (1 + (h * scv).^2);