% Hamiltonian-preserving symplectic method
% Version 2 using fsolve
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);
  % Now solve using the Hamiltonian-preserving equation
  fsopts = optimset('TolX',tol,'TolFun',tol);
  vnew = fsolve(@(v)Hpresfunc_ho(v,x(i-1),p(i-1),h),[x(i),p(i)],fsopts);
  x(i) = vnew(1);
  p(i) = vnew(2);
  H(i) = k*x(i)^2/2 + p(i)^2/(2*m);
end
