%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Orthogonal Decomposition Algorithm Using Gradient Methods (ODAGM)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Rather than using the Hessian of the objective function as in the 
% original ODA routine to solve equality-constrained optimization problems 
% with arbitrary function, delta_u is found in ODAGM at each iteration
% using one of two gradient methods: the Fletcher_Reeves (FR) method or the
% Broyden-Fletcher-Golfarb-Shanno (BFGS) quasi-newton method. 
% 
% 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 output
%       -> ODA_gf.m : the gradient vector of f   (nx1)
% 
% which are all function of the n-dimensional design vector x.
% Notice that p is equivalent to the number of constraints.
% 
% The following 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)
%       -> FR.m        : to minimize du using FR          ((n-p)x1)
%       -> BFGS.m      : to minimize du using BFGS        ((n-p)x1)
%       -> fls.m       : Fletcher's Line Search algorithm (for FR and BFGS)
%       -> fls_f.m 
%       -> fls_gf.m
%       -> HH.m
%       -> HHMat.m
%       -> HHVect.m
% 
% The tolerances required to achieve the optimimum values need to be  
% defined by the user. The first two tolerances usually used are
% 
%       -> e1 = 1e-5
%       -> e2 = 1e-5
%       -> e3 = 1e-5
% 
% by Jean-François Gauthier : 2008/02/07
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function[x_opt,f_opt] = ODAGM(GM,x_init,e1,e2,e3)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% where -> GM     : Gradient Method used (1 for FR, 2 for BFGS)
%       -> x_init : is the initial guess x^{0} = [x1,x2,...,xn]
%       -> e1     : the tolerance desired for the norm of dx
%       -> e2     : the tolerance desired for the norm of g
%       -> e3     : the tolerance desired for the GM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

x = x_init
n = length(x)
g = ODA_g(x)
p = length(g)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Same initial guess for delta_u at every iteration (the zero vector)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

du = zeros(n-p,1);  % (n-p)x1 vector

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ODAGM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
while((norm_dx >= e1) || (norm_g >= e2))
    
    k = k+1
        
    J       = ODA_J(x);
    [L,dx0] = ODA_L_dx0(J,-g);
    
    save designV x;     % To be used in fls_f.m and fls_gf.m
        
    if (GM == 1)
        du  = FR(du,e3);
    elseif (GM == 2)
        du  = BFGS(du,e3);
    end
        
    dx      = dx0 + L*du;
        
    x       = x + dx;
    f       = ODA_f(x);
    g       = ODA_g(x);
        
    norm_dx = norm(dx);
    norm_g  = norm(g);
        
end

x_opt = x;
f_opt = f;


