function StaticContact
% Function to test optimal stepsize selection for implicit Euler integrator
% to cause a free-falling ball to settle onto the ground as quickly as
% possible.
% Assume that contact between the ball and the ground is modeled as a
% nonlinear spring F = k*y^1.5, which is like a Hertzian contact model.
% Make y = 0 be ground level, and make the state vector x = [y yp]'.
% The static solution is specified by
% k*abs(y)^1.5 = m*g, or y = (m*g/k)^(1/1.5)
global k m g

% Define system parameter values
k = 10000;
m = 10;
g = 9.8;
tol = 1e-4;
t = 0;

% Calculate analytical static solution for comparison
ystatic = -(m*g/k)^(1/1.5);

% Define initial conditions
yold = 1000;
ypold = 0;

% Define initial state vector
xold = zeros(2,1);
xold(1,1) = yold;
xold(2,1) = ypold;

% Define initial maximum acceleration
dxold = freefall(t,xold);
MaxAccel = abs(dxold(2,1));
fprintf('Initial acceleration = %e\n\n', MaxAccel)

% Perform implicit Euler integration with variable step size chosen to
% produce minimum acceleration at each time step
% Step size adjustment within each iteration uses the following logic:
% If NewAccel > MaxAccel
%    reject the step and cut the step size in half
% If NewAccel = MaxAccel
%    accept the step and double the step size
% If NewAccel < MaxAccel
%    accept the step and keep the same step size

% First try integrating this function to see if the ball will bounce
% and to demonstrate the difference between a non-stiff single-step
% explicit integrator and a stiff multi-step implicit integrator
tspan = [0:0.1:200];
x0 = xold;
options = odeset('AbsTol',1e-7,'RelTol',5e-4);
[tout,x]=ode45(@freefall,tspan,x0,options);
plot(tout,x(:,1),'k-')
hold on
[tout,x]=ode15s(@freefall,tspan,x0,options);
plot(tout,x(:,1),'r-')
legend('ode45','ode15s')
% Note that the implicit ode15s integrator loses energy while the explicit
% ode45 integrator does not
pause

hold off
h = 1;
count = 1;
iter = 1;
Positions = [xold(1,1)];
Accels = [MaxAccel];

while MaxAccel > tol
    % First take an explicit Euler step to the end of the interval
    % to create an initial guess for xnew
    dxold = freefall(t,xold);
    xnew = xold + dxold*h;
    
    % Next take an implicit Euler step using the guess for xnew from the
    % explicit Euler step. For now, don't worry about iterating the
    % solution until convergence. It turns out that fixed point iteration
    % often diverges here, so a Newton-Raphson rootfinding approach would
    % be needed.
    dxnew = freefall(t,xnew);
    xnew = xold + dxnew*h;
    
    % Finally update the acceleration estimate at the end of the step
    dxnew = freefall(t,xnew);
    
    % Adjust the step size h based on the new maximum acceleration
    NewAccel = abs(dxnew(2,1));
    
    if NewAccel > MaxAccel
        h = 0.5*h;
        fprintf('Halving the step size\n')
    elseif NewAccel == MaxAccel
        xold = xnew;
        h = 2*h;
        MaxAccel = NewAccel;
        fprintf('Maximum acceleration = %e\n', MaxAccel)
        fprintf('Doubling the step size\n')
        Positions = [Positions; xold(1,1)];
        Accels = [Accels; MaxAccel];
    else % NewAccel < MaxAccel
        xold = xnew;
        MaxAccel = NewAccel;
        fprintf('Maximum acceleration = %e\n', MaxAccel)
        fprintf('Keeping the step size\n')
        Positions = [Positions; xold(1,1)];
        Accels = [Accels; MaxAccel];
    end
    
    % Reset initial velocity to zero
    xold(2,1) = 0;
    
%     fprintf('Maximum acceleration = %e\n', MaxAccel)
%     fprintf('Position = %e\n', xnew(1,1))
%     fprintf('Step size = %e\n', h)
%     pause
    
    iter = iter+1;
end

fprintf('\nAnalytic static solution = %e\n', ystatic)
fprintf('Numerical static solution = %e\n', xnew(1,1))
fprintf('Number of steps = %d\n', size(Accels,1))
fprintf('Number of function evals = %d\n', 2*(iter-1))

plot(Positions,'bo-')
xlabel('Step')
ylabel('Position')
pause
plot(Accels,'bo-')
xlabel('Step')
ylabel('Acceleration')

%------------------------------------------------------------------------
function dx = freefall(t,x)

global k m g

y = x(1,1);
yp = x(2,1);

if y < 0
    F = k*(abs(y))^1.5-m*g;
else
    F = -m*g;
end

ypp = F/m;

dx(1,1) = yp;
dx(2,1) = ypp;