% Hamiltonian-preserving symplectic method
global k m
k = 200
m = 0.25
h = 0.01
tmax = 10;
i=1;
x(1) = 0.1
p(1) = 0
H(1) = k*x(1)^2/2 + p(1)^2/(2*m)
t(1)=0;

% Maximum absolute error
tol=1e-12;

while t(i)<tmax
  i = i+1;
  t(i) = t(i-1) + h;
  % Estimate (x,p) using the semi-implicit Euler method
  x(i) = x(i-1) + h*p(i-1)/m;
  p(i) = p(i-1) - h*k*x(i);
  % Refine using the Hamiltonian-preserving method
  err = 1; 
  while err>tol
    xold=x(i);
    pold=p(i);
    x(i) = x(i-1) + h*(H_ho(x(i-1),p(i))-H_ho(x(i-1),p(i-1)))/(p(i)-p(i-1));
    p(i) = p(i-1) - h*(H_ho(x(i),p(i))-H_ho(x(i-1),p(i)))/(x(i)-x(i-1));
    err = abs(x(i)-xold) + abs(p(i)-pold);
  end
  H(i) = k*x(i)^2/2 + p(i)^2/(2*m);
end
