% file name:  quadpen.m 
% This Matlab code implements the quadratic penalty method for 
%     min  f(x)  s.t.  h(x)=0
% Cauchy's steepest descent method using Armijo stepsize rule
% is used to minimize  f_c(x) = f(x) + c*||h(x)||^2/2 inexactly. 
% It terminates when the norm of the gradient is below epsilon.
% The objective function value is read from the file "f.m".
% The objective gradient value is read from the file "gradf.m".
% The constraint function value is read from the file "h.m".
% The constraint Jacobian value is read from the file "gradh.m".

% Read in inputs
  x=input('enter the initial column vector x ');
  c=input('enter the initial penalty ');

% Initialize 
  hx=h(x);
  gradfc=gradf(x)+c*gradh(x)*hx;

% Armijo stepsize rule parameters
  sigma = .1;
  beta = .5;
  k=0;  				% k is the total iteration count
  nf=0;				% nf is the total # function evaluations
  t1=cputime;
  
  while max(norm(gradfc),norm(hx)) > 1e-3

% Take epsilon to be inversely proportional to c 
    eps=1/c;
    fc=f(x)+c*norm(hx)^2/2;
    gradfc=gradf(x)+c*gradh(x)*hx;
    nf = nf+1;
 
% Apply steepest descent method to minimize f_c
    while  norm(gradfc) > eps
      d = -gradfc; 			% d is the direction
      t = 1;				
      while f(x+t*d)+c*norm(h(x+t*d))^2/2 - fc > sigma*t*gradfc'*d
        t = t*beta;
        nf = nf+1;
      end
      x = x + t*d;
      hx=h(x);
      fc=f(x)+c*norm(hx)^2/2;
      gradfc=gradf(x)+c*gradh(x)*hx;
      k = k + 1;
    end
    
    c=c*2;
    fprintf(' c= %g  \n',c);
    
  end
 
% Output x, k, nf, cpu
fprintf(' iter= %g  #func= %g  cpu= %g  \n',k,nf,cputime-t1);

  x