function [x0,Ac,ac,Nc,nc]=initial_condition(A,b,xk)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [x0,Ac,ac,Nc,nc]=initial_condition(A,b,x)                             %
%                                                                         %
%  It returns an initial feasible point and an adequate sets of active    %
% and inactive constraints. The function solve an auxiliary problem       % 
%                                                                         %
%                       min  J(x)=sum (bi-ai'*x), for i belong to V(k)    %
%                       subject to  ai'*x>=bi                             %
%                                                                         %
%   INPUTS:                                                               %
%       A  - matrix of the constraints (left side)                        %
%       v  - column vector of the constraints (right side)                %
%       x  - current point
%                                                                         %
%   OUTPUTS:                                                              %
%       x0 - initial feasible point                                       %
%       Ac - matrix of active constraints                                 %
%       ac - column vector of active constraints                          %
%       Nc - matrix of inactive constraints                               %
%       nc - column vector of inactive constraints                        %
%                                                                         %
%                                                                         %
%                                                                         %
%                                                 Last update: 21.05.2010 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nx=length(xk);                  % Number of variables x

Nc=A;                           % Creating of inactive set
nc=b;

Ac=eye(nx);                     % Creating of initial active set
ac=zeros(nx,2);                 % not. - first culomn contains right side
                                % of constraints
ac(:,1)=xk;                     %
                                %  second culomn denotes type of constraint
                                %                      0 - pseudoconstraint
                                %                      1 - inequality

% -------------------------------------------------------------------------
z=1;                            % If z=2, xk is feasible, else the opt.
                                %  algorithm has to be executed.


% ------------------------- Optimization algorithm ------------------------
while (z==1)
    if (~isempty(nc))          % It there is some constraint in inactive set
        fi=nc-Nc'*xk;          % a filter for objective function calculation
        nA=sum(fi>0);          % Number of overstep constraints
        nN=size(Nc,2)-nA;      % Number of kept constraints
        if (nA>0)
            Axk=zeros(nx,nA);
            axk=zeros(nA,2);
        end
        if (nN>0)
            Nxk=zeros(nx,nN);
            nxk=zeros(nN,2);
        end
        
        j=1;
        k=1;
        for i=1:length(nc)
            if fi(i)>0
                Axk(:,j)=Nc(:,i);
                axk(j,:)=[nc(i) i];
                j=j+1;
            else
                Nxk(:,k)=Nc(:,i);
                nxk(k,:)=[nc(i) i];
                k=k+1;
            end
        end
        
        if (nA>0)
            ek=ones(nA,1);               
            lambda=Ac\(-Axk*ek);% and index q
        else
            lambda=0;    
        end
        
        for i=1:length(lambda)
            if (lambda(i)>0) && (ac(i,2)==0)
                lambda(i)=-lambda(i);
                Ac(:,i)=-Ac(:,i);
                ac(i,1)=-ac(i,1);
            end
        end
    else
        lambda=0;             % else lambda is 0.
    end
    
    [lambdaq,q]=min(lambda);% is chosen   
    
    if ((lambdaq>=0) || (det(Ac)==0))% If there isn't any feasible descent direction
        x0=xk;                  % the algorithm terminate.
        [Ac,ac]=artific_remove(Ac,ac);
        break
    else
        Acp=eye(size(Ac))/Ac';
        sq=Acp(:,q);     
        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(nxk,1)      % All inactive constraints are tested
                ai=Nxk(:,i);
                 if (ai'*sq<0)         % and if this condition holds alfa is
                    alx=(nxk(i,1)-ai'*xk)/(ai'*sq);
                    if (alx>=0)      % calculated and saved to as.
                        as(j,:)=[alx nxk(i,2)];
                        j=j+1;          % and index pointer of as is increased. 
                    end                 % The variable i is index pointer of Nc and
                 end
        end                     % and is stored in second column of matrix
        
        for i=1:nA              % All active constraints are tested
                ai=Axk(:,i);
                 if (ai'*sq>0)         % and if this condition holds alfa is
                    alx=(axk(i,1)-ai'*xk)/(ai'*sq);
                    if (alx>=0)      % calculated and saved to as.
                        as(j,:)=[alx axk(i,2)];
                        j=j+1;          % and index pointer of as is increased. 
                    end                 % The variable i is index pointer of Nc and
                 end
        end                     % and is stored in second column of matrix        
                                % as.
        [~,p]=min(as(:,1)); % get the index of smallest abs. value and use it for getting the real value                        
        alfa=as(p,1);             % suitable alfa is chosen.
        
        p=as(p,2);              % index of a constraint, which will be 


        xk=xk+alfa*sq;          % New feasible solution is calculated. 
        if (p~=0)               % If some inactive constraint was used
                                % The p-th column is removed for inactive
                                % (Nc,nc) set to the active one (Ac,ac).
            [Ac,ac,Nc,nc]=set_modificationLP(Ac,ac,Nc,nc,q,p);
        end
    end
end

function [Xn,xn]=artific_remove(X,x)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [Xn,xn]=artific_remove(X,x)                                           %
%                                                                         %
%   The function remove all artificial constraints from variables X and x.%
%                                                                         %
%   INPUTS:                                                               %
%       X  - matrix created from left site of constraints                 %
%       x  - matrix - first column is made from right site  of constraints%
%            second one containts indicators:                             %
%                                   0 - artificial constraint             %
%                                   1 - inequality constraint             %               
%                                                                         %
%   OUTPUTS:                                                              %
%       Xn - modified matrix X                                            %
%       xn - vector created from first column of x after artificial const.%
%            removing.
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sx=size(x,1);
j=1;

Xn=[];
xn=[];

for i=1:sx
    if (x(i,2)~=0)
        Xn(:,j)=X(:,i);
        xn(j,1)=x(i,1);     
        j=j+1;
    end
end

function [Ac,ac,Nc,nc]=set_modificationLP(Ac,ac,Nc,nc,q,p)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [Ac,ac,Nc,nc]=set_modificationLP(Ac,ac,Nc,nc,q,p)                     %
%                                                                         %
%   The function change q-th culomn in matrix Ac and p-th culomn in matrix%
%   Nc. It does same with vector ac and nc. Pseudoconstraints are         %
%   removed from the problem once they come inactive.                     %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Xq=Ac(:,q);
xq=ac(q,1);
Xp=Nc(:,p);
xp=nc(p);

if (ac(q,2)~=0)                 % If it isn't pseudoconstraint
    Nc(:,p)=Xq;
    nc(p)=xq;
else
    Nc=remove_column(Nc,p);
    nc=remove_element(nc,p);
end

Ac(:,q)=Xp;
ac(q,:)=[xp 1];

function X=remove_column(X,i)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   X=remove_column(X,i)                                                  %
%                                                                         %
%   The function remove i-th column from the matrix X.                    %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sx=size(X,2);           % Number of column in matrix X

if (i==1)
    if (sx>1)
        X=X(:,2:end);
    else
        X=[];
    end
elseif (i==sx)
    X=X(:,1:i-1);
else
    X=[X(:,1:i-1) X(:,i+1:end)];
end
    
function x=remove_element(x,i)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   x=remove_element(x,i)                                                 %
%                                                                         %
%   The function remove i-th member of the vector x.                      %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sx=length(x);             % Number of member in vector x

if (i==1)
    if (sx>1)
        x=x(2:end);
    else
        x=[];
    end
elseif (i==sx)
    x=x(1:i-1);
else
    x=[x(1:i-1);x(i+1:end)];
end