clear
close all

S = sysParabola([]);
T = 10;
N = 100;
h = T/N;

opts           = optimset('fsolve');
opts.s         = 1;
opts.BoundData = 'velocity';

[q,p] = varInt(S,S.q0,S.v0,[],[],T,N,[],[],opts);
    q = squeeze(q);
    p = squeeze(p);
    v = (q(2:end)-q(1:end-1))/h;

max(abs(q'-S.q(0:h:T,S)));


E.p = zeros(1,N); % momenta
E.l = zeros(1,N); % left point
E.r = zeros(1,N); % right point
E.m = zeros(1,N); % mid point
E.v = zeros(1,N); % verlet <-- corresponds to the integration method
for i = 1:N
  E.p(i) = S.E(q(i),p(i),S);
  E.l(i) = S.E(q(i),v(i),S);
  E.r(i) = S.E(q(i+1),v(i),S);
  E.m(i) = S.E((q(i)+q(i+1))/2,v(i),S);
  E.v(i) = (S.E(q(i),v(i),S)+S.E(q(i+1),v(i),S))/2;
end

figure
hold on
plot(h:h:T,E.p,'-b');
plot(h:h:T,E.l,'-g');
plot(h:h:T,E.r,'-r');
plot(h:h:T,E.m,'-y');
plot(h:h:T,E.v,'-k');
legend('momenta','left point','right point','mid point','verlet');
xlabel('time');
ylabel('energy');
hold off

figure
hold on
plot(h:h:T,E.p,'-b');
plot(h:h:T,E.m,'-y');
plot(h:h:T,E.v,'-k');
legend('momenta','mid point','verlet');
xlabel('time');
ylabel('energy');
hold off

max(abs(E.m-E.v)) % 'verlet' and 'mid point' coincide 'cause of simplicity
max(abs(E.v-E.v(1)-E.p))
