function [GS,GSeq,GGS,GGSeq] = goalcon(V,neqgoal,funfcn,confcn,WEIGHT,GOAL,x,varargin)
%GOALCON Utility function to translate gradient in goal-attainment problem.
%   Intermediate function used to translate goal attainment
%   problem into constrained optimization problem.
%   Used by FGOALATTAIN, FMINIMAX, and FMINCON (via private/NLCONST). 
%
%   See also GOALFUN.

%   Copyright 1990-2005 The MathWorks, Inc.
%   $Revision: 1.6.4.4 $  $Date: 2005/06/21 19:44:00 $

nx=length(V)-1;
x(:)=V(1:nx);
lambda=V(nx+1);

% Compute the constraints
f=[]; gf=[];  % Tell parser that f and g are variables.
switch funfcn{1} % evaluate function and possibly gradients
case 'fun'
   f = feval(funfcn{3},x,varargin{:});  
case 'fungrad'
   [f,gf] = feval(funfcn{3},x,varargin{:});
case 'fun_then_grad'
   f = feval(funfcn{3},x,varargin{:});  
   gf = feval(funfcn{4},x,varargin{:});
otherwise
   error('optim:goalcon:UndefinedCalltypeFgoalattain','Undefined calltype in FGOALATTAIN.');
end

gc = []; gceq=[]; c=[]; ceq=[];
% Evaluate constraints
switch confcn{1}
case 'fun'
   [ctmp,ceqtmp] = feval(confcn{3},x,varargin{:});
   c = ctmp(:); ceq = ceqtmp(:);
   gc = [];
   gceq = [];
case 'fungrad'
   [ctmp,ceqtmp,gc,gceq] = feval(confcn{3},x,varargin{:});
   c = ctmp(:); ceq = ceqtmp(:);
   
case 'fun_then_grad'
   [ctmp,ceqtmp] = feval(confcn{3},x,varargin{:});
   c = ctmp(:); ceq = ceqtmp(:);
   [gc,gceq] = feval(confcn{4},x,varargin{:});
  
case ''
   c=[]; ceq =[];
   gc = [];
   gceq = [];
otherwise
   error('optim:goalcon:UndefinedCalltypeFgoalattain','Undefined calltype in FGOALATTAIN.');
end

nGoalCnstr = length(f); % number of goal attainment constraints 

% Calculate goal attainment constraints ( f(i)-GOAL(i) )/WEIGHT(i)-lambda <= 0
GS = zeros(nGoalCnstr+neqgoal,1);
for i=1:nGoalCnstr
     if WEIGHT(i)~=0
       diff=f(i)-GOAL(i);
       GS(i)=sign(real(diff))*norm(diff)/WEIGHT(i)-lambda; 
       if i<=neqgoal % neqgoal comes from options.GoalsExactAchieve 
          GS(i+nGoalCnstr)=-GS(i)-2*lambda; % f(i)+lambda*WEIGHT>=GOAL
       end
     else % hard constraint
       GS(i)=f(i)-GOAL(i);
       if i<=neqgoal 
          GS(i+nGoalCnstr)=-GS(i)-1e-10; % f(i)>=GOAL(i)-1e-10
       end
     end
end

% Append goal attainment constraint at the end of inequalities vector
GS=[c(:);GS]; 

% Equalities and gradient matrix of equalities
GSeq = ceq(:);
size_ceq = size(GSeq);
GGSeq = [gceq; zeros(1,size_ceq(1))]; % add zero row (derivatives w.r.t. lambda)

if isempty(gf) && isempty(gc)
   GGS=[]; GGSeq = [];
elseif (isempty(gf) && ~isempty(gc)) 
   error('optim:goalcon:GradFunRequiredForGradCon','Must provide gradient of function if gradient of constraints to be used.');
else % grad of f available, grad of inequalities available or not
   
   % Gradient matrix of inequality constraints: 
   % [grad_x of inequalities c(x)          | grad_x of attainment constraint]
   % [zero row (derivatives w.r.t. lambda) | -1 -1 . . . . . .  . . . . ..-1]
   GL = -ones(1,nGoalCnstr+neqgoal);
   for i=1:nGoalCnstr
      if WEIGHT(i)~=0
         gf(:,i)=gf(:,i)/WEIGHT(i);
         if i<=neqgoal,
            gf(:,i+nGoalCnstr)=-gf(:,i);
         end 
      else % hard constraint
         GL(1,i)=0; 
      end
   end
   
   GGS=[gf;GL];
   
   sizegc=size(gc);
   % Put gc first
   if sizegc(1)>0, 
      GGS=[[gc;zeros(1,sizegc(2))],GGS]; 
   end
end
