function [x,f,g]=newt_froz(fgh_name,x,optpar,varargin)
%
%Newton minimization with frozen Hessian.  M.Zibulevsky
%
%[x,f,g]=newt_froz(fgh_name,x,optpar,varargin)
%
%Input:
%   fgh_name - name of the user function, which provides 
%              function, gradient and Hessian at given point x
%       [f,g,H]= funeval('fghname',x,varargin)
%       x       - vector of optimized variables
%       varargin   - structure with user problem data
%       f,g,H - function, gradient and Hessian at point x
%
%   x      - initial vector of optimized variables
%   optpar - parameters of the optimization method (can be empty [])
%   varargin  - arguments with user problem data, necessary for fgh evaluation
%
%Output:
%   x - optimal point 
%   f - function value at x
%   g - gradient at x



if isfield(optpar,'normGrdTol')
  normGrdTol = optpar.normGrdTol; 
else
  normGrdTol = 1e-9;
end

if isfield(optpar,'NnewtTol')
  NnewtTol   = optpar.NnewtTol; 
else 
  NnewtTol   = 500;
end

CholTol=0;

Ngrad_uncTol = 3;
%Ngrad_uncTol = 1;


Ngrad_uncTol1=Ngrad_uncTol;
igrad_unc=Ngrad_uncTol1; % for consistency with old version for PBM

igrad_total=1;

figure(100); % title('Norm of Gradient');hold on;
normg_array=[];

  for inewt=1:NnewtTol,
    n=length(x);

    if igrad_unc>=Ngrad_uncTol1
        fprintf('#');
      %[f,g,H]= fgh_aggregate(x,penpar,u,varargin{:});
      [f,g,H]= feval(fgh_name,x,varargin{:});

      if 0   %probl.optpar.SPARSE_SOLVER
      	fprintf('Sparse Cholesky factorization...')
  	ii=probl.indreord;
  	CholFact=cholinc(H(ii,ii),CholTol);
      else
      	%fprintf('Cholesky factorization...')
	    ii=[1:n];
      	[CholFact,p]=chol(full(H)); D=ones(n,1);
        if p>0,
            fprintf('Nonpos_Hess'); % eigvals=sort(eig(H))', H
            %disp('Hessian is not positive definite. Modified Cholessky is used')
            [CholFact,D]=mcholmz3(full(H)); CholFact=CholFact'; %Modified Colesky
        end
      end

      %disp(' done');

    end


    for igrad_unc=1:Ngrad_uncTol
      fprintf('.');

      %[f,g]= fgh_aggregate(x,penpar,u,varargin{:});
      [f,g]= feval(fgh_name,x,varargin{:});

      %fprintf('Cholessky substitution...')
      d=zeros(n,1);
      d(ii)= cholsub(CholFact,-g(ii),D);
      %disp(' done');
      
      %if d'*g>0, keyboard;end
      

      b1=0;b2=1;EpsGold=0.1; dftol=1e-8; dxtol=1e-8; nlinsrchmax=30;
      %[xnew,fnew,gradnew,tbest,resEpsGold,b1,b2] = cublinsrch1(fgh_name,...
      %  x,f,g,d,b1,b2,EpsGold,dftol,dxtol,nlinsrchmax,varargin{:});
      [xnew,fnew,gradnew]= armijostep(fgh_name,x,f,g,d,0.3,0.3,varargin{:});
      x=xnew;
      normg=norm(gradnew);
      normg_array=[normg_array;normg];
      %fprintf('normg=%.1e  ',normg)
      igrad_total=igrad_total+1;
      semilogy(normg_array, '*');  grid;title('Norm of Gradient in Newton Optimization');drawnow;
      if normg<normGrdTol, break; end
    end
   if normg<normGrdTol, break; end
  end


function x=cholsub(C,b,d)

%function x=cholsub(C,b)

% Solve C'Cx=b; C is upper triangular matrix;
%   or  C'diag(d)Cx=b

y=C'\b;

if nargin>2, y=y./d;end
    
x=C\y;

return
% 
% H=rand(5);H=H*H'; 
% b=rand(5,1) 
% C=chol(H); 
% x=cholsub(C,b);
% Hx=H*x
% 
% [C,D]=mchol(H);
% x=cholsub(C',b,diag(D));
% Hx=H*x
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%          MODIFIED CHOLESKY FACTORIZATION
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  [L,d,E,pneg]=mcholmz3(G)
%
%  Given a symmetric matrix G, find a matrix E of "small" norm and c
%  L, and D such that  G+E is Positive Definite, and 
%
%      G+E = L*diag(d)*L'
%
%  Also, calculate a direction pneg, such that if G is not PSD, then
%
%      pneg'*G*pneg < 0
%
%  Reference: Gill, Murray, and Wright, "Practical Optimization", p111.
%  Author:        Brian Borchers (borchers@nmt.edu)
%  Modification:  Michael Zibulevsky   (mzib@ee.technion.ac.il)
%
function [L,D,E,pneg]=mcholmz3(G)
%
%  n gives the size of the matrix.
%
n=size(G,1);
%
%  gamma, zi, nu, and beta2 are quantities used by the algorithm.  
%
gamma=max(diag(G));
zi=max(max(G-diag(diag(G))));
nu=max([1,sqrt(n^2-1)]);
beta2=max([gamma, zi/nu, 1.0E-15]);


C=diag(diag(G));

L=zeros(n);
D=zeros(n,1);  %  use vestors as diagonal matrices
E=zeros(n,1);  %%****************** **************


%
%  Loop through, calculating column j of L for j=1:n
%


for j=1:n,

    bb=[1:j-1];
    ee=[j+1:n];


    if (j > 1),
        L(j,bb)=C(j,bb)./D(bb)';     %  Calculate the jth row of L.  
    end;


    
    if (j >= 2)
        if (j < n), 
            C(ee,j)=G(ee,j)- C(ee,bb)*L(j,bb)';    %  Update the jth column of C.
        end;
    else
        C(ee,j)=G(ee,j);
    end;
    
    
    %%%%     Update theta. 
    
    if (j == n)
        theta(j)=0;
    else
        theta(j)=max(abs(C(ee,j)));
    end;
    
    %%%%%  Update D 
    
    D(j)=max([eps,abs(C(j,j)),theta(j)^2/beta2]');
    
    
    %%%%%%  Update E. 
    
    E(j)=D(j)-C(j,j);

    
    
    %%%%%%%  Update C again...

    
    %for i=j+1:n,
    %    C(i,i)=C(i,i)-C(i,j)^2/D(j,j);
    %end;
    
    ind=[j*(n+1)+1 : n+1 : n*n]';
    C(ind)=C(ind)-(1/D(j))*C(ee,j).^2;


end;

%
% Put 1's on the diagonal of L
%
%for j=1:n,
%    L(j,j)=1;
%end;

ind=[1 : n+1 : n*n]';
L(ind)=1;


%
%  if needed, finded a descent direction.  
%
if (nargout == 4)
    [m,col]=min(diag(C));
    rhs=zeros(n,1);
    rhs(col)=1;
    pneg=L'\rhs;
end;


return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%      ARMIJO INEXACT LINE SEARCH
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



function [x,f,grad,t] = armijostep(fg,x0,f0,grad0,dx,alpha,beta,varargin)

%armijostep - Descent of the function f(x) in direction dx:
%
%                     f(x0 + t*dx) <= f(x0) - alpha*|f'_dx (x0)|

%CALLED AS:
%==========
%
%        [x,f,grad,t,res_alpha] = armijostep(fg,x0,f0,grad0,dx,alpha,beta,varargin)
%
%INPUT:
%======
%
% fg    - name of matlab function, which computes function f and 
%         gradient grad at the point x:
%
%                   [f,grad]=fg(x,varargin)
%
%                   varargin - an arbitrary number of additional arguments   
%          
%
% x0    - initial value of the optimized variable x
% f0    - value  of the optimized function at the point x0
% grad0 - value  of the gradient at the point x0
% dx    - direction of search
% alpha - required descent rate:  f(x0 + t*dx) <= f(x0) - alpha*|f'_dx (x0)|
% beta  - factor of step decreasing: t_new = beta*t_old
%
% varargin - an arbitrary number of additional arguments,which are 
%            passed to fg(...)
%
%OUTPUT:
%=======
% x          - resulting x
% f          - resulting f(x)
% grad       - resulting gradient of f(x)
% t          - resulting search parameter: x = x0 + t*dx


  %Some parameters:
 
  nitermax=30;   % - max number of iterations
  t=1;           %  - initial stepsize


  df0=grad0'*dx; 
  if df0>0, 
    disp('Armijo search: Direction dx is of increase, changing it to the opposite !!!');
    dx=-dx; df0=-df0; 
  end

  x=x0+t*dx;
  [f,grad] = feval(fg,x,varargin{:});
  df=grad'*dx; 
  %if df<=0, return; end
    
  for i=1:nitermax,
    %fprintf('armijostep ');keyboard
    
    if (f <= f0 + alpha*df0*t) | ((df < 0) & (f < f0 + 1e-9 * max(1,abs(f0)))), return; end;
    %    if (f <= f0 + alpha*df0*t) | (df<0 & f<f0), return; end;
  
    fprintf('-');
    t= beta*t;
    x=x0+t*dx;
    [f,grad]=feval(fg,x,varargin{:}); 
    df=grad'*dx; 
    %if t<1e-6, fprintf('\n Armijostep.m); keyboard,end
  end    

  if f>f0, disp('Armijostep.m : f>f0 !!!');keyboard;end

return