function x=qpas(x0,G,g,Ac,ac,Nc,nc)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   x=qpas(x0,G,g,Ac,ac,Nc,nc)                                            %
%                                                                         %
%   QPAS is a short cut from Quadratic Programming - Active Set method.   %
%  It returns feasible solution of optimization problem with constraints. %
%                                                                         %
%                       min  J(x)=1/2*x'*G*x+g'*x                         %
%                       subject to  Ac'*x>=b                              %
%                                                                         %
%   INPUTS:                                                               %
%       x0 - initial feasible point (vector)                              %
%       G  - matrix defining the objective function                       %
%       g  - vector defining the objective function                       %
%       Ac - matrix of active constraints                                 %
%       ac - vector of active constraints                                 %
%       Nc - matrix of inactive constraints                               %
%       nc - vector of inactive constraints                               %
%                                                                         %
%   OUTPUTS:                                                              %
%       x  - feasible solution of optimization problem                    %
%                                                                         %
%   Variables x0, Ac, ac, Nc and nc should be generated by the function   %
% initial_condition.                                                      %          
%                                                                         %
% Remark:                                                                 %
%       It does not consider equality constraints                         %
%                                                                         %
%                                                 Last update: 16.09.2009 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%----------------------------------- Loop ---------------------------------
z=1;                            % It ensures an infinite loop
zsg=zeros(length(x0),1);        % Culomn matrix of zeros of x length

while (z==1)
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
%   Only active constraints are considered (defined by Ac and ac). The    %
% problem is handled like equality problem and transformed to the form    %
%                                                                         %
%              min  J(delta)=1/2*delta'*G*delta+delta'*g(k)               %
%              subject to  Ac'*delta=0                                    %
%                                                                         %
% where delta at iteration k is defined by equation                       %
%              delta(k)=x(k+1)-x(k)                                       %
%                                                                         %
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %

    gk=g+G*x0;                  % Calculation of g(k) from the original
                                % objective function g(k)=g+G*x(k).
                                                        
    if (size(Ac,1)==0)          % The problem is handled like unconstrained
        H=inv(G);               % if the active set is empty 
    else                        % else:
        H=inv(G)-inv(G)*Ac*inv(Ac'*inv(G)*Ac)*Ac'*inv(G);
    end
    delta=-H*gk;                % Delta contains feasible changes of x 
                                % leading to the optimal solution of actual
                                % objective function.
    delta=round(delta*1e6)/1e6;
    if (delta==zsg)             % Termination test has to be made or active
                                % set has to be modified if delta is 
                                % solution of the current objective
                                % function.
        if (size(Ac,1)==0)      % The problem is handled like unconstrained
            T=0;                % if the active set is empty.
        else                    % else:
            T=inv(G)*Ac*inv(Ac'*inv(G)*Ac);
        end
        lambda=T'*gk;           % Lambda contains slopes of feasible edges
        [lambdaq,q]=min(lambda);% and if all of them are bigger or equal to
        if (lambdaq>=0)         % zero there is no feasible way to reduce 
            x=x0;               % the objective function and the algorithm 
            break               % terminated.
        else                    % Otherwise the active set is modified.                        
            [Ac,ac,Nc,nc]=set_modification(Ac,ac,Nc,nc,q);
                                % It is removed the q-th constraints from 
                                % active set (Ac,ac). This constraint is
        end                     % added to inactive set (Nc,nc). 

    else                        % New feasible point has to be calculated
                                % if the vector delta is nonzero.
                                % A length of the step between x(k) and
                                % x(k+1) has to be determined. It is
                                % defined by delta and alfa where alfa is
                                % a number from (0;1>.  
        as=[];                  % This matrix will contain all possible 
                                % alfa.  
        j=1;                    % The index pointer for as is set to 1 on 
                                % begin.
        for i=1:size(Nc,2)      % All inactive constraints are tested !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
            ai=Nc(:,i);
            if (ai'*delta<0)    % and if this condition holds alfa is
                                % calculated and saved to as.
                as(j,:)=[(nc(i,1)-ai'*x0)/(ai'*delta) i];
                j=j+1;          % and index pointer of as is increased. 
            end                 % The variable i is index pointer of Nc and
        end                     % and is stored in second column of matrix
                                % as.
        
        if size(as)>=1          % If some alfa has been determined a 
            [alfa,p]=min([1;as(:,1)]);% suitable alfa is chosen. 
            if (p~=1)           % If the best isn’t the first of [1;as(:,1)]
                p=as(p-1,2);    % the index of a constraint, which will be 
                                % replaced to active set, is saved to p
            else                % else the index pointer is set to zero.
                p=0;            % 'No constraint will be added to the
            end                 % active set.'
        else
            alfa=1;             % If no alfa is determined, alfa is set to
            p=0;                % one and index pointer p is zero.
        end                     % 'No constraint will be added to the
                                % active set.'      

        x0=x0+alfa*delta;       % New feasible solution is calculated. 
        if (p~=0)               % If some inactive constraint was used
                                % The p-th column of inactive set (Nc,nc)
                                % is replaced to the active set (Ac,ac).
            [Nc,nc,Ac,ac]=set_modification(Nc,nc,Ac,ac,p);
        end                                                    
    end
end

function [X,x,Y,y]=set_modification(X,x,Y,y,i)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [X,x,Y,y]=set_modification(X,x,Y,y,i)                                 %
%                                                                         %
%   This function removes i-th column from the matrix X. This column is   %
% added to the matrix Y. The function does same with i-th element of      %
% vector x.                                                               %                                                               
%                                                                         %
%   VARIABLES:                                                            %
%       X,Y - matrix                                                      %
%       x,y - column vector                                               %
%       i   - index of column X (element x)                               %
%                                                                         %
%                                                 Last update: 16.09.2009 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Y=[Y X(:,i)];
y=[y;x(i)];
if (i==1)
    if (size(X,2)>1)
        X=X(:,2:end);
        x=x(2:end);
    else
        X=[];
        x=[];
    end
elseif (i==size(X,2))
    X=X(:,1:end-1);
    x=x(1:end-1);
else
    X=[X(:,1:i-1) X(:,i+1,end)];
    x=[x(1:i-1);x(i+1,end)];
end