clear
close all

sysName = 'Hager';
S = eval(['sys',sysName,'([])']);

T = 6;
h = 0.4;
N = int32(T/h);
s = 2;

q0 = S.q(0,S);
v0 = S.v(0,S);
qT = S.q(T,S);
vT = S.v(T,S);

t  = 0:h/s:T;
qig = reshape([S.q(t,S)+0.1*rand(1,s*N+1),zeros(1,s-1)],1,s,N+1);
uig = reshape([S.u(t,S)+0.1*rand(1,s*N+1),zeros(1,s-1)],1,s,N+1);

options = optimset('Algorithm', 'sqp');
options.s = s;
options.data = 'velocity';

[q, p, u] = varInt(S,q0,v0,qT,vT,T,N,qig,uig,options);
q = squeeze(q(:,1,:))';
p = squeeze(p)';
u = squeeze(u(:,1,:))';

t  = 0:h:T;
qe = S.q(t,S);
pe = S.p(t,S);
ue = S.u(t,S);

max(abs(q-qe))
max(abs(u-ue))