#include "MemoryManager.h"
#include "spnbox.h"
#include "matrixmath.h"

/*Subroutine declarations. See subroutine implementation for descriptions.*/
static void AddPlaces(pn2acpn_r* result, matrix* Dm, int Transition, int* Qflag, int QLength, int* ipl, int ipCount, int* dpl, int dpCount);

static int GetQPlace(matrix* U, int Pair, int* Qflag, int* mask, pn2acpn_r* result);

static matrix GetU(int* cardbt, pn2acpn_r* result, int Transition, int OriginalPlaces);

static int InitParts(MemoryManager* mem, pn2acpn_r* result, int** cardbt, int** cardbtx, int** Qflag, matrix* Dm, matrix* Dp, matrix* MX, matrix* L0);

static int CheckParams(MemoryManager* mem, matrix* Dm, matrix* Dp, int** Mask, matrix* MX, matrix* L0, matrix* L, int **ipl, int *ipCount, int **dpl, int *dpCount);

static int CheckParams(MemoryManager* mem, matrix* Dm, matrix* Dp, int** Mask, matrix* MX, matrix* L0, matrix* L, int **ipl, int *ipCount, int **dpl, int *dpCount);

static void FinalizeResult(pn2acpn_r* result, matrix* L, matrix* Dm, int* ipl, int ipCount);

/*The function itself. pn2acpn transforms a Petri net to an asymmetric choice
Petri net*/
pn2acpn_r pn2acpn(matrix* Dm, matrix* Dp, int* Mask, matrix* MX, matrix* L0, matrix* L, int *ipl, int ipCount, int* dpl, int dpCount)
{
	int Transition, Transitions, Places, i, j, x;
	int *cardbt, *cardbtx, *Qflag;	
	MemoryManager mem;
	matrix U;
	pn2acpn_r result;
	
	/*Initialize the value to be returned to an error value.*/
	memset(&result, 0, sizeof(pn2acpn_r));
	
	/*Initialize the memory manager.*/
	mem = CreateMemoryManager(10, 5, 0, 0);
	
	if (! CheckParams(&mem, Dm, Dp, &Mask, MX, L0, L, &ipl, &ipCount, &dpl, &dpCount))
	{
		return result;
	}
	Places = NumberOfRows(*Dm);
	Transitions = NumberOfColumns(*Dm);
	
	/*Initialize parts of the return value and the flag matrices cardbt and 
	cardbtx. This function returns the index of the first transition which should 
	be processed.*/
	Transition = InitParts(&mem, &result, &cardbt, &cardbtx, &Qflag, Dm, Dp, MX, L0);
	
	/*Loop until there are no more transitions to be processed. This will be when
	Transition has been set to -1 by either of the functions responsible for 
	finding the next transition to be processed.*/
	while (Transition >= 0)
	{		
		cardbtx[Transition] = 0;
		/*Get U, a 2xn matrix each column of which contains a pair of place
		indices for which a particular relationship holds.*/
		U = GetU(cardbt, &result, Transition, NumberOfRows(*Dm));
				
		/*Iterate through each of the place pairs. From each pair we will develop
		the index of a single place, for which we will set the Q-flag and with
		which we will make an alteration to the cardbtx flag array.*/
		/*The Q-flag array defaults to all cleared.*/
		memset(Qflag, 0, sizeof(int) * Places);		
		for (i = 0; i < NumberOfColumns(U); i++)
		{
			/*Get the index of some particular place algorithmically.*/
			if ((x = GetQPlace(&U, i, Qflag, Mask, &result)) >= 0)
			{
				/*Set the corresponding q flag.*/
				Qflag[x] = 1;
				/*We OR cardbtx with cardbt AND the place from Dm*/
				for (j = 0; j < NumberOfColumns(*Dm); j++)
				{
					cardbtx[j] = cardbtx[j] || (cardbt[j] && GetMatrixEl(Dm, x, j));
				}				
			}
		}
		/*Free the memory allocated for the U vector now that we don't need it
		anymore.*/
		DeallocateMatrix(&U);
		/*Fill in the new places and transitions we reserved memory for above.*/
		AddPlaces(&result, Dm, Transition, Qflag, Places, ipl, ipCount, dpl, dpCount);
		/*Clear the current element of cardbtx in case it was reset some how.*/
		cardbtx[Transition] = 0;
		/*Find the next transition to be processed. This will be the first one with
		a set cardbtx flag. The next transition number defaults to -1 to indicate
		that there are no more transitions to be processed.*/
		Transition = -1;
		for (i = 0; i < Transitions; i++)
		{
			if (cardbtx[i])
			{
				Transition = i;
				break;
			}
		}		
	}
	/*Finalize the result structure; fill in the last few values.*/
	FinalizeResult(&result, L, Dm, ipl, ipCount);
	FreeMemory(&mem);
	return result;
}

/*******************************************************************************
FinalizeResult takes the result structure and fills in values that are not
computed during the main processing loop: the incidence matrix Df, the new 
marking constraint matrix Lf, and the new independent place list iplf.
*/
void FinalizeResult(pn2acpn_r* result, matrix* L, matrix* Dm, int* ipl, int ipCount)
{
	int places, oldplaces, i, place, newplaces;
	/*Fill the incidence matrix.*/
	result->Df = SubtractMatrix(&result->Dpf, &result->Dmf, (matrix*) 2);
	
	oldplaces = NumberOfRows(*Dm);
	places = NumberOfRows(result->Df);
	newplaces = places - oldplaces;
	
	/*The new L matrix should be the same as the old with columns of zeros added
	for all the new places.*/
	if (L)
	{
		AllocateMatrixType(2, &result->LF, NumberOfRows(*L), NumberOfColumns(*L) + newplaces);
		CopyBlock(NumberOfRows(*L), NumberOfColumns(*L), L, 0, 0, &result->LF, 0, 0);
	}
	
	/*The new independent place list should contain all the places that were 
	added and each of the old places.*/
	result->iplfCount = ipCount + (places - oldplaces);
	result->iplf = tcalloc(result->iplfCount, sizeof(int));
	for (i = 0; i < ipCount; i++)
	{
		result->iplf[i] = ipl[i];
	}
	place = oldplaces;
	for (; i < result->iplfCount; i++)
	{
		result->iplf[i] = place++;
	}
}

/*******************************************************************************
AddPlaces is responsible for filling in the values in the transitions and 
places that are added during each iteration of the main processing loop. It 
fills in values in the Petri net matrices, in the initial marking constraint 
matrix L0f, and in the independent-to-dependent marking transformation matrix
MXf. It requires the result vector, the original output matrix, the transition
currently being processed by the main processing loop, the vector of Q flags,
the length of the vector of Q flags, and the lists of dependent and independent
places.
*/
void AddPlaces(pn2acpn_r* result, matrix* Dm, int Transition, int* Qflag, int Qlength, int* ipl, int ipCount, int* dpl, int dpCount)
{
	int Rows, Cols, i, j, x, ToAdd = 0;
	matrix Dmf, Dpf, MX, L0;
	/*We will be adding a place and transition for each set Qflag. Count them.*/
	for (i = 0; i < Qlength; i++)
	{
		if (Qflag[i]) ToAdd++;
	}
	
	/*We will be adding new columns to MX and L0. Reseve space and copy
	existing columns.*/
	if (result->MXF.type)	
	{
		AllocateMatrixType(2, &MX, NumberOfRows(result->MXF), NumberOfColumns(result->MXF) + ToAdd);
		CopyBlock(NumberOfRows(result->MXF), NumberOfColumns(result->MXF), &result->MXF, 0, 0, &MX, 0, 0);
	}
	else
	{
		/*Zero MX.*/
		AllocateMatrix(&MX, 0, 0);
	}
	if (result->L0F.type)
	{
		AllocateMatrixType(2, &L0, NumberOfRows(result->L0F), NumberOfColumns(result->L0F) + ToAdd);
		CopyBlock(NumberOfRows(result->L0F), NumberOfColumns(result->L0F), &result->L0F, 0, 0, &L0, 0, 0);
	}
	else
	{
		AllocateMatrix(&L0, 0, 0);
	}

	/*We add rows and columns to the Petri net matrices. Store them temporarily 
	in the local Dmf and Dpf matrices. Allocate space and copy the existing 
	matrices into the new space.*/	
	Rows = NumberOfRows(result->Dmf);
	Cols = NumberOfColumns(result->Dmf);
	AllocateMatrixType(2, &Dmf, Rows + ToAdd, Cols + ToAdd);
	AllocateMatrixType(2, &Dpf, Rows + ToAdd, Cols + ToAdd);
	CopyBlock(Rows, Cols, &result->Dmf, 0, 0, &Dmf, 0, 0);
	CopyBlock(Rows, Cols, &result->Dpf, 0, 0, &Dpf, 0, 0);
	/*Iterate through the places which have been Q-flagged. At each one add a new
	place and transition. Use j to keep track of which place or transition is
	being added.*/	
	j = 0;
	for (i = 0; i < Qlength; i++)
	{
		if (! Qflag[i]) continue;
		/*Zero the output arc at the place and transition of interest*/
		SetMatrixEl(&Dmf, i, Transition, 0);		
		/*The place gets an output arc added at the new transition*/
		SetMatrixEl(&Dmf, i, Cols + j, 1);
		/*The new place gets an input arc added at the new transition*/
		SetMatrixEl(&Dpf, Rows + j, Cols + j, 1);
		/*The new place gets an output arc added to the current transition, with 
		the same weight as the original output arc at the place and transition of
		interest.*/
		SetMatrixEl(&Dmf, Rows + j, Transition, GetMatrixEl(Dm, i, Transition));
		/*Modify MX and L0 if they exist.*/
		if (NumberOfColumns(MX) || NumberOfColumns(L0))
		{
			/*Find out if the current place is independent..*/		
			for (x = 0; x < ipCount; x++)
			{
				if (ipl[x] == i)
				{				
					break;
				}
			}
			/*If it is, we want to copy to the new columns of MX and L0 the values 
			in their respective existing columns corresponding to the independent 
			place just found. (if they exist)*/
			if (x < ipCount)
			{					
				if (NumberOfColumns(MX))
				{
					CopyBlock(NumberOfRows(MX), 1, &MX, 0, x, &MX, 0, NumberOfColumns(MX) - ToAdd + j);
				}
				if (NumberOfColumns(L0))
				{
					CopyBlock(NumberOfRows(L0), 1, &L0, 0, x, &L0, 0, NumberOfColumns(L0) - ToAdd + j);
				}
			}
			/*If not, we fill the new column of MX with a -1 in the row corresponding
			to the dependent place indexed to by i. The new column of L0 remains zeroed.*/
			else
			{
				/*Get the index of the place.*/
				for (x = 0; x < dpCount; x++)
				{
					if (dpl[x] == i) break;
				}
				if (x < dpCount && NumberOfRows(MX))
				{
					SetMatrixEl(&MX, x, NumberOfColumns(MX) - ToAdd + j, -1);
				}
			}
		}
		/*Increment the added-place counter.*/
		j++;
	}
	/*Now deallocate the old matrices and replace them with the new.*/
	DeallocateMatrix(&result->Dmf);
	result->Dmf = Dmf;
	DeallocateMatrix(&result->Dpf);
	result->Dpf = Dpf;
	if (NumberOfColumns(MX))
	{
		DeallocateMatrix(&result->MXF);
		result->MXF = MX;
	}
	if (NumberOfColumns(L0))
	{
		DeallocateMatrix(&result->L0F);
		result->L0F = L0;
	}
}

/*******************************************************************************
GetQPlace examines the pair of place indices in the U matrix that is
currently being processed and selects and returns on of the indicies, to be used
to set a Q-flag and perform operations on the cardbtx flag vector.
It requires the U matrix, the pair number ot process, the vector of Q-flags,
the mask provided to pn2acpn, and the result structure for read access to Dmf
and Dpf.*/
int GetQPlace(matrix* U, int Pair, int* Qflag, int* mask, pn2acpn_r* result)
{
	int U1, U2, i, arcCount1, arcCount2;
	U1 = GetMatrixEl(U, 0, Pair);
	U2 = GetMatrixEl(U, 1, Pair);
	
	/*If neither U1 nor U2 indexes a place that has a high mask bit, there is no
	valid Q index. Return -1.*/
	if (! (mask[U1] || mask[U2]))
	{
		return -1;
	}
	/*If only one of U1 or U2 index a place with a high mask bit, that place is
	the Q index.*/
	if (mask[U1] && (! mask[U2]))
	{
		return U1;
	}
	if ((! mask[U1]) && mask[U2])
	{
		return U2;
	}
	/*Otherwise, we do some other tests to eliminate one of the numbers.*/
	/*If the qflags for both of the U indices are still 0...*/
	if (! (Qflag[U1] || Qflag[U2]))
	{
		//If we are examining any but the last pair of place indexes:
		if (Pair < NumberOfColumns(*U) - 1)
		{
			/*If the first index in the next pair is the same as the first index in 
			this pair, return that index.*/
			if (U1 == GetMatrixEl(U, 0, Pair + 1))
			{
				return U1;
			}
			else
			{
				/*Otherwise, if there exist any pairs after the current one such that one 
				of their indicies is the same as the second index of the current pair, 
				return the second index of the current pair.*/
				for (i = Pair + 1; i < NumberOfColumns(*U); i++)
				{
					if (GetMatrixEl(U, 0, i) == U2 || GetMatrixEl(U, 1, i) == U2)
					{
						return U2;
					}
				}
			}
		}
		/*If we get here, then we count the number of nonzero output arcs in the 
		places specified by U1 and U2. We return the index of the place with the 
		most nonzero output arcs.*/
		arcCount1 = 0;
		arcCount2 = 0;
		for (i = 0; i < NumberOfColumns(result->Dmf); i++)
		{
			if (GetMatrixEl(&result->Dmf, U1, i)) arcCount1++;
			if (GetMatrixEl(&result->Dmf, U2, i)) arcCount2++;
		}
		if (arcCount1 >= arcCount2)
		{
			return U1;
		}
		else
		{
			return U2;
		}
	}
	//If we get here, no valid place number was found. Return -1.
	return -1;
}

/*******************************************************************************
GetU creates and returns a 2 x n matrix, each column of which is a pair of
place indices such that the places exhibit some special relationship. The U
matrix is specific to the transition currently being investigated. Its 
calculation requires the cardbt flag vector, the result structure for access
to the Petri net matrices, the index of the transition currently being processed,
and a count of the number of places in the original Petri net.*/
matrix GetU(int* cardbt, pn2acpn_r* result, int Transition, int OriginalPlaces)
{
	int *Row0, *Row1;
	int NextCol, ColCapacity, i, j, k, flag0, flag1, Transitions, dmi, dmj;	
	int iCount, jCount;
	Transitions = NumberOfColumns(result->Dmf);
	matrix U;
	
	/*U will be a 2xn matrix, where each column contains a pair of place
	indices. The pairs are determined algorithmically by examining all places in
	the current transition that are nonzero.*/
	
	/*It is hard to predict the number of pairs that will be found. Store pairs
	in an array that will be reallocated periodically if necessary. Allocated
	in blocks the size of the number of places.*/
	Row0 = tcalloc(OriginalPlaces, sizeof(int));
	Row1 = tcalloc(OriginalPlaces, sizeof(int));
	NextCol = 0;
	ColCapacity = OriginalPlaces;
	
	/*Iterate through all the places in the transition of interest. Process only
	the first n - 1 with nonzero output arcs, where n is the number that originally
	had nonzero output arcs. Let i be the index of the place and iCount be the
	number of arcs that will have been processed if the loop condition holds
	and the loop is allowed to iterate.*/	
	i = -1;
	iCount = 0;
	while (++iCount < cardbt[Transition])
	{
		/*Find the next place for which the current transition has a nonzero output
		arc. If we exceed the matrix boundaries looking for it, fail.*/
		while (! GetMatrixEl(&result->Dmf, ++i, Transition))
		{			
			if (i == NumberOfRows(result->Dmf) - 1)
			{
				merror(0, "PN2ACPN: Internal Error");
				/*Set U equal to an error value and return it.*/
				AllocateMatrix(&U, 0, 0);
				return U;
			}
		}	
		
		/*Iterate through all the remaining places with nonzero output arcs to the
		transition of interest up to n many, for n, as before, the original number
		of nonzero output arcs. Let j be the place index and jCount n for the nth
		nonzero place.*/
		j = i;
		jCount = iCount;
		while (++jCount <= cardbt[Transition])
		{
			/*Find the next place for which the current transition has a nonzero output
			arc. If we exceed the matrix boundaries looking for it, fail.*/
			while (! GetMatrixEl(&result->Dmf, ++j, Transition))
			{			
				if (j == NumberOfRows(result->Dmf) - 1)
				{
					merror(0, "PN2ACPN: Internal Error");
					/*Set U equal to an error value and return it.*/
					AllocateMatrix(&U, 0, 0);
					return U;
				}
			}			
			/*The original Matlab algorithm set a flag according to this
			algorithm: (some variable	names changed to make it apparent in terms 
			of variables already used):
			prj = Dmf(i,:) ~= 0; % p_j\bullet 
			prk = Dmf(j,:) ~= 0; % p_k\bullet 
			flag = sum((~prj) & (prj | prk)) & sum((~prk) & (prj | prk));
			The same operation can be reduced to a pair of examinations, one
			to get the results of the operation logical of 
			sum((~prj) & (prj | prk)) and the	next to get the results of the 
			operation logical of sum((~prk) & (prj | prk))*/
			flag0 = 0;
			flag1 = 0;
			for (k = 0; k < Transitions; k++)
			{
				dmi = GetMatrixEl(&result->Dmf, i, k);
				dmj = GetMatrixEl(&result->Dmf, j, k);
				if ((! dmi) && dmj)
				{
					flag0 = 1;							
				}
				if ((! dmj) && dmi)
				{
					flag1 = 1;
				}						
			}
			/*If both flags are set, we add the current pair of places to the
			U-matrix.*/
			if (flag0 && flag1)
			{
				/*If there is space left in the temporary holding places, use it. If
				not, reallocate with more space.*/
				if (NextCol == ColCapacity)
				{
					Row0 = realloc(Row0, (ColCapacity += OriginalPlaces) * sizeof(int));
					Row1 = realloc(Row1, (ColCapacity += OriginalPlaces) * sizeof(int));
				}
				Row0[NextCol] = i;
				Row1[NextCol++] = j;						
			}
		}
	}
	/*Copy the contents of Row0 and Row1 into the U-matrix.*/
	AllocateMatrixType(2, &U, 2, NextCol);
	for (i = 0; i < NextCol; i++)
	{
		SetMatrixEl(&U, 0, i, Row0[i]);
		SetMatrixEl(&U, 1, i, Row1[i]);
	}
	free(Row0);
	free(Row1);
	
	return U;
}

/*******************************************************************************
InitParts initializes intermediate variables cardbt and cardbtx, flag arrays,
Qflag, a flag array, and the Petri net matrices in the result structure.
*/
int InitParts(MemoryManager* mem, pn2acpn_r* result, int** cardbt, int** cardbtx, int** Qflag, matrix* Dm, matrix* Dp, matrix* MX, matrix* L0)
{
	int Places, Transitions, i, j, res = -1;
	Places = NumberOfRows(*Dm);
	Transitions = NumberOfColumns(*Dp);
	
	/*The result Petri net matrices should default to the same as the passed
	matrices. The result MX and L0 should also default to the same as the passed
	MX and L0, if they were given.*/
	AllocateMatrixType(2, &result->Dmf, Places, Transitions);
	CopyMatrix(Dm, &result->Dmf);
	AllocateMatrixType(2, &result->Dpf, Places, Transitions);
	CopyMatrix(Dp, &result->Dpf);	
	if (MX)
	{
		AllocateMatrixType(2, &result->MXF, NumberOfRows(*MX), NumberOfColumns(*MX));
		CopyMatrix(MX, &result->MXF);
	}
	if (L0)
	{
		AllocateMatrixType(2, &result->L0F, NumberOfRows(*L0), NumberOfColumns(*L0));
		CopyMatrix(L0, &result->L0F);
	}
	
	/*The Q-flag array has an element for each place.*/
	*Qflag = mcalloc(mem, Places, sizeof(int));
	
	/*cardbt and cardbts are flag arrays with elements for each transition. They
	default to containing a count of the number of nonzero output arcs associated 
	with the appropriate transition.*/
	*cardbt = mcalloc(mem, Transitions, sizeof(int));
	*cardbtx = mcalloc(mem, Transitions, sizeof(int));
	/*The return value should be the index of the first transition which will be
	processed. This is the index of the first transition such that the associated
	value of cardbt is greater than one.*/
	for (i = 0; i < Transitions; i++)
	{
		for (j = 0; j < Places; j++)
		{
			if (GetMatrixEl(Dm, j, i))
			{
				(*cardbt)[i]++;
				(*cardbtx)[i]++;
			}
			if (res < 0 && (*cardbt)[i] > 1)
			{
				res = i;
			}
		}
	}	
	
	return res;
}
	
/*******************************************************************************
CheckParams examines the parameters to make sure that required parameters are
present and dimensions and array counts are consistent. It also fills in default
values where appropriate.
*/
int CheckParams(MemoryManager* mem, matrix* Dm, matrix* Dp, int** Mask, matrix* MX, matrix* L0, matrix* L, int **ipl, int *ipCount, int **dpl, int *dpCount)
{
	int Places, Transitions, i, j;
	
	/*Dm - output matrix. Required. Same size as Dp.
	Dp - input matrix. Required. Same size as Dm. */
	if (! (Dm && Dp))
	{
		merror(0, "PN2ACPN: One of the required parameters Dm or Dp was null");
		return 0;
	}
	Places = NumberOfRows(*Dm);
	Transitions = NumberOfColumns(*Dm);
	if (NumberOfRows(*Dp) != Places || NumberOfColumns(*Dm) != Transitions)
	{
		merror(0, "PN2ACPN: The sizes of the input and output matrices Dp and Dm do not agree");
		return 0;
	}
	
	/*Mask = array of int flags. Not required. Defaults to array of 1s, one for 
	each place (row of D).*/
	if (! *Mask)
	{
		(*Mask) = mcalloc(mem, Places, sizeof(int));
		for (i = 0; i < Places; i++)
		{
			(*Mask)[i] = 1;
		}
	}
	
	/*ipl, dpl: unpredictable-length int arrays, holding indices of independent 
	and dependent places. Not required. If either array pointer is null the count
	should be set to zero and vice-versa.
	Total number of ipl and dpl must be same as number of places if given. ipl 
	defaults to indexes of all places, dpl defaults to empty set.*/
	if (! *ipl) 
	{
		*ipCount = 0;
	}
	else if (! ipCount)
	{
		*ipl = 0;
	}
	if (! *dpl) 
	{
		*dpCount = 0;
	}
	else if (! (*dpCount))
	{
		*dpl = 0;
	}
	
	if (! (*ipCount))
	{
		*ipl = mcalloc(mem, Places, sizeof(int));
		for (i = 0; i < Places; i++)
		{
			(*ipl)[i] = i;
		}
		*ipCount = Places;
	}
	
	if (*ipCount + *dpCount != Places)
	{
		FreeMemory(mem);
		merror(0, "PN2ACPN: The independent and dependent place list lengths do not agree with the number of places");
		return 0;
	}

	/*L0 - matrix, initial marking constraint. Not required. If present must have
	the same number of columns as there are independent places.*/
	if (L0)
	{
		if (NumberOfColumns(*L0) != *ipCount)
		{
			FreeMemory(mem);
			merror(0, "PN2ACPN: The size of L0 and the independent place list do not agree");
			return 0;
		}
	}
	
	/*L - matrix, marking constraint. Not required. If present must have the same
	number of columns as there are independent places.*/
	if (L)
	{
		if (NumberOfColumns(*L) != *ipCount)
		{
			FreeMemory(mem);
			merror(0, "PN2ACPN: The size of L and the independent place list do not agree");
			return 0;
		}
	}
	
	/*MX - matrix, same meaning as in msplit: is the transformation matrix from 
	the independent marking to the dependent marking. Not required. If present, 
	must have the same number of rows as there are dependent places and the same
	number of columns as there are independent places (if those counts are given)*/	
	if (MX && ipCount && dpCount)
	{
		if (NumberOfRows(*MX) != *dpCount || NumberOfColumns(*MX) != *ipCount)
		{
			FreeMemory(mem);
			merror(0, "PN2ACPN: The size of the transformation matrix MX and the dependent and independent place lists do not agree");
			return 0;
		}
	}
	return 1;
}
