function f = sysHarOscOCP(S)
% min int(u(t)^2) s.t.
% a(t) + k.q(t) = u(t)

if isempty(S)
    f.k  = 1;
    f.q0 = 1;
    f.v0 = 1;
    f.qT = 0;
    f.vT = 0;

    f = sysHarOscOCP(f);
    return
elseif ~prod(double(isfield(S,{'k','q0','v0','qT','vT'})))
    error('sysMech:params',['When input is given, all the system', ...
          ' parameters must be defined.'])
end

 m = 1; % WARNING: do not change!
 r = 0; % WARNING: do not change!
 k = S.k;
q0 = S.q0;
v0 = S.v0;
qT = S.qT;
vT = S.vT;

f = sysHarOsc(struct('m',m,'r',r,'k',k,'q0',q0,'v0',v0));
f.qT = qT;
f.vT = vT;
f.dimU = 1;
f.f = @(t,q,v,u,S)u;
f.cost = @(t,q,v,u,S)u.^2;

z = r^2 - 4*m*k;
if z > 0
%   disp('Overdamping');
    disp('WARNING: ignoring q0, v0, qT, pT');
    rk = sqrt(-k);
    T = 3/rk;
    f.q = @(t,S)(1+t).*cosh(rk*t);
    f.v = @(t,S)cosh(rk*t)+rk*(1+t).*sinh(rk*t);
    f.u = @(t,S)2*rk*sinh(rk*t);
elseif z == 0
%   disp('Critical damping');
    disp('WARNING: ignoring q0, v0, qT, pT');
    T = 10;
    f.q = @(t,S)1+t;
    f.v = @(t,S)1;
    f.u = @(t,S)0;
else
%   disp('Underdamping');
    rk = sqrt(k);
    T = 2*pi/rk;
    A = q0;
    B = (qT-q0)*rk/(2*pi);
    C = (v0-B)/rk;
    D = (vT-v0)/(2*pi);
    f.q = @(t,S)(A+B*t).*cos(rk*t)+(C+D*t).*sin(rk*t);
    f.v = @(t,S)B*cos(rk*t)+D*sin(rk*t)-...
                rk*(A+B*t).*sin(rk*t)+rk*(C+D*t).*cos(rk*t);
    f.u = @(t,S)2*rk*(D*cos(rk*t) - B*sin(rk*t));
end
f.p = @(t,S)f.Leg(f.q(t,S),f.v(t,S),S);

f.T = T;

f.q0 = f.q(0,f);
f.v0 = f.v(0,f);
f.p0 = f.p(0,f);

f.qT = f.q(T,f);
f.vT = f.v(T,f);
f.pT = f.p(T,f);
end