function x0=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                         %
%                                                                         %
%  Author: Pavel Skrabanek                        Last update: 10.01.2011 %
%  Version: 1.1                                                           %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%----------------------------------- Loop ---------------------------------
zsg=zeros(length(x0),1);        % Culomn matrix of zeros of x length
noc=length(nc)+length(ac);      % Number of constraints


invG=eye(size(G))/G;            % Inversion of matrix G
k=0;                            % Set the counter

while (k<noc)                   % The solution has to be find maximal in n
                                % steps.
% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - %
%   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
        T=0;                    % if the active set is empty.
        H=invG;
    else                        % else:
        HT=Ac'*invG;
        T=invG*Ac/(HT*Ac);
        H=invG-T*HT;        
    end
    delta=-H*gk;                % Delta contains feasible changes of x 
                                % leading to the optimal solution of actual
                                % objective function.    
    
    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.
        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 
            break               % the objective function and the algorithm 
                                % 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)';
%             aip=ai*delta;
%             if (aip<0)    % and if this condition holds alfa is
%                 alx=(nc(i,1)-ai*x0)/aip;
%                 if (alx>=0)     % calculated and saved to as.
%                     as(j,:)=[alx i];
%                     j=j+1;      % and index pointer of as is increased.
%                 end
%             end                 % The variable i is index pointer of Nc and
%         end                     % and is stored in second column of matrix
%                                 % as.



        Ai=Nc';
        ncl=size(Nc,2);
        Aip=Ai*delta;
        Alx=[(nc-Ai*x0)./Aip (1:ncl)'];
        asi=zeros(1,ncl);
        for i=1:ncl
            if (Aip(i)<0) && (Alx(i)>=0) 
                asi(j)=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.   
        as=Alx(asi(1:j-1),:);
                                
        
        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
        k=k+1;                  % Update the counter
    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