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

static int CheckParams(admcon_p *params);
static int GetRowCount(admcon_p *params);
static matrix BuildZ(admcon_p *params);
static matrix BuildDX(admcon_p *p, matrix *Z);
static int *nlplace(admcon_p *p, matrix *DX, int *pCount);
static ipsolve_r RawSolution(admcon_p *param, matrix *DX, int* p, int pCount);
static admcon_r BuildResult(ipsolve_r *lpresult, int *p, int pCount, int rowCount);

admcon_r admcon(admcon_p *params)
{
	/*Initialize to an error value.*/
	admcon_r result;
	memset(&result, 0, sizeof(result));
	
	/*Check parameters.*/
	if (! CheckParams(params))
	{	
		return result;
	}
	
	/*Initialize the result to a default value.*/
	result.how = 1;
	
	/*Get the row count and the intermediate matrix Z.*/
	int rowCount = GetRowCount(params);	
	matrix Z = BuildZ(params);
	/*Get the intermediate linear programming matrix DX. Z will be deallocated
	by the function call. If DX returns an empty matrix, it means that the 
	default solution, a copy of the siphon, is feasible. Return immediately.*/
	matrix DX = BuildDX(params, &Z);
	if (! DX.type)
	{
		/*Build the result and return. l should be a copy of the siphon vector.*/
		result.l = tcalloc(NumberOfRows(params->Dm), sizeof(int));
		result.lCount = NumberOfRows(params->Dm);
		memcpy(result.l, params->siphon, sizeof(int) * NumberOfRows(params->Dm));		
		return result;
	}
	
	/*Get p, a list of places. This function is an aggregate of the nlplace function
	present in the Matlab version of this function, and several lines of code
	immediately after it. If nlplace returns a null pointer, it means that no
	feasible solution exists.*/
	int pCount;
	int *p;
	if (! (p = nlplace(params, &DX, &pCount)))
	{	
		result.how = 0;
		DeallocateMatrix(&DX);
		return result;
	}
	
	/*Otherwise, proceed to build and solve another linear programming problem
	using p. This problem deallocates DX.*/
	ipsolve_r lpresult = RawSolution(params, &DX, p, pCount);
	
	/*Get the new solution.*/	
	return BuildResult(&lpresult, p, pCount, rowCount);
}

/*******************************************************************************
This function processes the solution returned by RawSolution to build the new
solution. It also deallocates everything that still needs to be deallocated.*/
admcon_r BuildResult(ipsolve_r *lpresult, int *p, int pCount, int rowCount)
{
	int i;
	admcon_r result;	
	
	/*If the lp solution is valid, copy the appropriate values to the result.
	Otherwise leave it blank.*/
	if (! strcmp(lpresult->mhow, HOW_OK))
	{
		result.l = tcalloc(rowCount, sizeof(int));
		result.lCount = rowCount;
		for (i = 0; i < pCount; i++)
		{
			result.l[p[i]] = (int) lpresult->res[p[i]];
		}
		result.how = 1;
	}
	else
	{
		memset(&result, 0, sizeof(admcon_r));
	}
	
	/*Deallocate p and the lpresult.*/
	free(p);
	DeallocateIpsolve(lpresult);
	return result;
}

/*******************************************************************************
This function builds and solves a linear programming problem to get the modified
solution vector l. It returns the raw ipsolve results - not the processed
version that is the final solution. It also deallocates DX, which is not used
after this point.*/
ipsolve_r RawSolution(admcon_p *param, matrix *DX, int* p, int pCount)
{
	MemoryManager mem;
	mem = CreateMemoryManager(3, 1, 0, 0);
	/*The constraint matrix is an identity followed below by those columns
	of DX which have indices present in p. We will do this by adding rows directly
	to DX (it has been optimized for row ops), nulling out the columns we are not
	interested in, and putting in 1s for the identity where we are interested.*/
	InsertNullRows(DX, 0, pCount, -1);
	ManageMatrix(&mem, DX);
	
	/*Insert the 1s we are interested in. While doing it, build a flag array that
	is set at each index present in p.*/
	int *pFlag = mcalloc(&mem, NumberOfColumns(*DX), sizeof(int));
	int i, j;
	for (i = 0; i < pCount; i++)
	{
		pFlag[p[i]] = 1;
		SetMatrixEl(DX, i, p[i], 1);		
	}
	/*Now go back and zero all the columns not flagged in pFlag.*/
	for (i = 0; i < NumberOfColumns(*DX); i++)
	{
		if (! pFlag[i]) MakeZeroColumn(DX, i);
	}
	
	/*The constraint vector should be 1s for each row that was added by the
	identity and zeros for all elements thereafter except the last.*/
	double *B = mcalloc(&mem, NumberOfRows(*DX), sizeof(double));
	for (i = 0; i < pCount; i++)
	{
		B[i] = 1;
	}
	B[NumberOfRows(*DX) - 1] = 1;

	/*Cost should have, for each place indexed in p, an element that is the sum
	of all elements of Dm for that place that correspond to elements of the
	incidence matrix D that are negative.*/
	double *F = mcalloc(&mem, NumberOfColumns(*DX), sizeof(double));
	for (i = 0; i < pCount; i++)
	{
		for (j = 0; j < NumberOfColumns(param->Dm); j++)
		{
			int dm = GetMatrixEl(&param->Dm, p[i], j);
			int dp = GetMatrixEl(&param->Dp, p[i], j);
			if (dp - dm < 0) F[p[i]] += (double) dm;
		}
	}
	
	/*Solve the problem.*/
	ipsolve_r result = ipsolve(DX, B, F, 0, 0, 0, 0);
	
	/*Free memory and return the solution.*/
	FreeMemory(&mem);
	return result;
}

/*******************************************************************************
Get the list of places p that are live. Return the list length in the integer
pointed to by parameter pCount.*/
int *nlplace(admcon_p *p, matrix *DX, int *pCount)
{
	MemoryManager mem;
	mem = CreateMemoryManager(4, 1, 0, 0);
	
	/*We need to build a linear programming problem. We will iterate through each
	column, adjust the problem slightly, and resolve it.*/
	
	/*We need to know how many places are flagged in the siphon flag array.*/
	int placeCount = 0, i, j, k;
	for (i = 0; i < NumberOfRows(p->Dm); i++)
	{
		if (p->siphon[i]) placeCount++;
	}
	
	/*The constraint matrix is the columns (?) of DX for which flags are set in the
	siphon listing, followed to the right by a negative identity, then below by a
	vector which will change from iteration to iteration.*/
	matrix L;
	MAllocateMatrixType(&mem, 2, &L, NumberOfRows(*DX) + 1, placeCount + NumberOfRows(*DX));
	j = 0;
	/*Copy the appropriate columns of DX.*/
	for (i = 0; i < NumberOfRows(p->Dm); i++)
	{
		if (p->siphon[i])
		{
			CopyBlock(NumberOfRows(*DX), 1, DX, 0, i, &L, 0, j++);
		}		
	}
	/*Fill in the negative identity.*/
	for (i = 0; i < NumberOfRows(*DX); i++)
	{
		SetMatrixEl(&L, i, i + placeCount, -1);
	}
	
	/*The constraint vector should be all zeros except the last element, which
	should be 1.*/
	double *B = mcalloc(&mem, NumberOfRows(L), sizeof(double));
	B[NumberOfRows(L) - 1] = 1;
	
	/*Cost vector should be all ones.*/
	double *F = mcalloc(&mem, NumberOfColumns(L), sizeof(double));
	for (i = 0; i < NumberOfColumns(L); i++)
	{
		F[i] = 1;
	}
	
	/*Constraint type should be all equalities (0) except that of the last column,
	which should be a greater-than (1).*/
	short *ctype = mcalloc(&mem, NumberOfRows(L), sizeof(short));
	ctype[NumberOfRows(L) - 1] = 1;
	
	/*IntList should be all zeroes.*/
	short *intList = mcalloc(&mem, NumberOfColumns(L), sizeof(short));	
	
	/*During our iterations we will modify a flag array, cov, with an element for
	each of the columns that came from DX. Allocate space.*/
	int *cov = mcalloc(&mem, placeCount, sizeof(int));
	
	/*Start the iterations. Store in j the index of the last column of L such that
	its element in the last row was set to 1 within the loop. -1 means no column
	set yet.*/
	j = -1;
	for (i = 0; i < placeCount; i++)
	{
		/*Process only columns which are not flagged yet in cov.*/
		if (cov[i]) continue;
		
		/*The last row of the constraint matrix should be all zeros except for the
		element corresponding to the current column, which should be 1.*/		
		if (j >= 0) SetMatrixEl(&L, NumberOfRows(L) - 1, j, 0);
		j = i;
		SetMatrixEl(&L, NumberOfRows(L) - 1, i, 1);
		
		/*Solve the linear programming problem. Let the bounds default to 0 and inf.*/
		ipsolve_r lpresult = ipsolve(&L, B, F, intList, 0, 0, ctype);
		
		/*If the solution is valid, set any elements of cov for which there is a
		nonzero (and not very small) value in the solution vector.*/
		if (! strcmp(lpresult.mhow, HOW_OK))
		{
			for (k = 0; k < placeCount; k++)
			{
				if (lpresult.res[k] > HOW_MAX_ERROR || lpresult.res[k] < -HOW_MAX_ERROR)
				{
					cov[k] = 1;
				}
			}
		}
		/*Deallocate any memory used by the solution.*/
		DeallocateIpsolve(&lpresult);
	}
	
	/*Build the result. It is a list of the indices of elements of the siphon list
	(places) that correspond to nonzero elements in cov. Start by counting them.*/
	*pCount = 0;
	for (i = 0; i < placeCount; i++)
	{
		if (cov[i]) (*pCount)++;
	}
	/*Allocate space and do the building. Each index should be not the index
	within cov, but, for the ith element of cov, the index of the ith nonzero
	element in siphon. Use j to count indices in siphon.*/
	int *result = tcalloc(*pCount, sizeof(int));
	j = -1;
	k = 0;
	for (i = 0; i < placeCount; i++)
	{
		/*Find the ith nonzero place in siphon.*/
		j++;
		while (! p->siphon[j]) j++;
		/*If the current nonzero place is flagged in cov, copy the index.*/
		if (cov[i]) result[k++] = j;
	}
	/*Free up memory.*/
	FreeMemory(&mem);
	
	/*Make sure that there are at least two places with indices in p that are
	also present in the active place list. Count places like that in k.*/
	k = 0;
	for (i = 0; i < *pCount; i++)
	{
		for (j = 0; j < p->apCount; j++)
		{
			if (result[i] == p->apl[j])
			{
				if (++k == 2) break;
			}
		}
	}
	/*If not, no feasible solution to the admcon problem exists. Return a null
	pointer to indicate this.*/
	if (k < 2)
	{
		free(result);
		*pCount = 0;
		return 0;
	}	
	return result;
}

/*******************************************************************************
This function builds the intermediate linear programming description matrix DX.
It runs a test on DX, and if the value of l (the solution vector) is already
correct it returns an empty matrix to signify that admcon should return
immediately. This function also deallocates the Z matrix, which, is not used
again.*/
matrix BuildDX(admcon_p *p, matrix *Z)
{
	/*DX is a matrix composed of a right-to-left concatentation of submatrices, all transposed:
	DX = [Auc, Auo, -Auo, -Ds, d]'
	For:
	Auc = Z * DI(:,Tuc)
	Auo = Z * DI(:,Tuo)
	Ds  = (Dp - Dm)(:,ntr) %that is, the part of the incidence matrix corresponding
	                       %to transitions that came from transition split.
	d   = a flag array, one for each place, set to 1 for active places.	
	*/
	/*Allocate DX.*/
	matrix DX;
	AllocateMatrixType(2, &DX, p->TucCount + p->TuoCount + p->TuoCount + p->ntCount + 1, NumberOfRows(*Z));
	
	int i, j, k;
	for (i = 0; i < NumberOfColumns(DX); i++)
	{
		/*Fill in Auc.*/
		for (j = 0; j < p->TucCount; j++)
		{
			SetMatrixEl(&DX, j, i, MultiplyVector(Z, &p->DI, i, p->Tuc[j], 0));
		}
		/*Fill in Auo and -Auo.*/
		for (j = 0; j < p->TuoCount; j++)
		{
			k = MultiplyVector(Z, &p->DI, i, p->Tuo[j], 0);
			SetMatrixEl(&DX, j + p->TucCount, i, k);
			SetMatrixEl(&DX, j + p->TucCount + p->TuoCount, i, -k);
		}
		/*Fill in -Ds.*/
		for (j = 0; j < p->ntCount; j++)
		{
			k = GetMatrixEl(&p->Dm, i, p->ntr[j]) - GetMatrixEl(&p->Dp, i, p->ntr[j]);
			SetMatrixEl(&DX, j + p->TucCount + (2 * p->TuoCount), i, k);
		}
	}
	/*Fill in d.*/
	for (i = 0; i < p->apCount; i++)
	{
		SetMatrixEl(&DX, NumberOfRows(DX) - 1, p->apl[i], 1);
	}
	/*Deallocate Z. It will not be needed anymore.*/
	DeallocateMatrix(Z);
	
	/*Run a test. If DX * siphon (the default solution vector is simply the siphon)
	>= b, the default	solution vector is good. b should be a vector of all zeros 
	except for the last	element, which should be 1. We can skip the building of b 
	and simply put the test in a loop.*/
	if (p->siphon)
	{
		for (i = 0; i < NumberOfRows(DX); i++)
		{
			k = MultiplyVector(&DX, (matrix*) p->siphon, i, MULTIPLYVECTOR_ARRAY, 0);
			if (k < (i < NumberOfRows(DX) - 1 ? 0 : 1))
			{
				break;
			}
		}
		/*If we made it through the loop without breaking out early, it means that the
		default solution vector is valid. Deallocate and zero DX and return to
		indicate this. Otherwise just return DX as-is for the next stage in
		processing.*/
		if (i == NumberOfRows(DX))
		{
			DeallocateMatrix(&DX);
		}
	}
	return DX;
}

/*******************************************************************************
This function builds the matrix Z, an intermediate matrix of the admcon
algorithm.*/
matrix BuildZ(admcon_p *p)
{
	/*Build the independent place flag array. This is an array of flags, one for
	each element of the independent place list. The flag should be set of the
	place indexed by the corresponding ind. place list element is also present in
	the original place list.*/
	int *ipFlag = 0;
	int ipFlagged = 0, i, j, k;
	if (p->ipl)
	{
		ipFlag = tcalloc(p->ipCount, sizeof(int));
		for (i = 0; i < p->ipCount; i++)
		{
			for (j = 0; j < p->opCount; j++)
			{
				if (p->ipl[i] == p->opl[j])
				{
					ipFlag[i] = 1;
					ipFlagged++;
					break;
				}
			}
		}
	}
	
	/*Initialize the result to an empty matrix.*/
	matrix result;
	memset(&result, 0, sizeof(matrix));
	
	if (ipFlagged)
	{
		/*Allocate space for Z.*/
		AllocateMatrixType(2, &result, p->ipCount + p->dpCount, ipFlagged);
		j = 0;
		/*Build Z. We'll need to examine each independent place.*/
		for (i = 0; i < p->ipCount; i++)
		{			
			if (ipFlag[i]) 
			{
				/*Build the part of Z based on independent places. We are inserting an
				identity, except that some columns (those not	flagged in ipFlagged) must
				be removed premptively. Thus, on the ith independent place, count how
				many places up to that point have been flagged. Use j. Set the jth
				column of Z (at the appropriate row) to a 1.*/
				SetMatrixEl(&result, p->ipl[i], j, 1);				
				
				/*Build the part of Z based on dependent places. We are copying rows
				from M to their corresponding rows in Z (dep. place rows) except that
				the columns not flagged in ipFlag will not be copied.*/
				if (p->M.type)
				{
					for (k = 0; k < p->dpCount; k++)
					{
						SetMatrixEl(&result, p->dpl[k], j, GetMatrixEl(&p->M, k, i));
					}
				}
					
				j++;
			}
		}
	}
	/*Free the flag array.*/
	if (ipFlag) free(ipFlag);
	return result;
}

/*******************************************************************************
This function gets the row count.*/
int GetRowCount(admcon_p *params)
{
	int max = 0, i;
	for (i = 0; i < params->ipCount; i++)
	{
		if (params->ipl[i] > max) max = params->ipl[i];
	}
	for (i = 0; i < params->dpCount; i++)
	{
		if (params->dpl[i] > max) max = params->dpl[i];
	}
	for (i = 0; i < params->opCount; i++)
	{
		if (params->opl[i] > max) max = params->opl[i];
	}
	return max + 1;
}	
	
/*******************************************************************************
This function checks the parameters to make sure they are valid.*/
int CheckParams(admcon_p *params)
{
	/*If the skipParamCheck flag is set, skip the checks. Return 1 immediately.*/
	if (params->skipParamCheck) return 1;
	
	int i, j;
	if (! params)
	{
		merror(0, "ADMCON: No parameters provided");
		return 0;
	}
	/*Dm and Dp must be present.*/
	if (! (params->Dm.type && params->Dp.type))
	{
		merror(0, "ADMCON: Dm and Dp were not initialized");
		return 0;
	}
	/*Dm and Dp must be the same size.*/
	if (NumberOfRows(params->Dm) != NumberOfRows(params->Dp) || NumberOfColumns(params->Dm) != NumberOfColumns(params->Dp))
	{
		merror(0, "ADMCON: Dm and Dp are not the same size");
		return 0;
	}
	/*DI must be present.*/
	if (! params->DI.type)
	{
		merror(0, "ADMCON: DI has not been initialized");
		return 0;
	}
	/*Make sure that array pointers are null if their counts are null.*/
	if (! params->ipCount) params->ipl = 0;
	else if (! params->ipl) params->ipCount = 0;
	if (! params->dpCount) params->dpl = 0;
	else if (! params->dpl) params->dpCount = 0;
	if (! params->opCount) params->opl = 0;
	else if (! params->opl) params->opCount = 0;
	if (! params->apCount) params->apl = 0;
	else if (! params->apl) params->apCount = 0;
	if (! params->ntCount) params->ntr = 0;
	else if (! params->ntr) params->ntCount = 0;
	if (! params->TucCount) params->Tuc = 0;
	else if (! params->Tuc) params->TucCount = 0;
	if (! params->TuoCount) params->Tuo = 0;
	else if (! params->Tuo) params->TuoCount = 0;
	
	/*If the dependent/independent place lists are provided, run some tests.*/
	if (params->ipl || params->dpl)
	{
		/*Their total count	must be that of the total number of places.*/
		if (params->ipCount + params->dpCount != NumberOfRows(params->Dm))
		{
			merror(0, "ADMCON: The number of dependent and independent places does not match the total number of places");
			return 0;
		}
		/*They cannot include any of the same places or include any out-of-range 
		indices.*/
		for (i = 0; i < params->ipCount; i++)
		{
			if (params->ipl[i] < 0 || params->ipl[i] > NumberOfRows(params->Dm))
			{
				merror(0, "ADMCON: The independent place list has an out-of-range index");
				return 0;
			}
		}
		for (i = 0; i < params->dpCount; i++)
		{
			if (params->dpl[i] < 0 || params->dpl[i] > NumberOfRows(params->Dm))
			{
				merror(0, "ADMCON: The dependent place list has an out-of-range index");
				return 0;
			}
		}
		for (i = 0; i < params->ipCount; i++)
		{
			for (j = 0; j < params->dpCount; j++)
			{
				if (params->ipl[i] == params->dpl[j])
				{
					merror(0, "ADMCON: The independent and dependent place lists contain common elements");
					return 0;
				}
			}
		}
	}
	/*If the list of transitions that resulted from the transition split is present,
	make sure there are no duplicate or out-of-range elements.*/
	if (params->ntr)
	{
		for (i = 0; i < params->ntCount; i++)
		{
			if (params->ntr[i] < 0 || params->ntr[i] > NumberOfColumns(params->Dm))
			{
				merror(0, "ADMCON: The new transition list contains an out-of-range index");
				return 0;
			}
			for (j = i + 1; j < params->ntCount; j++)
			{
				if (params->ntr[i] == params->ntr[j])
				{
					merror(0, "ADMCON: The new transition list containts duplicate entries");
				}
			}
		}
	}	
	/*If M is present (nonzero) it must have the same number of columns as there
	are independent places and the same number of rows as there are independent
	places.*/
	if (params->M.type && (params->ipl && params->dpl))
	{
		if (NumberOfColumns(params->M) != params->ipCount || NumberOfRows(params->M) != params->dpCount)
		{
			merror(0, "The dependent-to-independent marking transformation matrix is not the right size");
			return 0;
		}
	}
	/*At least one of Tuc or Tuo must be provided.*/
	if (! (params->Tuc || params->Tuo))
	{
		merror(0, "ADMCON: No unobservable or uncontrollable transitions were given");
		return 0;
	}
	/*Neither Tuc nor Tuo should contain duplicate or out-of-range indices.*/
	for (i = 0; i < params->TucCount; i++)
	{
		if (params->Tuc[i] < 0 || params->Tuc[i] >= NumberOfColumns(params->DI))
		{
			merror(0, "ADMCON: Tuc contains an out-of-range index");
			return 0;
		}
	}
	for (i = 0; i < params->TuoCount; i++)
	{
		if (params->Tuo[i] < 0 || params->Tuo[i] >= NumberOfColumns(params->DI))
		{
			merror(0, "ADMCON: Tuo contains an out-of-range index");
			return 0;
		}
	}	
	return 1;
}
