function [t,x] = gauss2(field, t0, T, x0, h, tol)
%GAUSS2 Implements the Gauss2 Implicit RK one-step ODE solver
%
% Parameters:
%  field  -- Right hand side function of ODE system: x'=f(t,x)
%  t0     -- Initial time
%  T      -- End time (T > t0)
%  x0     -- Initial value
%  h      -- Size of time step (h <= T-t0)
%  tol    -- Tolerance to use for solving the fixed point iteration (default=1e-10)
%
% Outputs:
%  t  -- [t0; t-0+h, t0+2*h; ...; t0+i*h; ...]
%  x  -- Matrix containing numerical solution, with each row the value of x
%        at each time step

arguments
    field; t0; T; x0; h;
    tol = 1e-10;
end

n = ceil((T-t0)/h);
t = t0+h*(0:n).';
x = ones(n+1,length(x0));
x(1,:) = x0;

a=sqrt(3);

for i=1:n
    k1_new = feval(field, t(i), x(i,:).');
    k2_new = feval(field, t(i), x(i,:).');
    k1_old = k1_new + 2*tol;
    k2_old = k2_new + 2*tol;
    while norm([k1_old-k1_new, k2_old-k2_new], 'inf') >= tol
        k1_old = k1_new;
        k2_old = k2_new;
        k1_new = feval(field, t(i)+(1/2-a/6)*h, ...
            x(i,:).'+h*(1/4*k1_old + (1/4-a/6)*k2_old));
        k2_new = feval(field, t(i)+(1/2+a/6)*h, ...
            x(i,:).'+h*((1/4+a/6)*k1_old + 1/4*k2_old));
    end
    x(i+1,:) = x(i,:).' + h*(k1_new+k2_new)/2;
end