#include <stdlib.h>
#include "../pnheaders/matrix.h"
#include "../pnheaders/pns.h"
#include "matrixmath.h"
#include "spnbox.h"
#include "MemoryManager.h"

static int CheckParams(matrix* L0, int* B0, matrix* l0);
static void BuildProblem(matrix* L0, int* B0, matrix* l0, int b0, matrix** L1, double** B1);

/*This is an implementation of chk_con2.m. chkcons.m, a more efficient but
more complicated version of the same function, will be implemented later.*/
chkcons_r chkcons(matrix* L0, int* B0, matrix* l0, int b0)
{
	chkcons_r result;
	ipsolve_r IPSolution;
	
	matrix *L1;
	double *B1;
	
	memset(&result, 0, sizeof(chkcons_r));
	
	if (! CheckParams(L0, B0, l0)) return result;
	
	/*Build L1 and B1, the constraint matrices and vector of the linear
	programming problem.*/
	BuildProblem(L0, B0, l0, b0, &L1, &B1);
	
	/*If verbosity is high enough print a period to indicate that an ipsolve
	call is beginning.*/
	if (is_verbose() >= VRB_CHKCONS)
	{
		printf(".\n");
	}
	/*Solve the linear programming problem solution.*/
	IPSolution = ipsolve(L1, B1, 0, 0, 0, 0, 0);
	
	/*If the problem was feasible copy it to the return vector*/
	if (! strcmp(IPSolution.mhow, HOW_OK))
	{
		result.resCount = NumberOfColumns(*L0);
		result.res = IPSolution.res;
	}
	/*Otherwise free the memory used by the solution and leave the return value
	empty.*/
	else
	{
		if (IPSolution.res) free(IPSolution.res);
	}
	
	/*Free intermediate memory and return.*/
	free(B1);
	if (L1 != L0)
	{
		DeallocateMatrix(L1);
		free(L1);
	}
	return result;
}

/*******************************************************************************
This function fills L1 with L0 if present concatenated rowwise with l0 if
present, and B1 with B0 if present followed by b0 if l0 is present*/
void BuildProblem(matrix* L0, int* B0, matrix* l0, int b0, matrix** L1, double** B1)
{
	int Vars, Cons, i;
	/*Figure out how many variables and constraint rows there will be overall.*/
	if (L0)
	{
		Vars = NumberOfColumns(*L0);
		Cons = NumberOfRows(*L0);
		if (l0) Cons++;
	}
	else
	{
		Vars = NumberOfColumns(*l0);
		Cons = 1;
	}
	/*Allocate space.*/
	/*If only L0 is present, optimize by never actually doing the copy. Merely
	set L1 to point to L0.*/
	if (L0 && ! l0)
	{
		*L1 = L0;
	}
	else
	{
		*L1 = tmalloc(sizeof(matrix));
		AllocateMatrixType(2, *L1, Cons, Vars);
	}
	(*B1) = tcalloc(Cons, sizeof(double));	
		
	/*If L0 is present, copy it into place and copy the appropriate elements of
	the constraint vector.*/
	if (L0)
	{
		CopyBlock(NumberOfRows(*L0), NumberOfColumns(*L0), L0, 0, 0, *L1, 0, 0);
		for (i = 0; i < NumberOfRows(*L0); i++)
		{
			(*B1)[i] = (double) B0[i];
		}		
	}
	/*If l0 is present, copy its negated version into the appropriate elements
	of the last row of the constraint matrix and fill the last element of the
	constraint vector with the appropriate value.*/
	if (l0)
	{
		for (i = 0; i < Vars; i++)
		{
			SetMatrixEl(*L1, Cons - 1, i, -GetMatrixEl(l0, 0, i));
		}
		(*B1)[Cons - 1] = (double) (1 - b0);
	}
}
/*******************************************************************************
CheckParams makes sure that required parameters are present and does some basic
validity checking.*/
int CheckParams(matrix* L0, int* B0, matrix* l0)
{
	/*One of L0 or l0 is required.*/
	if (! (L0 || l0))
	{
		merror(0, "CHK_CONS: Neither L nor l was given");
		return 0;
	}
	/*If L0 is present then B0 is required.*/
	if (L0 && ! B0)
	{
		merror(0, "CHK_CONS: L was given but B was not");
		return 0;
	}
	/*If l0 is present it must be a row vector.*/
	if (l0)
	{
		if (NumberOfRows(*l0) != 1)
		{
			merror(0, "CHK_CONS: l is not a row vector");
			return 0;
		}
		/*If L0 is also present the two must have the same number of columns.*/
		if (L0)
		{
			if (NumberOfColumns(*L0) != NumberOfColumns(*l0))
			{
				merror(0, "CHK_CONS: The number of columns in L is not the same as in l");
				return 0;
			}
		}
	}
	return 1;	
}
