// XXX FIXME XXX leave this in until problems with incorrectly specified
// number of equality constraints is resolved.
#define BOUNDS_CHECKING

#include <octave/oct.h>
#include <octave/pager.h>
#include <lo-ieee.h>
#include <cfloat>
#ifdef USE_OCTAVE_NAN
#define lo_ieee_nan_value() octave_NaN
#define lo_ieee_inf_value() octave_Inf
#endif

using namespace std;

// Copyright (C) 2000 Ben Sapp.  All rights reserved.
//
// This is free software; you can redistribute it and/or modify it
// under the terms of the GNU General Public License as published by the
// Free Software Foundation; either version 2, or (at your option) any
// later version.
//
// This is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
// for more details.

// 2001-09-20 Paul Kienzle <pkienzle@users.sf.net>
// * Use x((int)rint(m(i,j))) instead of x(m(i,j))

#define TRUE 1
#define FALSE 0

// 2.1.51 and later have identity matrix defined in utils.h, but can't use
// it to ensure that the code works with older octave versions
static inline Matrix lp_identity_matrix(int m,int n)
{
  int min = (m > n) ? n : m; 
  Matrix me(m,n,0.0);
  for(int i=0;i<min;i++){
    me(i,i) = 1.0;
  }
  return(me);
}

static inline Matrix lp_identity_matrix(int n)
{
  return(lp_identity_matrix(n,n));
}

// It would be nice if this was a function in the Matrix class
static Matrix pivot(Matrix T, int the_row,int the_col)
{
  int nr = T.rows();
  Matrix Id = lp_identity_matrix(nr);
  Matrix result;
  for(int i=0;i<nr;i++){
    Id(i,the_row) = -T(i,the_col)/T(the_row,the_col);
  }
  Id(the_row,the_row) = 1/T(the_row,the_col);
  //octave_stdout << "After pivot T =\n" << Id*T << "\n";
  result = Id*T;
  for(int j=0;j<nr;j++){
    T(j,the_col) = 0;
  }
  T(the_row,the_col) = 1;
  return(result);
}

// Remove the 
static Matrix multi_pivot(Matrix T,Matrix the_pivots)
{
  if(the_pivots.cols() != 2){
    octave_stdout << "Error in multi_pivot\n";
  }
  int nr = the_pivots.rows();
  for(int i=0;i<nr;i++){
    T = pivot(T,int(the_pivots(i,0)),int(the_pivots(i,1)));
  }
  return(T);
}

#define CASE_A 0 // h_j -> The upper bound is the smallest
#define CASE_B 1 // min{y_i0/y_ij} for all y_ij > 0 is the smallest 
#define CASE_C 2 // min{(y_i0-h_j)/y_ij) for all y_ij < 0 is the smallest
// This takes a tableau and reduces it to the solution tableau.  
static Matrix minimize(Matrix T, Matrix &basis,ColumnVector upper_bounds,RowVector &which_bound)
{
  int nr = T.rows();
  int nc = T.cols();
  int i,j,k,min_case;
  double min_val;
  int min_row = 0;
  int found_one;

  while(TRUE){
    found_one = FALSE;
    for(i=0;i<(nc-1);i++){
      if(T(nr-1,i) < -10.0*DBL_EPSILON){
	found_one = TRUE;
	break;
      }
    }
    if(!found_one){
      break;
    }
    // Now i is the column we will pivot on.  
    min_row = -1;
    min_case = CASE_A;
    min_val = upper_bounds(i);
    for(j=0;j<(nr-1);j++){
      if(T(j,i) > 0){
	if(min_val > (T(j,nc-1)/T(j,i))){
	  min_row = j;
	  min_val = T(j,nc-1)/T(j,i);
	  min_case = CASE_B;
	}
      }else if(T(j,i) < 0){
	if(min_val > ((T(j,nc-1)-upper_bounds(i))/T(j,i))){
	  min_row = j;
	  min_val = (T(j,nc-1)-upper_bounds(i))/T(j,i);
	  min_case = CASE_C;
	}
      }
    }
    
    int interesting_column=0;
    for(j=0;j<(nr-1);j++){
      if(basis(j,0) == min_row){
	interesting_column = int(basis(j,1));
	/*basis(j,0) = min_row;
	  basis(j,1) = i; */
	break;
      } 
    }
    
    // Now min_row is the row and i is the column to pivot on.
    Matrix Id;
    switch(min_case)
      {
      case CASE_A:
	for(k = 0; k < nr; k++)
	  {
	    T(k,i) = -T(k,i);
	    T(k,nc-1) += T(k,i);
	  }
	which_bound(i) = -which_bound(i);
	break;
      case CASE_B:
	T = pivot(T,min_row,i);
	for(j=0;j<nr;j++)
	  {
	    if(basis(j,0) == min_row)
	      { // This row was just pivoted out
		basis(j,0) = min_row;
		basis(j,1) = i;
		break;
	      }
	  }
	break;
      case CASE_C:
	which_bound(min_row) = -which_bound(min_row);
	T(min_row,interesting_column) = -T(min_row,interesting_column);
	T(min_row,nc-1) -= upper_bounds(interesting_column);
	T = pivot(T,min_row,i);// Be careful about the one below
	for(j=0;j<nr;j++)
	  {
	    if(basis(j,0) == min_row)
	      { // This row was just pivoted out
		basis(j,0) = min_row;
		basis(j,1) = i;
		break;
	      }
	  }
	break;
      default:
	break;
      }
  }
  return(T);
}

// A should be a tableau without the cost on the bottom. basis stores the current basis.  
static ColumnVector extract_solution(Matrix A,Matrix basis,ColumnVector upper_bounds,RowVector which_bound)
{
  ColumnVector x;
  int i;
  int nr = A.rows();
  int nc = A.cols()-1;
  if(basis.rows() != nr)
    {
      octave_stdout << "lp: internal error in extract_solution\n";
      octave_stdout << "please report this problem\n";
      octave_stdout << "A = \n" << A << "\n";
      octave_stdout << "basis =\n" << basis << "\n";
      octave_stdout << "nr = " << nr << ", basis.rows() = " << basis.rows() << "\n";
    }
  x = ColumnVector(nc,0.0);
  for(i=0;i<nr;i++)
    {
      // XXX FIXME XXX the condition used to be <= nc which is clearly
      // wrong since the column vector x only has nc elements, and
      // x(nc) is out of range.  Is this restricting the test to <nc
      // the proper fix?  [PAK]
      if(basis(i,1) < nc)
	{
	  x((int)rint(basis(i,1))) = A(i,nc);
	}
    }
  for(i=0;i<nc;i++)
    {
      if((which_bound(i) == -1) && (x(i) == 0))
	  x(i) = upper_bounds(i);
    }
  return(x);
}

int check_dimensions(const RowVector c, const Matrix A, const ColumnVector b,
		     ColumnVector &vlb, ColumnVector &vub)
{
  int errors = 0;
  int num_rows = A.rows();
  int num_cols = A.cols();

  if(c.length() != num_cols)
    {
      error("lp: columns in first argument do not match rows in the second argument.");
      errors--;
    }
  if(b.length() != num_rows)
    {
      error("lp: rows in the second argument do not match the third argument.");
      errors--;
    }
  if(vlb.length() != num_cols)
    {
      if (vlb.length() == 0) 
	{
	  vlb.resize (num_cols, -lo_ieee_inf_value());
	}
      else if (vlb.length() == 1)
	{
	  vlb.resize (num_cols, vlb(0));
	}
      else
	{
	  error("lp: columns in the second argument do not match the fourth argument.");
	  errors--;
	}
    }
  if(vub.length() != num_cols)
    {
      if (vub.length() == 0)
	{
	  vub.resize (num_cols, lo_ieee_inf_value());
	}
      else if (vub.length() == 1)
	{
	  vub.resize (num_cols, vub(0));
	}
      else
	{
	  error("lp: columns in the second argument do not match the fifth argument.");
	  errors--;
	}
    }
  return(errors);
}

DEFUN_DLD(lp,args, ,
"-*- texinfo -*-\n\
@deftypefn {Loadable Function} {x =} lp(@var{f},@var{a},@var{b} [,@var{lb},@var{ub},@var{h}])\n\
Solve linear programming problems.\n\
\n\
min @var{f}*x\n\
 x  \n\
Subject to: @var{a}*x <= @var{b}\n\
\n\
@table @var\n\
@item f \n\
  @var{m} dimensional cost vector.\n\
@item a\n\
  @var{m}x@var{n} matrix representing the system.\n\
@item b\n\
  @var{n} dimensional vector.\n\
@item lb\n\
  Additional lower bound constraint vector on x.  Make\n\
  this -Inf if you want no lower bound, but you\n\
  would like an upper bound or equality constraints.\n\
@item ub\n\
  Additional upper bound contraint vector on x.  If this\n\
  is Inf that implies no upper bound constraint.\n\
@item h\n\
  An integer representing how many of the constraints are\n\
  equality constraints instead of inequality.\n\
@end table\n\
\n\
\n\
@end deftypefn" )
{
#ifdef HAVE_OCTAVE_20
  error("lp: unavailable in Octave 2.0");
  return octave_value_list();
#else  /* !HAVE_OCTAVE_20 */
  const double inf = lo_ieee_inf_value ();
  int i,j,k,l;
  octave_value_list retval;
  int nargin = args.length();
  int ne = 0; // The number of equality constraints
  int include_in_basis;
  ColumnVector x;
  Matrix T;

  // Declarations of arguement variables
  RowVector c;
  Matrix A;
  ColumnVector b,vlb,vub;
  // Start checking arguements 
  if(nargin<3 || nargin > 6)
    {
	print_usage("lp");
	return retval;
    }
  else
    {
      c = RowVector(args(0).vector_value());
      A = args(1).matrix_value();
      b = ColumnVector(args(2).vector_value());
    }
  switch(nargin)
    {
    case 6:
      if(args(5).is_real_scalar())
	{
	  ne = int(args(5).double_value());
	}
      else
	{
	  error("You must supply a scalar for the number of constraints that are equalities");
	  return retval;
	}
    case 5:
      if (!args(4).is_empty()) vub = ColumnVector(args(4).vector_value());
    case 4:
      if (!args(3).is_empty()) vlb = ColumnVector(args(3).vector_value());
    case 3:
      break;
    }
  if (error_state) return octave_value_list();

  warning("lp is unreliable: the returned solution does not always lie within the bounds");
  
  int nr = A.rows();
  int nc = A.columns();

  if(check_dimensions(c,A,b,vlb,vub) < 0){return retval;}
  ColumnVector orig_vub(vub);

  // Now take care of upper and lower bounds
  Matrix freeVars(0,2,0.0);
  int freeVarNum = 0;
  for (i=0; i<nc; i++)
    {
      if (!lo_ieee_isinf(-vlb(i)))
	{
	  // std::cout << i << " translate variable up\n";
	  // Make the {x_min < x < x_max} constraint now equal to {0 < x_new < x_max - x_min}
	  b = b-A.column(i)*vlb(i);
	  // If the upper bound is Infinity we do not change it.  
	  if(!lo_ieee_isinf(vub(i))){
	    vub(i) = vub(i)-vlb(i);
	  }
	}
      else if (!lo_ieee_isinf(vub(i)))
	{
	  // std::cout << i << " translate variable down\n";
	  // Now we have the following constraint ==> {-Inf < x < x_max}
	  // After we are done it will be {0 < x_new < Inf}, where {x_new = -x+x_max}
	  b = b-A.column(i)*vub(i);
	  T = lp_identity_matrix(A.rows());
	  T(i,i) = -1.0;
	  A = A*T;
	  vub(i) = inf;
	}
      else
	{
	  // std::cout << i << " split variable\n";
	  // both bounds are infinity so make this into two variables;
	  // Now we have the following constraint -Inf < x < Inf 
	  // After we are done we have {0 < x1_new < Inf} and 
	  // {0 < x2_new < Inf} where {x = x1_new-x2_new} 
	  A = A.append(-A.column(i));
	  c.resize(c.length()+1,-c(i));
	  vub.resize(vub.length()+1,inf);
	  freeVars.resize(freeVarNum+1,2,0.0);
	  freeVars(freeVarNum,0) = i;
	  freeVars(freeVarNum,1) = A.cols()-1;
	  freeVarNum++;
	}
    }
  
  // find a basis.  Each row of basis holds where one element of the basis is. 
  // For example if [1,2] is on row 1 of basis then row 1 , column 2 is an 
  // element in the basis.   
  Matrix basis(nr,2,-1.0);
  RowVector which_bound(nc+freeVarNum,1.0);
  int index = 0;
  int slacks = 0;
  if(ne <= nr)
    {
      A = A.append(lp_identity_matrix(nr,(nr-ne)));
      for(i=0;i<(nr-ne);i++)
	{
	  basis(i,1) = i+nc;
	  basis(i,0) = i;
	  index++;
	  slacks++;
	}
      which_bound = which_bound.append(RowVector(nr-ne,1.0));
      vub = vub.stack(ColumnVector(nr-ne,inf));
      c = c.append(RowVector(nr-ne,0));
    }
  else
    {
      error("lp: it does not make sense to have more equalities than the rank of the system");
      return(retval);
    }

  if(index < nr)
    {
      include_in_basis = FALSE;
      // Loop over all columns
      for(i = 0;i < nc;i++)
	{
	  k=0;
	  // Decide if this column can be included in a basis
	  for(j = 0;j<nr;j++)
	    {
	      if(A(j,i) == 0){k++;}else{basis(index,0) =j;}
	      if((j-k) > 1){break; /* If there are already too many non-zero entries ... move on! */ }
	    }
	  if(k == (nr-1))
	    {
	      basis(index,1) = i;
	      include_in_basis = TRUE;
	      for(l = 0;l<index;l++)
		{// Make sure this is not already in the basis
		  if(basis(l,0) == basis(index,0))
		    {// Uh, oh!  it is in the basis.
		      include_in_basis = FALSE;
		      break;
		    }
		}
	      if(include_in_basis)
		{
		  // I believe this was extraneous
		  // A = pivot(A,int(basis(index,0)),int(basis(index,1)));
		  index++;
		  which_bound(i) = 1.0;
		  // This column can be included in the basis
		  if(index == nr){break;}
		}
	      else
		{
		  basis(index,0) = -1.0;
		  basis(index,1) = -1.0;
		}
	    }
	}
    }


  idx_vector tmp_rows;
  idx_vector tmp_cols; 
  if(index == nr)
    {
      // We have a full basis
      A = A.stack(Matrix(c));
      A = A.append(Matrix(b).stack(Matrix(1,1,0.0)));
      A = multi_pivot(A,basis);
      A = minimize(A,basis,vub,which_bound); 
      // now that we have the solution remove the slack variables.
      tmp_rows = Range(1,nr);
      tmp_cols = Range(1,nc+freeVarNum);
      T = Matrix(A.index(tmp_rows,tmp_cols));
      idx_vector tmp_cols2(Range(1,nc+freeVarNum));
      which_bound = RowVector(which_bound.index(tmp_cols2));
      idx_vector tmp_cols3(Range(nc+slacks+freeVarNum+1,A.cols()));
      T = T.append(Matrix(A.index(tmp_rows,tmp_cols3)));
    }
  else
    {
      // We still need to get a basis
      // I need to introduce artificial variable and solve the new lp in order to get a basis  
      int slop_columns = nr-index;
      int slop_column_index = 0;
      Matrix mp = Matrix(slop_columns,2,0.0);
      Matrix slop(nr,slop_columns,0.0);
      slop = slop.stack(Matrix(1,slop_columns,1.0));
      // loop over all rows looking to see which are not in the basis
      for(i=0;i<nr;i++)
	{
	  include_in_basis = TRUE;
	  // compare this row with each row in the basis
	  for(j=0;j<index;j++)
	    {
	      if(basis(j,0) == i)
		{
		  include_in_basis = FALSE;
		}
	    }
	  if(include_in_basis)
	    {
	      basis(index,0) = i;
	      basis(index,1) = nc+slop_column_index+freeVarNum;
	      slop(i,slop_column_index) = 1.0;
	      slop_column_index++;
	      index++;
	    }
	}
      which_bound = which_bound.append(RowVector(slop_column_index,1.0));
      T = A.stack(Matrix(1,A.columns(),0.0));
      T = T.append(slop);
      T = T.append(Matrix(b).stack(Matrix(1,1,0.0)));
      tmp_rows = Range(nr-slop_columns+1,nr);
      tmp_cols = Range(1,2);
      T = multi_pivot(T,Matrix(basis.index(tmp_rows,tmp_cols)));
      T = minimize(T,basis,vub,which_bound);
      // Go ahead and reuse the idx_vectors
      tmp_cols = Range(1,nc+freeVarNum);
      tmp_rows = Range(1,nr);
      A = Matrix(T.index(tmp_rows,tmp_cols));
      tmp_rows = Range(1.0,1.0);
      tmp_cols = Range(1,nc+freeVarNum);
      Matrix temp = Matrix(which_bound);
      temp = Matrix(temp.index(tmp_rows,tmp_cols));
      which_bound = RowVector(temp);
      A = A.stack(Matrix(c));
      tmp_cols = Range(T.cols(),T.cols());
      T(nr,T.cols()-1) = 0; 
      tmp_rows = Range(1,nr+1);
      temp = Matrix(T.index(tmp_rows,tmp_cols));
      A = A.append(temp);
      A = multi_pivot(A,basis);
      // Finally, I solve the problem.
      T = minimize(A,basis,vub,which_bound);
      // now remove the bottom row -- the cost
      tmp_cols = Range(1,T.cols());
      tmp_rows = Range(1,T.rows()-1);
      T = Matrix(T.index(tmp_rows,tmp_cols));
    }

  x = extract_solution(T,basis,vub,which_bound);

  // --------------------------------------------------------
  idx_vector aRange;
  idx_vector bRange(Range(1,1));
  idx_vector cRange;
  for(j=0,i=0;i<nc;i++)
    {
      if(!lo_ieee_isinf(-vlb(i)))
	{
	  // Make the {x_min < x < x_max} constraint now equal to {0 < x_new < x_max - x_min}
	  x(i) = x(i)-vlb(i);
	}
      else if(!lo_ieee_isinf(orig_vub(i)))
	{
	  // Translate negative variable up;
	  x(i) = -x(i)+orig_vub(i);
	}
      else
	{
	  // both bounds are infinity so make this into two variables;
	  if(x((int)rint(freeVars(j,0))) != 0)
	    {
	      if(x((int)rint(freeVars(j,1))) != 0)
		{
		  // This should be a mathematical impossibility!
		  octave_stdout << "You have found a bug in lp.\n";
		  octave_stdout << "Something that should be mathematically impossible occured\n";
		  octave_stdout << "The answer given may or may not be correct\n";
		  octave_stdout << "Please report the problem\n";
		  return retval;
		}
	      T = Matrix(x);
	      aRange = idx_vector(Range(1,freeVars(j,1)-1));
	      if(freeVars(j,1) < T.rows())
		{
		  cRange = idx_vector(Range(freeVars(j,1)+1,T.rows()));
		  T = Matrix(T.index(aRange,bRange)).stack(Matrix(T.index(cRange,bRange)));
		}
	      else
		{
		  T = Matrix(T.index(aRange,bRange));
		}
	      x = ColumnVector(T);	
	    }
	  else if(x((int)rint(freeVars(j,1))) != 0)
	    {
	      // This means that a free variable is nagative  
	      x((int)rint(freeVars(j,0))) = -x((int)rint(freeVars(j,1)));
	      T = Matrix(x);
	      aRange = idx_vector(Range(1,freeVars(j,1)));
	      if(freeVars(j,1) < (T.rows()-1))
		{
		  cRange = idx_vector(Range(freeVars(j,1)+1,T.rows()));
		  T = Matrix(T.index(aRange,bRange)).stack(Matrix(T.index(cRange,bRange)));
		}
	      else
		{
		  T = Matrix(T.index(aRange,bRange));
		}
	      x = ColumnVector(T);	
	    }
	  else
	    {
	      // This means that both variables are zero.
	      // I simply remove the extra one and proceed as normal
	      T = Matrix(x);
	      aRange = idx_vector(Range(1,freeVars(j,1)));
	      if(freeVars(j,1) < T.rows())
		{
		  cRange = idx_vector(Range(freeVars(j,1)+1,T.rows()));
		  T = Matrix(T.index(aRange,bRange)).stack(Matrix(T.index(cRange,bRange)));
		}
	      else
		{
		  T = Matrix(T.index(aRange,bRange));
		}	
	      x = ColumnVector(T);
	    }

	  j++;
	}
    }
  // --------------------------------------------------------
  
  return octave_value(x);
#endif /* !HAVE_OCTAVE_20 */
}