clear
close all

q0 =    0;
v0 = -0.5;
 S = struct('m',2,'k',.5,'r',.25,'q0',q0,'v0',v0);
 S = sysHarOsc(S);
 S = sysHarOsc([]); %default
%S = sysGeoMech(sysHarOsc([]));
q0 = S.q0;
v0 = S.v0;
p0 = S.p0;

s = 3;
N = 100;
T = 50;
h = T/N;

options = optimset('TolFun',  1e-12, ...
                   'TolX',    1e-12, ...
                   'MaxIter', 1000, ...
                   'Display', 'off');
options.s = s;
options.data = 'momentum';

[q, p] = varInt(S, q0, p0, [], [], T, N, options);
q = squeeze(q(:,1,:))';
E = S.E(q,S.invLeg(q,p,S),S);

t = 0:h:T;
qe = S.q(t,S);
ve = S.v(t,S);
pe = S.p(t,S);
Ee = S.E(qe,ve,S);

%  E = zeros(N+1,1);
% qe = zeros(size(q));
% pe = zeros(size(p));
% for i = 1:1:N+1
%        E(i) = S.E(q(:,i),S.invLeg(q(:,i),p(:,i),S),S);
% %     AM(i) = S.AM,q(:,i),p(:,i),S);
%     qe(:,i) = S.q((i-1)*h,S);
%     pe(:,i) = S.p((i-1)*h,S);
% end

[err_q,err_q_step] = max(abs(qe-q))
[err_p,err_p_step] = max(abs(pe-p))
[err_E,err_E_step] = max(abs(Ee-E))

% plot (t,q)
figure
plot(0:h:T,q)
xlabel('t')
ylabel('q')

% plot (t,p)
figure
plot(t,p)
xlabel('t')
ylabel('p')

% plot (t,E)
figure
plot(t,E)
xlabel('t')
ylabel('E')