function [x,y,ez,flag] = simplex(A,b,c,x0,B0)
% function [x,y,z,flag] = simplex(A,b,c,x0,B0)
% Fernando Guevara Vasquez - Oct. 2004
% Solves the LP
%
%       min  c'*x 
%       s.t. Ax=b, x>=0.
%
% using the simplex method. Based on Chapter 13 in Nocedal and Wright.
%
% inputs:
%  A	mxn matrix (m<n)
%  b	m vector (RHS of constraints)
%  c	n vector (objective)
%  x0	[optional] n vector basic feasible point
%  B0	[optional] m vector of indices corresponding to basis of x0
%		   if this parameter is ommitted, defaults to x0>0
% outputs:
%  x	n vector optimal solution to the LP   (only if flag==0)
%  y	m vector optimal Lagrange multipliers (only if flag==0)
%  z	n vector optimal slacks               (only if flag==0)
%  flag	0 if optimal solution was found
%       1 if problem is unbounded
%       2 if big-M method failed to find a bfp (which maybe due to M
%	  not being big enough)

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % parameters
 eps = 1e-10;
 M = 1e6;
 debug=1;
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % sanity checks
 m=size(A,1); n=size(A,2);
 if (m>=n) error('must have m<n'); end;
 if (m~=size(b,1) | 1~=size(b,2)) error('b must be an mx1 vector'); end;
 if (n~=size(c,1) | 1~=size(c,2)) error('c must be an nx1 vector'); end;
 if (rank(A)<m) error('A must be full rank'); end;

 % only in the case where an initial BFP is given
 if (nargin>=4)
  if (n~=size(x0,1) | 1~=size(x0,2)) 
   error('x0 must be an nx1 vector');
  end;
  % find the basis set for x0 (if not given)
  if (nargin==5)
   B = B0;
  else
   B = find(x0>0);
  end;
  if (length(B)~=m)
   error('start bfp does not have exactly m non zero entries');
  end;
  % some checks to see if (x0,B) is really a bfp
  % check feasability of x0
  if (any(x0<0)) error('x0 not feasible: it has negative entries!!'); end;
  if (norm(A*x0-b,inf)>eps) error('x0 not feasible: Ax0 != b '); end;
  % check all components outside basis are zero
  if ( sum(x0(setdiff(1:n,B)))>eps ) 
   error('not all components of x0 outside basis set are zero');
  end;
  % check A(:,B) is invertible
  if (rank(A(:,B))<m) 
   error('the columns of A corresp. to this bfp are not lin. indep');
  end;
  x=x0;
 end;

 if (debug)
  fprintf('** m=%d,n=%d\n',m,n);
 end;

 % if no initial bfp is provided, we default to the big-M method
 if (nargin==3)
  if (debug) fprintf('** no initial bfp provided -> big-M method\n'); end;
  E = 1*(b>=0) -1*(b<0); % I don't use sign since sign(0)=0
  E = diag(E); 
  [xM,yM,zM,xMflag] = simplex([A E],b,[c; M*ones(m,1)],...
                              [zeros(n,1); abs(b)],n+1:n+m);
  x = xM(1:n);
  y = yM;
  ez = zM(1:n);
  flag = 2*xMflag;
  return;
 end;
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % main loop
 icount=0;
 while (1)
  icount=icount+1;
  if (debug) 
   fprintf('-- iteration %d\n',icount);
   fprintf('B: '); fprintf('%d ',B); fprintf('\n');
  end;
  I = setdiff(1:n,B);
  
  % 1)
  y = A(:,B)'\c(B);
  z = c(I) - A(:,I)'*y;
  
  % 2) check for solution
  if (all(z>=0))
   % we have found a solution
   flag=0;
   if (debug) fprintf('\n** simplex found optimal solution\n'); end;
   % expand z
   ez = zeros(n,1);
   ez(I) = z;
   return;
  end;

  % 3) choose entering index q according to Bland's rule to avoid cycling
  %    (ie choose the smallest)
  iq = find(z<0); iq = iq(1); q = I(iq);
  
  % 4) Compute descent direction
  d = - A(:,B)\A(:,q);

  % 5) check for unboundedness
  if (all(d>=0))
   % the problem is unbounded
   flag=1;
   if (debug) fprintf('\n** the problem is unbounded\n'); end;
   % expand z
   ez = zeros(n,1);
   ez(I) = z;
   return;
  end;

  % 6) Choose the index that we are replacing q with
  idx = B(find(d<0));
  tmp = -x(idx)./d(find(d<0));
  t = min(tmp);
  % use Bland's rule: find the smallest one realizing the min
  l = idx(find(t==tmp)); l = l(1);
  
  % 7) update x and the basis B
  x(B) = x(B) + t*d; % we are doing the op. for x(l) too
  x(q) = t;
  B = [ setdiff(B,l) q];
  x(setdiff(1:n,B)) = 0; % but here x(l) = 0
 end;