function testHager3()
  T = 6;
  h = 0.2;
  N = T/h;
  eps = 0.1;

  [x,u] = solveHager(T,h,eps);

  t  = (0:h:T)';
  err_x=max(abs(x(3:N-2)-x_sol(t(3:N-2))));
  err_u=max(abs(u(3:N-2)-u_sol(t(3:N-2))));
  disp('')
  fprintf('err_x = %f\n',err_x)
  fprintf('err_u = %f\n',err_u)
  disp('')

  figure;
  plot(t,x);
  figure;
  plot(t,u);
end

function [x,u] = solveHager(T, h, eps)
  N = round(T/h);
  
  % Cost function
  J = @(y)costFunction(y(1:2*N+1),y(2*N+2:4*N+2),h);
  % Dynamical constraints + boundary constraints
  G = @(y)[constraintBoundaryInitial(y(1:2*N+1),y(2*N+2:4*N+2),h,T);
           constraintDynamic(y(1:2*N+1),y(2*N+2:4*N+2),h);
           constraintBoundaryFinal(y(1:2*N+1),y(2*N+2:4*N+2),h,T)];

  t  = (0:h/2:T)';
  x0 = x_sol(t)+eps*rand(2*N+1,1);
  u0 = u_sol(t)+eps*rand(2*N+1,1);
  y0 = [x0;u0];

  % y = sqp(y0,J,G); % Octave
  y = fmincon(J, ...
            y0, [], [], [], [], [], [], ...
            @(y)emptyAdder(G(y)));
  x = y(1:2:2*N+1);
  u = y(2*N+2:2:4*N+2);
end

function [empty,out] = emptyAdder(in)
  out = in;
  empty = [];
end


% Problem generators

function J = costFunction(x,u,h)
  N = (size(x,1)-1)/2;
  J = 0;
  for k = 1:N
    J = J + cost2rdVerlet(x(2*k-1:2*k+1),u(2*k-1:2*k+1),h);
  end
end

function g = constraintDynamic(x,u,h)
  N = (size(x,1)-1)/2;
  g = zeros(2*N-1,1);
  g(1) = dynamic2rdVerletMicro(x(1:3),u(1:3),h);
  for k = 1:N-1
    g(2*k  )   = dynamic2rdVerletMacro(x(2*k-1:2*k+3),u(2*k-1:2*k+3),h);
    g(2*k+1)   = dynamic2rdVerletMicro(x(2*k+1:2*k+3),u(2*k+1:2*k+3),h);
  end
 % g(2*N-3) = dynamic2rdVerletMacro(x(2*N-3:2*N+1),u(2*N-3:2*N+1),h);
end

function g = constraintBoundaryInitial(x,u,h,T)
  g = [x(1)-x_sol(0); ...
%      -3*x(1)+4*x(2)-x(3) - h*y_sol(0); ...
       3/h*y_sol(0) + (-7*x(1)+8*x(2)-x(3))/h^2 - (1+u(1))/2];
end

function g = constraintBoundaryFinal(x,u,h,T)
  g = [(-x(end-2)+8*x(end-1)-7*x(end))/h^2 - (1+u(end))/2 + 3/h*y_sol(T); ...
%      x(end-2)-4*x(end-1)+3*x(end) - h*y_sol(T); ...
       x(end)-x_sol(T)];
end


% 2rd Verlet discretization

function f = cost2rdVerlet(x,u,h)
  f = 7*x(1)^2 + 16*x(2)^2 + 7*x(3)^2 ...
      - 16*x(1)*x(2) + 2*x(1)*x(3) - 16*x(2)*x(3);
  f = 2*f/h^2 + u(1)^2 + 4*u(2)^2 + u(3)^2;
  f = f*h/6;
end

function f = dynamic2rdVerletMacro(x,u,h)
  f = (-x(1)+8*x(2)-14*x(3)+8*x(4)-x(5))/h^2 - 1 - u(3);
end

function f = dynamic2rdVerletMicro(x,u,h)
  f = 4*(x(1)-2*x(2)+x(3))/h^2 - 1 - u(2);
end


% Exact solutions

function x = x_sol(t)
  x = (cosh(t)-1)/cosh(1);
end

function y = y_sol(t)
  y = sinh(t)/cosh(1);
end

function u = u_sol(t)
  u = (cosh(t)-cosh(1))/cosh(1);
end
