#include <string.h>
#include "lpkit.h"
#include "lpglob.h"
#include "debug.h"

/* Globals used by solver */
static short JustInverted;
static short Status;
static short Doiter;
static short DoInvert;
static short Break_bb;


static void ftran(lprec *lp, REAL *pcol)
{
  int  i, j, k, r, *rowp;
  REAL theta, *valuep;

  for(i = 1; i <= lp->eta_size; i++) {
    k = lp->eta_col_end[i] - 1;
    r = lp->eta_row_nr[k];
    theta = pcol[r];
    if(theta != 0) {
      j = lp->eta_col_end[i - 1];
      
      /* CPU intensive loop, let's do pointer arithmetic */
      for(rowp = lp->eta_row_nr + j, valuep = lp->eta_value + j;
	  j < k;
	  j++, rowp++, valuep++)
	pcol[*rowp] += theta * *valuep;
      
      pcol[r] *= lp->eta_value[k];
    }
  }

  /* round small values to zero */
  for(i = 0; i <= lp->rows; i++)
    my_round(pcol[i], lp->epsel);
} /* ftran */


void btran(lprec *lp, REAL *row)
{
  int  i, j, k, *rowp;
  REAL f, *valuep;

  for(i = lp->eta_size; i >= 1; i--) {
    f = 0;
    k = lp->eta_col_end[i] - 1;
    j = lp->eta_col_end[i - 1];

    for(rowp = lp->eta_row_nr + j, valuep = lp->eta_value + j;
	j <= k;
	j++, rowp++, valuep++)
      f += row[*rowp] * *valuep;
    
    my_round(f, lp->epsel);
    row[lp->eta_row_nr[k]] = f;
  }
} /* btran */


static short isvalid(lprec *lp)
{
  int i, j, *rownum, *colnum;
  int *num, row_nr;

  if(!lp->row_end_valid) {
    MALLOC(num, lp->rows + 1);
    MALLOC(rownum, lp->rows + 1);

    for(i = 0; i <= lp->rows; i++) {
      num[i] = 0;
      rownum[i] = 0;
    }

    for(i = 0; i < lp->non_zeros; i++)
      rownum[lp->mat[i].row_nr]++;
    
    lp->row_end[0] = 0;

    for(i = 1; i <= lp->rows; i++)
      lp->row_end[i] = lp->row_end[i - 1] + rownum[i];

    for(i = 1; i <= lp->columns; i++)
      for(j = lp->col_end[i - 1]; j < lp->col_end[i]; j++) {
	row_nr = lp->mat[j].row_nr;
	if(row_nr != 0) {
	  num[row_nr]++;
	  lp->col_no[lp->row_end[row_nr - 1] + num[row_nr]] = i;
	}
      }

    free(num);
    free(rownum);
    lp->row_end_valid = TRUE;
  }

  if(lp->valid)
    return(TRUE);

  CALLOC(rownum, lp->rows + 1);
  CALLOC(colnum, lp->columns + 1);

  for(i = 1 ; i <= lp->columns; i++)
    for(j = lp->col_end[i - 1]; j < lp->col_end[i]; j++) {
      colnum[i]++;
      rownum[lp->mat[j].row_nr]++;
    }

  for(i = 1; i <= lp->columns; i++)
    if(colnum[i] == 0) {
      if(lp->names_used)
        fprintf(stderr, "Warning: Variable %s not used in any constraints\n",
		lp->col_name[i]);
      else
        fprintf(stderr, "Warning: Variable %d not used in any constraints\n",
		i);
    }
  free(rownum);
  free(colnum);
  lp->valid = TRUE;
  return(TRUE);
} 

static void resize_eta(lprec *lp, int min_size)
{
  while(lp->eta_alloc <= min_size)
    lp->eta_alloc *= 1.5;
  /* fprintf(stderr, "resizing eta to size %d\n", lp->eta_alloc); */
  REALLOC(lp->eta_value, lp->eta_alloc + 1);
  REALLOC(lp->eta_row_nr, lp->eta_alloc + 1);
} /* resize_eta */

static void condensecol(lprec *lp,
			int row_nr,
			REAL *pcol)
{
  int i, elnr, min_size;
  
  elnr = lp->eta_col_end[lp->eta_size];

  min_size = elnr + lp->rows + 2;
  if(min_size >= lp->eta_alloc) /* maximum local growth of Eta */
    resize_eta(lp, min_size);

  for(i = 0; i <= lp->rows; i++)
    if(i != row_nr && pcol[i] != 0) {
      lp->eta_row_nr[elnr] = i;
      lp->eta_value[elnr] = pcol[i];
      elnr++;
    }

  lp->eta_row_nr[elnr] = row_nr;
  lp->eta_value[elnr] = pcol[row_nr];
  elnr++;
  lp->eta_col_end[lp->eta_size + 1] = elnr;
} /* condensecol */


static void addetacol(lprec *lp)
{
  int  i, j, k;
  REAL theta;
  
  j = lp->eta_col_end[lp->eta_size];
  lp->eta_size++;
  k = lp->eta_col_end[lp->eta_size] - 1;
  theta = 1 / (REAL) lp->eta_value[k];
  lp->eta_value[k] = theta;
  for(i = j; i < k; i++)
    lp->eta_value[i] *= -theta;
  JustInverted = FALSE;
} /* addetacol */


static void setpivcol(lprec *lp,
		      short lower, 
		      int   varin,
		      REAL *pcol)
{
  int  i, colnr;
  
  for(i = 0; i <= lp->rows; i++)
    pcol[i] = 0;

  if(lower) {
    if(varin > lp->rows) {
      colnr = varin - lp->rows;
      for(i = lp->col_end[colnr - 1]; i < lp->col_end[colnr]; i++)
	pcol[lp->mat[i].row_nr] = lp->mat[i].value;
      pcol[0] -= Extrad;
    }
    else
      pcol[varin] = 1;
  }
  else { /* !lower */
    if(varin > lp->rows) {
      colnr = varin - lp->rows;
      for(i = lp->col_end[colnr - 1]; i < lp->col_end[colnr]; i++)
	pcol[lp->mat[i].row_nr] = -lp->mat[i].value;
      pcol[0] += Extrad;
    }
    else
      pcol[varin] = -1;
  }

  ftran(lp, pcol);
} /* setpivcol */


static void minoriteration(lprec *lp,
			   int colnr,
			   int row_nr)
{
  int  i, j, k, wk, varin, varout, elnr;
  REAL piv = 0, theta;
  
  varin = colnr + lp->rows;
  elnr = lp->eta_col_end[lp->eta_size];
  wk = elnr;
  lp->eta_size++;

  if(Extrad != 0) {
    lp->eta_row_nr[elnr] = 0;
    lp->eta_value[elnr] = -Extrad;
    elnr++;
    if(elnr >= lp->eta_alloc)
      resize_eta(lp, elnr);
  }

  for(j = lp->col_end[colnr - 1] ; j < lp->col_end[colnr]; j++) {
    k = lp->mat[j].row_nr;

    if(k == 0 && Extrad != 0)
      lp->eta_value[lp->eta_col_end[lp->eta_size - 1]] += lp->mat[j].value;
    else if(k != row_nr) {
      lp->eta_row_nr[elnr] = k;
      lp->eta_value[elnr] = lp->mat[j].value;
      elnr++;
      if(elnr >= lp->eta_alloc)
	resize_eta(lp, elnr);
    }
    else
      piv = lp->mat[j].value;
  }

  lp->eta_row_nr[elnr] = row_nr;
  lp->eta_value[elnr] = 1 / piv;
  theta = lp->rhs[row_nr] / piv;
  lp->rhs[row_nr] = theta;

  for(i = wk; i < elnr; i++)
    lp->rhs[lp->eta_row_nr[i]] -= theta * lp->eta_value[i];

  varout = lp->bas[row_nr];
  lp->bas[row_nr] = varin;
  lp->basis[varout] = FALSE;
  lp->basis[varin] = TRUE;

  for(i = wk; i < elnr; i++)
    lp->eta_value[i] /= -piv;

  lp->eta_col_end[lp->eta_size] = elnr + 1;
} /* minoriteration */


static void rhsmincol(lprec *lp,
		      REAL theta,
		      int row_nr,
		      int varin)
{
  int  i, j, k, varout;
  REAL f;
  
  if(row_nr > lp->rows + 1) {
    fprintf(stderr, "Error: rhsmincol called with row_nr: %d, rows: %d\n",
	    row_nr, lp->rows);
    fprintf(stderr, "This indicates numerical instability\n");
    exit(EXIT_FAILURE);
  }

  j = lp->eta_col_end[lp->eta_size];
  k = lp->eta_col_end[lp->eta_size + 1];
  for(i = j; i < k; i++) {
    f = lp->rhs[lp->eta_row_nr[i]] - theta * lp->eta_value[i];
    my_round(f, lp->epsb);
    lp->rhs[lp->eta_row_nr[i]] = f;
  }

  lp->rhs[row_nr] = theta;
  varout = lp->bas[row_nr];
  lp->bas[row_nr] = varin;
  lp->basis[varout] = FALSE;
  lp->basis[varin] = TRUE;
} /* rhsmincol */


void invert(lprec *lp)
{
  int    i, j, v, wk, numit, varnr, row_nr, colnr, varin;
  REAL   theta;
  REAL   *pcol;
  short  *frow;
  short  *fcol;
  int    *rownum, *col, *row;
  int    *colnum;

  if(lp->print_at_invert) 
    fprintf(stderr, "Start Invert iter %d eta_size %d rhs[0] %g \n",
	    lp->iter, lp->eta_size, (double) - lp->rhs[0]); 
 
  CALLOC(rownum, lp->rows + 1);
  CALLOC(col, lp->rows + 1);
  CALLOC(row, lp->rows + 1);
  CALLOC(pcol, lp->rows + 1);
  CALLOC(frow, lp->rows + 1);
  CALLOC(fcol, lp->columns + 1);
  CALLOC(colnum, lp->columns + 1);
 
  for(i = 0; i <= lp->rows; i++)
    frow[i] = TRUE;

  for(i = 0; i < lp->columns; i++)
    fcol[i] = FALSE;

  for(i = 0; i < lp->rows; i++)
    rownum[i] = 0;

  for(i = 0; i <= lp->columns; i++)
    colnum[i] = 0;

  for(i = 0; i <= lp->rows; i++)
    if(lp->bas[i] > lp->rows)
      fcol[lp->bas[i] - lp->rows - 1] = TRUE;
    else
      frow[lp->bas[i]] = FALSE;

  for(i = 1; i <= lp->rows; i++)
    if(frow[i])
      for(j = lp->row_end[i - 1] + 1; j <= lp->row_end[i]; j++) {
	wk = lp->col_no[j];
	if(fcol[wk - 1]) {
	  colnum[wk]++;
	  rownum[i - 1]++;
	}
      }

  for(i = 1; i <= lp->rows; i++)
    lp->bas[i] = i;

  for(i = 1; i <= lp->rows; i++)
    lp->basis[i] = TRUE;

  for(i = 1; i <= lp->columns; i++)
    lp->basis[i + lp->rows] = FALSE;

  for(i = 0; i <= lp->rows; i++)
    lp->rhs[i] = lp->rh[i];

  for(i = 1; i <= lp->columns; i++) {
    varnr = lp->rows + i;
    if(!lp->lower[varnr]) {
      theta = lp->upbo[varnr];
      for(j = lp->col_end[i - 1]; j < lp->col_end[i]; j++)
	lp->rhs[lp->mat[j].row_nr] -= theta * lp->mat[j].value;
    }
  }

  for(i = 1; i <= lp->rows; i++)
    if(!lp->lower[i])
      lp->rhs[i] -= lp->upbo[i];

  lp->eta_size = 0;
  v = 0;
  row_nr = 0;
  lp->num_inv = 0;
  numit = 0;

  while(v < lp->rows) {
    row_nr++;
    if(row_nr > lp->rows)
      row_nr = 1;

    v++;

    if(rownum[row_nr - 1] == 1)
      if(frow[row_nr]) {
	v = 0;
	j = lp->row_end[row_nr - 1] + 1;

	while(!(fcol[lp->col_no[j] - 1]))
	  j++;

	colnr = lp->col_no[j];
	fcol[colnr - 1] = FALSE;
	colnum[colnr] = 0;

	for(j = lp->col_end[colnr - 1]; j < lp->col_end[colnr]; j++)
	  if(frow[lp->mat[j].row_nr])
	    rownum[lp->mat[j].row_nr - 1]--;

	frow[row_nr] = FALSE;
	minoriteration(lp, colnr, row_nr);
      }
  }
  v = 0;
  colnr = 0;
  while(v < lp->columns) {
    colnr++;
    if(colnr > lp->columns)
      colnr = 1;

    v++;
    
    if(colnum[colnr] == 1)
      if(fcol[colnr - 1]) {
	v = 0;
	j = lp->col_end[colnr - 1] + 1;

	while(!(frow[lp->mat[j - 1].row_nr]))
	  j++;

	row_nr = lp->mat[j - 1].row_nr;
	frow[row_nr] = FALSE;
	rownum[row_nr - 1] = 0;

	for(j = lp->row_end[row_nr - 1] + 1; j <= lp->row_end[row_nr]; j++)
	  if(fcol[lp->col_no[j] - 1])
	    colnum[lp->col_no[j]]--;

	fcol[colnr - 1] = FALSE;
	numit++;
	col[numit - 1] = colnr;
	row[numit - 1] = row_nr;
      }
  }
  for(j = 1; j <= lp->columns; j++)
    if(fcol[j - 1]) {
      fcol[j - 1] = FALSE;
      setpivcol(lp, lp->lower[lp->rows + j], j + lp->rows, pcol);
      row_nr = 1;

      while((row_nr <= lp->rows) && (!(frow[row_nr] && pcol[row_nr])))
	row_nr++;

      /* if(row_nr == lp->rows + 1) */
      if(row_nr > lp->rows) /* problems! */
	error("Inverting failed");

      frow[row_nr] = FALSE;
      condensecol(lp, row_nr, pcol);
      theta = lp->rhs[row_nr] / (REAL) pcol[row_nr];
      rhsmincol(lp, theta, row_nr, lp->rows + j);
      addetacol(lp);
    }

  for(i = numit - 1; i >= 0; i--) {
    colnr = col[i];
    row_nr = row[i];
    varin = colnr + lp->rows;

    for(j = 0; j <= lp->rows; j++)
      pcol[j] = 0;

    for(j = lp->col_end[colnr - 1]; j < lp->col_end[colnr]; j++)
      pcol[lp->mat[j].row_nr] = lp->mat[j].value;

    pcol[0] -= Extrad;
    condensecol(lp, row_nr, pcol);
    theta = lp->rhs[row_nr] / (REAL) pcol[row_nr];
    rhsmincol(lp, theta, row_nr, varin);
    addetacol(lp);
  }

  for(i = 1; i <= lp->rows; i++)
    my_round(lp->rhs[i], lp->epsb);

  if(lp->print_at_invert) 
    fprintf(stderr,
	    "End Invert                eta_size %d rhs[0] %g\n",
	    lp->eta_size, (double) - lp->rhs[0]); 

  JustInverted = TRUE;
  DoInvert = FALSE;
  free(rownum);
  free(col);
  free(row);
  free(pcol);
  free(frow);
  free(fcol);
  free(colnum);
} /* invert */

static short colprim(lprec *lp,
		     int *colnr,
		     short minit,
		     REAL   *drow)
{
  int  varnr, i, j;
  REAL f, dpiv;
  
  dpiv = -lp->epsd;
  (*colnr) = 0;
  if(!minit) {
    for(i = 1; i <= lp->sum; i++)
      drow[i] = 0;
    drow[0] = 1;
    btran(lp, drow);
    for(i = 1; i <= lp->columns; i++) {
      varnr = lp->rows + i;
      if(!lp->basis[varnr])
	if(lp->upbo[varnr] > 0) {
	  f = 0;
	  for(j = lp->col_end[i - 1]; j < lp->col_end[i]; j++)
	    f += drow[lp->mat[j].row_nr] * lp->mat[j].value;
	  drow[varnr] = f;
	}
    }
    for(i = 1; i <= lp->sum; i++)
      my_round(drow[i], lp->epsd);
  }
  for(i = 1; i <= lp->sum; i++)
    if(!lp->basis[i])
      if(lp->upbo[i] > 0) {
	if(lp->lower[i])
	  f = drow[i];
	else
	  f = -drow[i];
	if(f < dpiv) {
	  dpiv = f;
	  (*colnr) = i;
	}
      }
  if(lp->trace) {
    if((*colnr)>0)
      fprintf(stderr, "col_prim:%d, reduced cost: %g\n",
	      (*colnr), (double)dpiv);
    else
      fprintf(stderr,
	      "col_prim: no negative reduced costs found, optimality!\n");
  }
  if(*colnr == 0) {
    Doiter   = FALSE;
    DoInvert = FALSE;
    Status   = OPTIMAL;
  }
  return((*colnr) > 0);
} /* colprim */

static short rowprim(lprec *lp,
		     int colnr,
                     int *row_nr,
		     REAL *theta,
		     REAL *pcol)
{
  int  i;
  REAL f, quot; 

  (*row_nr) = 0;
  (*theta) = lp->infinite;
  for(i = 1; i <= lp->rows; i++) {
    f = pcol[i];
    if(f != 0) {
      if(my_abs(f) < Trej) {
	debug_print(lp, "pivot %g rejected, too small (limit %g)\n",
		    (double)f, (double)Trej);
      }
      else { /* pivot alright */
	quot = 2 * lp->infinite;
	if(f > 0)
	  quot = lp->rhs[i] / (REAL) f;
	else if(lp->upbo[lp->bas[i]] < lp->infinite)
	  quot = (lp->rhs[i] - lp->upbo[lp->bas[i]]) / (REAL) f;
	my_round(quot, lp->epsel);
	if(quot < (*theta)) {
	  (*theta) = quot;
	  (*row_nr) = i;
	}
      }
    }
  }
  if((*row_nr) == 0)  
    for(i = 1; i <= lp->rows; i++) {
      f = pcol[i];
      if(f != 0) {
	quot = 2 * lp->infinite;
	if(f > 0)
	  quot = lp->rhs[i] / (REAL) f;
	else
	  if(lp->upbo[lp->bas[i]] < lp->infinite)
	    quot = (lp->rhs[i] - lp->upbo[lp->bas[i]]) / (REAL) f;
	my_round(quot, lp->epsel);