#include "spnbox.h"
#include "../pnheaders/matrix.h"
#include "matrixmath.h"
#include "MemoryManager.h"

/*This structure is returned by ConvertProblem, responsible for creating copies
of the problem parameters compatible with ipslv.*/
typedef struct ipslv_problem
{
	REAL** L;
	REAL* B;
	REAL* Cost;
	REAL* HiBound;
	REAL* LoBound;
	int* IntList;
	int* ctype;
	int Constraints;
	int Variables;
} ipslv_problem;

/*Function prototypes. See the definitions for comments as to purpose.*/
static int CheckParams(MemoryManager* mem, matrix* L0, double** B0, double** Cost0, short**IntList, double** HiBound, double **LoBound, short** ctype);

static int ProcessCols(MemoryManager* mem, matrix* L, int** ColKeys, double* Cost, double* HiBound, double* LoBound, ipsolve_r* result);

static int ProcessRows(MemoryManager* mem, matrix* L0, int** RowKeys, short* ctype, double* B0, ipsolve_r* result);

static ipslv_problem ConvertProblem(MemoryManager* mem, int* RowKeys, int* ColKeys, int Rows, int Cols, matrix* L, double* B, double* Cost, double* HiBound, double* LoBound, short* IntList, short* ctype);

static void SetMessage(ipsolve_r* result, ipslv_r* lpresult, int Variables);

static int CheckSolution(matrix* L, double* B, short* ctype, ipsolve_r* result);

ipsolve_r ipsolve(matrix *L0, double* B0, double *Cost0, short int *IntList0, double *HiBound0, double *LoBound0, short int *ctype0)
{
	/*Possible arguments:
	L = a matrix.
	B = a vector.
	f = a vector, which I will call the cost vector.
	intlist = a vector of logic entries with one element for each element of x 
	(aka same number of elements as L has columns). All true elements mark where 
	an element of x must be an integer. ub, lb - vectors containing upper and 
	lower bounds inclusive on each entry in x. Same number of elements as L has 
	columns. I will call these HiBound and LoBound. ctype - a vector each element 
	of which is negative, zero, or positive. Same number of elements as L has 
	columns. I will call this ctype*/
	
	/*The structure that will be returned.*/
	ipsolve_r result;	
	
	/*Variables*/
	int i, j, k, l;
	
	/*The result returned from ipslv*/
	ipslv_r lpresult;	
	
	/*Indicators of rows and columns that have been removed or altered, plus 
	indices to which rows are not being skipped.*/
	int *RowKeys, *ColKeys;
	
	/*The number of rows and columns in the final constraint matrix L. The number 
	of columns of L corresponds to the number of elements in the solution vector.*/
	int LRows1 = 0, LCols1 = 0;
	
	/*The problem that will be passed to ipslv (suitable types and null columns
	and rows removed.*/
	ipslv_problem lp;
	
	/*Initialize the result structure to something we can return if we encounter 
	an error.*/
	result.mhow = HOW_ERROR;
	result.res = 0;
	
	/*The variable that will be used at the end to check the results.*/
	double LTimesXLessB;
	
	/*Remember what we allocate so we can free it later.*/
	MemoryManager memory;
	memory = CreateMemoryManager(20, 0, 10, 0);
	
	/*Check the parameters and initialize defaults.*/
	if (! CheckParams(&memory, L0, &B0, &Cost0, &IntList0, &HiBound0, &LoBound0, &ctype0)) return result;
	
	/*Do the column preprocessing. The value returned is the number of columns
	left after preprocessing has designated some for removal. The number will be
	negative if ProcessCols has encountered an infeasible set of boundaries and
	requests an immediate return.*/
	if ((LCols1 = ProcessCols(&memory, L0, &ColKeys, HiBound0, LoBound0, Cost0, &result)) < 0)
	{
		return result;
	}
	/*Do the row proprocessing. This function behaves much like ProcessCols.*/
	if ((LRows1 = ProcessRows(&memory, L0, &RowKeys, ctype0, B0, &result)) < 0)
	{
		return result;
	}
	
	/*If we have removed all rows or all columns, then there are no constraints 
	imposed by the constraint matrix and we can go straight ahead to checking the 
	feasible solution we developed earlier. Otherwise, proceed with solving for 
	the constraints imposed by the constraint matrix.*/
	if (LRows1 && LCols1)
	{
		lp = ConvertProblem(&memory, RowKeys, ColKeys, LRows1, LCols1, L0, B0, Cost0, HiBound0, LoBound0, IntList0, ctype0);
    
		/*Solve: Syntax is:
		ipslv(REAL* f, REAL** L, REAL* b, int m, int n, int* intlist, REAL* ub, 
		REAL* lb, int* ctype);*/		
		lpresult = ipslv(lp.Cost, lp.L, lp.B, lp.Constraints, lp.Variables, lp.IntList, lp.HiBound, lp.LoBound, lp.ctype);
		
		/*If the result type is unbounded, it means that in trying to minimize the 
		cost function the ipslv routine has returned an unbounded variable in the '
		solution vector. If this occurs, zero the cost function and get a feasible 
		solution.*/
		if (lpresult.kind == 3)
		{			
			for (i = 0; i < LCols1; i++)
			{
				lp.Cost[i] = 0;
			}
			free(lpresult.solution);		
			lpresult = ipslv(lp.Cost, lp.L, lp.B, lp.Constraints, lp.Variables, lp.IntList, lp.HiBound, lp.LoBound, lp.ctype);
		}
		
		if (lpresult.kind == 3 || lpresult.kind == 0)
		{
			/*Now we have a solution to the subset of variables that we actually 
			solved for. Go back and map this into the complete solution vector.*/
			for (i = 0, k = 0; i < NumberOfColumns(*L0); i++)
			{
				if(ColKeys[i])
        {
          /*If we get here, the column was used and there is a corresponding 
					variable in the subset used with ipslv.*/
          result.res[i] = lpresult.solution[k++];
        }
			}		
		}
		if (lpresult.solution) free(lpresult.solution);
	}
	else
	{
		/*If we get here it means that we removed all rows or all columns. Set 
		solution type to OK.*/
		lpresult.kind = 0;
	}

	/*Set the appropriate status message.*/
	SetMessage(&result, &lpresult, NumberOfColumns(*L0));
	
	/*If the solution is OK or unbounded check to make sure it is feasible.*/
	if (lpresult.kind == 0 || lpresult.kind == 3)
	{		
		if (! CheckSolution(L0, B0, ctype0, &result))
		{
			/*The returned solution was infeasible, indicating an internal error.
			Free memory before failing with an error message.*/
			FreeMemory(&memory);
			merror(0, "IPSOLVE: Internal Error");
			return result;
		}
	}
	/*Free memory and return.*/
	FreeMemory(&memory);
	return result;
}

/*******************************************************************************
CheckParams
This function checks to make sure that L has been provided and fills all the 
other optional parameters with their default values.*/
int CheckParams(MemoryManager* mem, matrix* L0, double** B0, double** Cost0, short**IntList, double** HiBound, double **LoBound, short ** ctype)
{
	int LCols, LRows, i;
	/*Check to make sure required arguments aren't null pointers.*/
	if (! L0)
	{
		merror(0, "L0 is a null pointer");
		return 0;
	}
	/*Get the number of rows and columns that will be needed for various things:*/
	LCols = NumberOfColumns(*L0);
	LRows = NumberOfRows(*L0);
	
	/*If B is empty, allocate a zero matrix of the appropriate length. */	
	if(! *B0)
  {
    *B0 = mcalloc(mem, LRows, sizeof(double));    
  }
	/*Setup the default arguments (whichever optional arguments are null pointers)
	*/
	if (! *Cost0)	
  {
    /*Cost vector is not included. Default to all zeros. There is one element in 
		the cost vector for each element in the solution vector.*/
		*Cost0 = mcalloc(mem, LCols, sizeof(double));
  }
	if (! *IntList)
  {
		/*IntList is not included. Default to all ones (all solution elements 
		required to be integers)*/
		*IntList = mcalloc(mem, LCols, sizeof(short));		
		for (i = 0; i < LCols; i++)
		{
			(*IntList)[i] = 1;
		}
	}
  if (! *HiBound)
  {
		/*Upper bounds on x are not included. Default to infinity*/
		*HiBound = mcalloc(mem, LCols, sizeof(double));		
		for (i = 0; i < LCols; i++)
		{
			(*HiBound)[i] = INF;
		}
	}
  if (! *LoBound)
  {
		/*Lower bounds on x are not included. Default to zeros. There are as many 
		lower bounds as there are values of x.*/
		*LoBound = mcalloc(mem, LCols, sizeof(double));		
  }
	if (! *ctype)
  {  
		/*ctype is not included. Defaults to all 1s(all constraints >=). 
		There are as many constraint type entries as there are rows of L, since each 
		row of L generates one row in the product that is constrained.*/
		*ctype = mcalloc(mem, LRows, sizeof(short));		
		for (i = 0; i < LRows; i++)
		{
			(*ctype)[i] = 1;
		}
	}	
	return 1;
}

/*******************************************************************************
ProcessCols
This function fills ColKeys with flags indicating which columns are null and 
can be skipped, does preprocessing on IntList, and fills result with a feasible
solution. It returns the number of columns left after eliminating null columns
or -1 in case a condition is encountered requiring an immmediate return.*/
int ProcessCols(MemoryManager* mem, matrix* L, int** ColKeys, double* HiBound, double* LoBound, double* Cost, ipsolve_r* result)
{
  int i, j, NewCols = 0, Rows, Cols;
	int *HiBoundF, *LoBoundF;
	
	Rows = NumberOfRows(*L);
	Cols = NumberOfColumns(*L);
	/*This is the column scan loop. We scan to see which columns are non-null and 
	will be copied, build the feasible solution, and do a few other things that 
	need to be done by column.*/
	//Allocate space.	
	*ColKeys = mcalloc(mem, Cols, sizeof(int));	
	HiBoundF = tcalloc(Cols, sizeof(int));
	LoBoundF = tcalloc(Cols, sizeof(int));
	result->res = tcalloc(Cols, sizeof(double));
	for (i = 0; i < Cols; i++)
	{		
		/*Fill the finite bounds indicators with the appropriate value. We reserve a 
		value of HiBound for positive infinity and a value of LoBound for negative 
		infinity.*/
		if(HiBound[i] != INF)
		{
			HiBoundF[i] = 1;
		}
		if(LoBound[i] != -INF)
		{
			LoBoundF[i] = 1;
		}
		/*Test to make sure the column contains at least one nonzero entry.*/
    for (j = 0; j < Rows; j++)
    {
      if (GetMatrixEl(L, j, i))
      {
        (*ColKeys)[i] = 1;
        NewCols++;
        break;
      }
    }
		/*If the column will be copied, we need to set up a feasible solution:*/
		if((*ColKeys)[i])
		{
			/*If any upper bound is lower than a lower bound, no feasible solution 
			exists.*/
			if (HiBound[i] < LoBound[i] && HiBoundF[i])
			{
				result->mhow = HOW_INFEASIBLE;
				/*Free allocated memory.*/
				FreeMemory(mem);
				free(result->res);
				free(HiBoundF);
				free(LoBoundF);
				result->res = 0;
				return -1;
			}
			/*If both the high and low bounds are negative or zero, the feasible 
			solution should be the high bound.*/
			else if (HiBound[i] <= 0 && (LoBound[i] < 0 || (! LoBoundF[i])))
			{
				result->res[i] = HiBound[i];
			}
			/*If the high bound is above zero and the low bound is below zero, the 
			feasible solution will be a zero.*/
			else if((HiBound[i] >= 0 || (! HiBoundF[i])) && (LoBound[i] < 0 || (! LoBoundF[i])))
			{
				result->res[i] = 0;
			}
			else
			{
				/*Both upper and lower bounds are greater than 0 and the feasible 
				solution can be set to the lower bound.*/
				result->res[i] = LoBound[i];
			}
		}
		/*If the column is going to be skipped because it is all zeros, then the 
		corresponding variable does not contribute to the constraint equation, and
		the cost vector and the bounds are the only constraints on the variable. It 
		can be set by them.*/
		else
		{
			if(Cost[i] < 0)
			{
				result->res[i] = HiBound[i];
			}
			else if (Cost[i] > 0)
			{
				result->res[i] = LoBound[i];
			}
		}
	}
	free(HiBoundF);
	free(LoBoundF);	
	return NewCols;
}

/*******************************************************************************
ProcessRows
This function marks null rows for removal and verifies that null rows do not
result in unmeetable constraints.
*/
int ProcessRows(MemoryManager* mem, matrix* L0, int** RowKeys, short* ctype, double* B0, ipsolve_r* result)
{
	int Rows, Cols, i, j, NewRows = 0;
	double cmultiplier;
	Rows = NumberOfRows(*L0);
	Cols = NumberOfColumns(*L0);
	/*Initialize the row and column keys. These will be filled with values by the 
	first preprocessing loop to tell the second what to do with specific rows and 
	columns. They default to zero, meaning remove row or column.*/
	*RowKeys = mcalloc(mem, Rows, sizeof(int));

  /*This is the row scan loop. We use it to scan which rows will be copied and 
	do some more constraint checking.*/	  
	for (i = 0; i < Rows; i++)
	{
	/*Check to make sure this row contains at least one nonzero entry.*/
    for (j = 0; j < Cols; j++)
    {
      if(GetMatrixEl(L0, i, j))
      {
        NewRows++;
        (*RowKeys)[i] = 1;
        break;
      }
    }
    if (! (*RowKeys)[i])
		{
			/*If the row is not being copied, it means that this row is empty. If a 
			row is empty it cannot add up to anything but zero. Make sure the 
			constraints allow this. We must have not have a constraint for >= 
			positive, <= negative, or = nonzero*/
			cmultiplier = (ctype[i] > 0) ? 1.0 : ((ctype[i] < 0.0) ? -1.0 : 0.0);
			
			if (cmultiplier * B0[i] > 0 || ((! ctype[i]) && B0[i]))
			{
				/*Constraints cannot be met. Notify the calling function and return.*/
				result->mhow = HOW_INFEASIBLE;
				/*Free allocated memory.*/
				FreeMemory(mem);
				free(result->res);
				result->res = 0;
				return -1;
			}			
		}
	}
	return NewRows;
}

/*******************************************************************************
ConvertProblem
This function takes the parameters passed to ipsolve, the lists of rows and 
columns to remove, builds the parameters of the trimmed-down problem, converting
them to types suitable for passing to ipslv.*/
ipslv_problem ConvertProblem(MemoryManager* mem, int* RowKeys, int* ColKeys, int Rows, int Cols, matrix* L, double* B, double* Cost, double* HiBound, double* LoBound, short* IntList, short* ctype)
{
	int i, j, k, l;
	ipslv_problem result;
	result.Constraints = Rows;
	result.Variables = Cols;
	
	/*Allocate the storage for the various copied parameters.*/
  result.B = mcalloc(mem, Rows, sizeof(REAL));
  result.ctype = mcalloc(mem, Rows, sizeof(int));		
  result.HiBound = mcalloc(mem, Cols, sizeof(REAL));
  result.LoBound = mcalloc(mem, Cols, sizeof(REAL));				
  result.Cost = mcalloc(mem, Cols, sizeof(REAL));
  result.IntList = mcalloc(mem, Cols, sizeof(int));	

  /*Column copy loop. Copy all the column-related parameters (those related to 
	specific solution variables). Use k to be the index in the destination 
	arrays.*/
  k = 0;
  for (i = 0; i < NumberOfColumns(*L); i++)
  {
    if(ColKeys[i])
		{
			result.Cost[k] = Cost[i];
			result.HiBound[k] = HiBound[i];
			result.LoBound[k] = LoBound[i];			
			result.IntList[k++] = IntList[i] ? 1 : 0;
		}
  }
  
  /*Row copy loop. Copy all of the row-related parameters (those related to the 
	constraint equations). Use k to be the index in the destination arrays.*/
  k = 0;
  for (i = 0; i < NumberOfRows(*L); i++)
  {
    if (RowKeys[i])
		{
			result.B[k] = B[i];			
			if (ctype[i] < 0)
			{
				result.ctype[k] = -1;
			}
			else if (ctype[i] > 0)
			{
				result.ctype[k] = 1;
			}
			k++;
		}
  }

	/*Allocate the storage for the matrix copy.*/
	result.L = mcalloc(mem, Rows, sizeof(REAL*));
	
	/*The constraint matrix copy loop. Use i and j to represent indices within 
	the source loop and k and l to represent indices in the destination loop.*/
	k = 0;    
	for (i = 0; i < NumberOfRows(*L); i++)
	{	
		if (RowKeys[i])
		{
			result.L[k] = mcalloc(mem, Cols, sizeof(REAL));
			l = 0;
			for (j = 0; j < NumberOfColumns(*L); j++)
			{
				if (ColKeys[j])
				{
					result.L[k][l++] = GetMatrixEl(L, i, j);
				}
			}
			k++;
		}
	}
	return result;
}

/*******************************************************************************
SetMessage
This function examines the solution returned by ipsolve and sets the appropriate
message in the result structure.*/
void SetMessage(ipsolve_r* result, ipslv_r* lpresult, int Variables)
{	
	int i;
	/*Set the result message.*/
	switch(lpresult->kind)
	{
	case 0:
		result->mhow = HOW_OK;
		break;
	case 2:
		result->mhow = HOW_INFEASIBLE;
		break;
	case 3:
		result->mhow = HOW_UNBOUNDED;
		break;
	default:
		result->mhow = HOW_NOTSOLVED;
		break;
	}
	
	/*If there are any infinities in the solution, set the result message to 
	unbounded.*/
	for (i = 0; i < Variables; i++)
	{
		if(result->res[i] == INF)
		{
			result->mhow = HOW_UNBOUNDED;
			break;
		}
	}
}

/*******************************************************************************
CheckSolution
This function examines a proposed solution to make sure that it actually meets
the constraints. It returns 1 if the solution meets the constraints and 0
otherwise.*/
int CheckSolution(matrix* L, double* B, short* ctype, ipsolve_r* result)
{	
	int i, j;
	double LTimesXLessB;
	/*Iterate through each constraint. Make sure that the proposed solution
	actually meets the constraint.*/
	for (i = 0; i < NumberOfRows(*L); i++)
	{
		LTimesXLessB = -B[i];
		for (j = 0; j < NumberOfColumns(*L); j++)
		{
			LTimesXLessB += ((double) GetMatrixEl(L, i, j)) * result->res[j];
		}			
		if (ctype[i] < 0)
		{
			if (LTimesXLessB - HOW_MAX_ERROR > 0)
			{
				return 0;
			}
		}
		else if (ctype[i] == 0)
		{
			if (LTimesXLessB - HOW_MAX_ERROR > 0 || LTimesXLessB + HOW_MAX_ERROR < 0)
			{
				return 0;
			}
		}
		else
		{
			if (LTimesXLessB + HOW_MAX_ERROR < 0)
			{
				return 0;
			}
		}
	}		
	return 1;
}

