%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Orthogonal Decomposition Algorithm for equality-constrained optimization
% problems with arbitrary objective function.
% 
% To use this algorithm, the following files have to be modified with
% respect to the problem at hand:
%           
%       -> ODA_g.m  : the constraints vector g (px1)
%       -> ODA_J.m  : the Jacobian of vector g (pxn)
%       -> ODA_f.m  : the objective function f (1x1) -> scalar
%       -> ODA_gf.m : the gradient vector of f (nx1)
%       -> ODA_Hf.m : the hessian matrix of f  (nxn)
% 
% which are all function of the n-dimensional design vector x.
% Notice that p is equivalent to the number of constraints.
% 
% The corresponding files are also required in the same folder
% 
%       -> ODA_L_dx0.m         : Orthogonal complement of J (L)   (nx(n-p))
%                              : minimum norm solution (dx0)      (nx1)
%       -> gerschgorin_shift.m
%       -> HH.m
%       -> HHMat.m
%       -> HHVect.m
%       -> BackSub.m
%       -> ForwardSub.m
% 
% The tolerances required to achieve the optimimum values need to be  
% defined by the user. The tolerances usually used are
% 
%       -> e1 = 1e-5
%       -> e2 = 1e-5
% 
% by Jean-François Gauthier : 2008/02/07
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function[x_opt,f_opt] = ODAH(x_init,e1,e2)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% where -> x_init : is the initial guess x^{0} = [x1,x2,...,xn]
%       -> e1     : the tolerance desired for the norm of delta_x
%       -> e2     : the tolerance desired for the norm of g
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial guess [x1,x2,...,xn] for the first iteration, x^{0}           
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x = x_init;
n = length(x);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialisation of the norms
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

norm_dx = 1;
norm_g  = 1;
k       = 0;

% damping is used to reduce the gerschgorin shift
damping = 0.025;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ODA Arbitrary routine
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

while((norm_dx >= e1) || (norm_g >= e2))
    
    k = k+1
         
    g       = ODA_g(x);
    J       = ODA_J(x);
    [L,dx0] = ODA_L_dx0(J,-g);
    norm_g  = norm(g);
    
    gf = ODA_gf(x);
    F  = ODA_Hf(x);
    
    mu = gerschgorin_shift(F)*damping;
    W  = F + mu*eye(n);
    
    H       = L'*W*L;
    [LL,U]  = lu(H);
    
    b  = L'*W*dx0+L'*gf;
    
    y  = ForwardSub(LL,b);
    du = BackSub(-U,y);
       
    dx = dx0 + L*du;
    
    x  = x + dx;
    f  = ODA_f(x);
    
    norm_dx = norm(dx);
    
end

x_opt = x;
f_opt = f;


