% Semi-implicit Euler method
k = 200
m = 0.25
h = 0.01
tmax = 25;
i=1;
x(1) = 0.1
p(1) = 0
H(1) = k*x(1)^2/2 + p(1)^2/(2*m)
Hp(1) = k*x(1)^2/2 + p(1)^2/(2*m) + h*k/(2*m)*x(1)*p(1)
t(1)=0;

while t(i)<tmax
  i = i+1;
  t(i) = t(i-1) + h;
  x(i) = x(i-1) + h*p(i-1)/m;
  p(i) = p(i-1) - h*k*x(i);
  H(i) = k*x(i)^2/2 + p(i)^2/(2*m);
  Hp(i) = k*x(i)^2/2 + p(i)^2/(2*m) + h*k/(2*m)*x(i)*p(i);
end
