function testHager2()
  T = 6;
  h = 0.3;
  N = T/h;
  eps = 0.1;
  method = 3; %Verlet (see below function solveHager)

  h = [0.6:-0.1:0.1,0.08];
  err_x = h;
  err_u = h;
  for i = 1:length(h)
    N = T/h(i);
    [x,u] = solveHager(T,h(i),eps,method);

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

  figure
  hold on
  plot(-log(h),log(err_x),'-b')
  plot(-log(h),log(err_u),'-g')
  legend('err_x','err_u')
  title('log_{err}')
  hold off
%   figure;
%   plot(t,x);
%   figure;
%   plot(t,u);
end

function [x,u] = solveHager(T, h, eps, method)
  N = round(T/h);

  %set method
  switch(method)
    case 1
      costType = @costMiddle;
      dynamicType = @dynamicMiddle;
      %note: interesting to modify 'cond' below to force 1st control
    case 2
      costType = @costMiddleAlt;
      dynamicType = @dynamicMiddleAlt;
      %warning: controls have and extra term than they should (lazy code)
      %         which causes an error at that term.
    case 3
      costType = @costVerlet;
      dynamicType = @dynamicVerlet;
  end

  % Cost function
  J = @(y)costFunction(y(1:N+1),y(N+2:2*N+2),h,costType);
  % Boundary constraints
  condInitial = @(y)[y(1)-x_sol(0);y(2)-x_sol(h)];
  condFinal = @(y)[y(N)-x_sol(T-h);y(N+1)-x_sol(T)];
  cond = @(y)[condInitial(y);condFinal(y)];%;y(N+2)-u_sol(0)];
  % Dynamical constraints + boundary constraints
  G = @(y)[dynamicConstraint(y(1:N+1),y(N+2:2*N+2),h,dynamicType);cond(y)];
  %bug: si llamo 'g' a 'G', Octave se raya con la 'g' de 'dynamicConstraint'

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

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

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


% Problem generators

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

function g = dynamicConstraint(x,u,h,f)
  N = size(x,1)-1;
  g = zeros(N-1,1);
  for k = 1:N-1
    g(k) = f(x(k),x(k+1),x(k+2),u(k),u(k+1),u(k+2),h);
  end
end


% Middle point discretization (no control correction)

function f = costMiddle(x0,x1,u0,u1,h)
  f = ((x1-x0)/h)^2 + ((u1+u0)/2)^2;
end

function f = dynamicMiddle(x0,x1,x2,u0,u1,u2,h)
  f = (x2-2*x1+x0)/h^2 - 1 - (u2+2*u1+u0)/4;
end


% Middle point discretization (with control correction)

function f = costMiddleAlt(x0,x1,v0,v1,h)
  f = ((x1-x0)/h)^2 + v0^2;
end

function f = dynamicMiddleAlt(x0,x1,x2,v0,v1,v2,h)
  f = (x2-2*x1+x0)/h^2 - 1 - (v1+v0)/2;
end


% Verlet discretization

function f = costVerlet(x0,x1,u0,u1,h)
  f = ((x1-x0)/h)^2 + (u1^2+u0^2)/2;
end

function f = dynamicVerlet(x0,x1,x2,u0,u1,u2,h)
  f = (x2-2*x1+x0)/h^2 - 1 - u1;
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
