function [bas,ib,il,iter,tab] = qpdantzg(tab,bas,ib,il,maxiter)
% QPDANTZG Solve quadratic program using Dantzg's algorithm.
% 	[bas, ib, il, iter, tab] = qpdantzg(tabi, basi, ibi, ili, maxiter)
%
% Inputs:
%  tabi      : Initial tableau, double, size of [n, n], where n is number
%              of variables to be determined (including slack variables).
%              The procedure determines an equal number of Lagrange
%              multipliers.
%  basi      : Initial basis, double, size of [n 1].
%  ibi       : Integer, size of [n 1].  If ibi(j) = k > 0, variable j is
%              basic initially and its value is basi(k).  Else if k < 0,
%              variable j is non-basic and k is its column in tabi.
%  ili       : Integer, size of [n 1].  If ili(j) = k > 0, multiplier j is
%              basic initially and its value is basi(k).  Else if k < 0,
%              multiplier j is non-basic and k is its column in tabi.
%  maxiter   : Integer, size of [1 1], limit on the number of iterations.
%
% Outputs:
%  bas       : Final basis vector
%  ib        : Final index vector for the variables -- see above
%  il        : Final index vector for the lagrange multipliers -- see
%              above.
%  iter      : Iteration counter.  If between 1 and maxiter, the solution
%              satisfies the Kuhn-Tucker optimality conditions.  If
%              maxiter+1, the maximum number of iterations was reached and
%              the solution is feasible but not optimal. If zero, the
%              initial basis was infeasible.
%  tab       : Final tableau
%
% Restrictions:
%  1.  The QP's Hessian matrix must be positive definite.
%  2.  The variables must be defined such that they are >= 0.
%  3.  The initial basis must be feasible, i.e., all variables in the basis
%      must be non-negative.
%  4.  The initial basis must be STANDARD, i.e., satisfying the
%      optimality condition that if ibi(j) > 0, ili(j) < 0 and vice versa.
%
% NOTE:  QPDANTZG does not verify that the QP meets these conditions.

% Author:  N. L. Ricker
% Copyright 1994-2011 The MathWorks, Inc.

%#codegen
coder.allowpcode('plain');
ZERO = zeros('like',tab);
ONE = ones('like',tab);

% Initialize the output variables
n = length(bas);

% Check initial feasibility
% if any(bas(ib(ib > ZERO)) < ZERO)
%     % Signal infeasible initial basis and return
%     iter = ZERO;
%     return
% end
for ct = 1:length(ib)
    if ib(ct) > ZERO
        tmp = ib(ct);
        if bas(tmp) < ZERO
            iter = ZERO;
            return
        end
    end
end

%  Initialize for main loop for QP solution procedure.
iter = ONE;
StandardBasis = true;
iBpair = ZERO;
iNBpair = ZERO;
while iter <= maxiter
%     fprintf('iter = %i, Standard = %i, iBpair = %i, iNBpair = %i\n', ...
%         iter, StandardBasis, iBpair, iNBpair)
    if StandardBasis
        % Check the values of the basic multipliers.  If all are >= ZERO,
        % we are finished.  Else, identify the minimum value.  In the
        % process, any basis variables that have gone negative due to
        % numerical imprecision are set to zero.
        iMult = ZERO;
        MinMult = ZERO;
        for i = ONE:n
            if il(i) > ZERO
                if bas(il(i)) < MinMult
                    iMult = i;
                    MinMult = bas(il(i));
                end
            end
            if ib(i) > ZERO
                if bas(ib(i)) < ZERO
                    bas(ib(i)) = ZERO;
                end
            end
        end                    
                
        if MinMult >= ZERO
            % Standard exit with optimal solution
            return
        end
        % Optimality condition not satisfied.  The variable corresponding
        % to the minimum multiplier must enter the basis.
        VariableEntering = true;
        iAdd = iMult;   % Identifies variable entering basis
        % Tau is its column in the tableau
        Tau = tab(:,-ib(iAdd));     
        % Determine what must leave.  Candidates are multiplier iMult or
        % any basic variable that would go negative (infeasible).  To check
        % the latter, find minimum positive ratio of each basic variable to 
        % its entry in the tableau column of the entering variable.
        [MinRatio, iVar] = FindMinimumRatio(n, ib, bas, Tau);        
        if iVar <= ZERO
            % No chance of infeasibility, so multiplier leaves.
            iRem = iMult;
            VariableLeaving = false;
        else
            % At least one basic variable could go negative.
            if Tau(il(iMult)) >= ZERO
                % Multiplier can't have the minimum positive ratio, so
                % variable must leave
                iRem = iVar;
                VariableLeaving = true;
            else
                if bas(il(iMult))/Tau(il(iMult)) <= MinRatio
                    % Multiplier leaves
                    iRem = iMult;
                    VariableLeaving = false;
                else
                    % Variable leaves
                    iRem = iVar;
                    VariableLeaving = true;
                end
            end
        end        
    else
        % Basis is non-standard.  Multiplier of the non-basic pair must
        % enter the basis
        iAdd = iNBpair;
        VariableEntering = false;
        % Tau is its column in the tableau
        Tau = tab(:,-il(iAdd));
        % Compute ratios to see whether infeasibility could occur
        [MinRatio, iVar] = FindMinimumRatio(n, ib, bas, Tau);
        if iVar <= ZERO
            % No chance of infeasibility, so multiplier of basic pair leaves.
            iRem = iBpair;
            VariableLeaving = false;
        else
            % At least one basic variable could go negative.
            if Tau(il(iBpair)) >= ZERO
                % Multiplier can't exhibit the minimum positive ratio, so
                % variable must leave.
                iRem = iVar;
                VariableLeaving = true;
            else
                if bas(il(iBpair))/Tau(il(iBpair)) <= MinRatio
                    % Multiplier leaves
                    iRem = iBpair;
                    VariableLeaving = false;
                else
                    % Variable leaves
                    iRem = iVar;
                    VariableLeaving = true;
                end
            end
        end        
    end    
    % Start of procedure that switches basis variables.  Update the
    % pointers and the tableau such that the variable (or multiplier)
    % becoming non-basic replaces the one becoming basic in the tableau.
    % Also update the basis accordingly.
    
    % Identify affected row in basis and column in tableau.
    if VariableEntering
        ic = -ib(iAdd);
    else
        ic = -il(iAdd);
    end
    if VariableLeaving
        ir = ib(iRem);
    else
        ir = il(iRem);
    end
    % Compute and save new values
%     fprintf('    ir = %i, ic = %i\n', ir, ic)
    PivotInv = ONE/tab(ir,ic);
    tab(ir,:) = tab(ir,:)*PivotInv;
    temp = bas(ir)*PivotInv;
    bas = bas - temp*tab(:,ic);
    bas(ir) = temp;
    tab(ir,ic) = ZERO;
    tab = tab - tab(:,ic)*tab(ir,:);
    tab(:,ic) = -PivotInv*tab(:,ic);
    tab(ir,ic) = PivotInv;
    % Update pointers
    if VariableEntering
        ib(iAdd) = ir;
    else
        il(iAdd) = ir;
    end
    if VariableLeaving
        ib(iRem) = -ic;
    else
        il(iRem) = -ic;
    end
    % Determine whether next basis will be standard
    iBpair = ZERO;
    iNBpair = ZERO;
    for i = ONE:n
        if ib(i) > ZERO && il(i) > ZERO
            % Both variable and multiplier are basic (non-standard)
            iBpair = i;
        elseif ib(i) < ZERO && il(i) < ZERO
            iNBpair = i;
        end
        if ~iBpair && ~iNBpair
            StandardBasis = true;
        else
            StandardBasis = false;
        end
    end
    iter=iter+ONE;
end

function [MinRatio, iVar] = FindMinimumRatio(n, ib, bas, Tau)
% Helper function determines minimum positive ratio for use in ratio test
ZERO = zeros('like',bas);
ONE = ones('like',bas);
iVar = ZERO;
MinRatio = ZERO;
for i = ONE:n
    ir = ib(i);
    if ir > ZERO
        % Variable is basic and  must be non-negative.  Can only
        % provide minimum positive ratio if its element in Tau is positive
        if Tau(ir) > ZERO
            if iVar > ZERO
                Check = bas(ir)/Tau(ir);
                if Check < MinRatio
                    MinRatio = Check;
                    iVar = i;
                end
            else
                iVar = i;
                MinRatio = bas(ir)/Tau(ir);
            end
        end
    end
end
        
