function xopt=lpfeasdir(A,b,c,x0)
% LPFEASDIR is a linear programming problem solver using a feasible direction
% algorithm
%Written by Vincent Nguyen, 10/2008
%
% LPFEASDIR(A,B,C,X0): attempts to solve a problem of the form maximize(B'X)
% subject to A'X >= C, near the initial point X0.  The maximization is
% respect to the parameter, X which can be a vector of length N.  C is a
% vector of with length M, which reflects the number of constraints.
%
% The input parameters should be characterized as follows:
%
% X0 is the initial point and should be of size N x 1.  X0 must lie in the
% feasible domain, i.e. A’X0 >= C.
%
% A and C define the constraints.  A should be size N x M, and C should be
% size M x 1
%
% B defines the linear function to be maximized, and is of size N x 1.


%max b'x => min -b'x so g=-b for the minimization problem

%check sizes of everything and that x0 is feasible
m=length(c);
n=length(b);
[An Am]=size(A);
if (length(x0)~=n)||(An~=n)||(Am~=m)
    xopt=[];
    fprintf('Dimensions incorrect\n')
    return
elseif sum(A'*x0<c)>0
    xopt=[];
    fprintf('x0 is not feasible\n');
    return
end

%check to see if x0 is on any constraints
%indexes of the active and nonactive constraints
wbars=find(A'*x0-c);
ws=setdiff(1:m,wbars);

xk=x0;

%If not on a constraint, move to hit at least one constraint before the
%loop. This avoids having to check if ws is empty every iteration before
%calculating the lamdas.
if isempty(ws)
    p=b;
    alphas=(c(wbars)-A(:,wbars)'*xk)./(A(:,wbars)'*p);
    alphamin=max(alphas);
    for count=1:length(alphas)
        if (alphas(count)>=0)&&(alphas(count)<=alphamin)
            index=count;
            alphamin=alphas(count);
        end
    end
    if alphamin<0  %means no constraints intersect in direction of decent
        fprintf('Unbounded problem \n')
        xopt=ones(n,1)*inf;
        return
    end
    xk=xk+alphamin*p;
    ws(end+1)=wbars(index);
    wbars=wbars([1:index-1 index+1:end]);
end

%Create the original factorization of the Aw matrix
[Q R]=qr(A(:,ws));

while(1)
    Q1=Q(:,1:length(ws));
    R1=R(1:length(ws),1:length(ws));
    lamdas=R1\(Q1'*(-b));
    [y i]=min(lamdas);
    dropped=0;
    if y<0
        [Q R]=qrdelete(Q,R,i);
        dropped=ws(i);
        %dropped constraint is not added to wbars until end of iteration.
        %This is so that the constraint will not pop up as the first
        %intersecting constraint along the step direction.
        ws=ws([1:i-1 i+1:end]);
    end

    if length(ws)==n
        %if this is true then all lamdas are pos, and fully constrainted => opt
        xopt=xk;
        return
    else
        %check to make at least one constraint is active
        if isempty(ws)
            p=b; %if no constraints are active, search direciton = -gradient
        else
            Z=Q(:,length(ws)+1:end);
            p=Z*(Z'*b);
        end

        if norm(p)~=0 %if there is a feasible downhill dir
            if isempty(wbars)
                %this means that there is a downhill dir, but no more
                %inactive constraints, so you can walk forever in direction
                %p.  this can happen if m < n.
                fprintf('Unbounded problem \n')
                xopt=ones(n,1)*inf;
                return
            end
            alphas=(c(wbars)-A(:,wbars)'*xk)./(A(:,wbars)'*p);
            alphamin=max(alphas);
            for count=1:length(alphas)
                if (alphas(count)>=0)&&(alphas(count)<=alphamin)
                    index=count;
                    alphamin=alphas(count);
                end
            end
            if alphamin<0  %means no constraints intersect in direction of decent
                fprintf('Unbounded problem \n')
                xopt=ones(n,1)*inf;
                return
            end
            xk=xk+alphamin*p;
            [Q R]=qrinsert(Q,R,length(ws)+1,A(:,wbars(index)));
            ws(end+1)=wbars(index);
            wbars=wbars([1:index-1 index+1:end]);
        else %there is no feasible downhill direction
            xopt=xk;
            return
        end
    end
    if dropped~=0 %if a constraint was dropped it is added to wbars here
        wbars(end+1)=dropped;
    end
end