% What does the err vector from twostep.m look like?
% Here I need a more hands-on approach, can't use my generic ode_fixed.

function twostep_test2

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

ODE = @Harmonic; % Define which ODE we are solving.
stepper = @rk2;  % Define which basic, single-step ODE solver we are using.
order = 2;       % Order of the solver.
x0 = [1,0];      % Initial state. State is expected to be a row vector.
t0 = 0;          % Initial time.
tfinal = 2*pi;   % Final time.
Nsteps = 10;     % Number of steps (before doubling).
h = (tfinal - t0)/Nsteps;  % Size of time step.
t = (t0 + [0:Nsteps]*h)';  % column vector of times
Ndim = numel(x0);          % How big is the phase space?
x = zeros(Nsteps, Ndim); 
   % This vector will hold the answers. Each row is a state at a given timestep.
err = zeros(Nsteps, Ndim);
   % This will hold error estimates. Each row is error at a given timestep.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  SOLVE THE ODE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x(1,:) = x0; % 
for n = 2:Nsteps+1 % Step through and get the answers.
   [x(n,:), err(n,:)] = twostep(ODE, x(n-1,:), t(n-1), h, stepper, order);
      % This is counterintuitive --- 
      % pass function handle as ode rather than %ode.
end

% Calculate cumulative errors
cumErr = cumsum(abs(err)); % This does a cumulative sum down the columns


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  PLOT  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(1) % Phase space plot
hold off
tfine = (0:1000)*tfinal/1000; % high resolution times for analytic solution
xfine = cos(tfine); % Analytic soln for x
yfine = sin(tfine); % Analytic soln for y
plot(x(:,1), x(:,2), 'b-+', xfine, yfine, 'g-')
title('Simple harmonic oscillator: Error analysis', 'fontsize', 18)
xlabel('x', 'fontsize', 18)
ylabel('y', 'fontsize', 18)
axis([-1.1,1.4,-1.1,1.1],'equal')
hold on

% Generate a box that can be scaled for cumulative error rectangles.
xe = [1,-1,-1,1,1];
ye = [1,1,-1,-1,1];

% Plot boxes scaled to cumulative error, centered over each phase space point.
for n=1:Nsteps+1
   plot(x(n,1) + xe*cumErr(n,1), x(n,2) + ye*cumErr(n,2), 'r-')
end
legend('doubled rk2, 10 steps', 'analytic solution', 'cumulative error estimates')

hold off
