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

typedef struct mroadm_d
{
	/*The matrices.*/
	matrix M, R1, R2, L, D;
	/*The number of places and transitions in the original Petri net.*/
	int places, transitions;
	/*The uncontrollable and unobservable transitions.*/
	int *Tuc, *Tuo;
	int TucCount, TuoCount;
	/*The initial marking.*/
	int* m0;
	/*B*/
	int* B;
	/*The list of constraints that need to be converted.*/
	int *conversionList;
	int conversionCount;
	/*The per-constraint status messages.*/
	char** dhow;
} mroadm_d;

static int CheckParams(matrix* L, int* B, matrix* D, int** Tuc, int* TucCount, int** Tuo, int* TuoCount, int* m0);
static mroadm_d CreateData(matrix* L, int* B, matrix* D, int* Tuc, int TucCount, int* Tuo, int TuoCount, int *m0);
static int FindConstraintsToConvert(mroadm_d* d);
static void BuildM(mroadm_d* d);
static int educ(mroadm_d* d);
static void zduo(mroadm_d* d);
static void czero(mroadm_d* d, int row, int col);
static void subtest(mroadm_d* d);
static mroadm_r BuildSolution(mroadm_d* d);
void display(char* beginning, char* middle, char* end, int i, int p);

mroadm_r mroadm(matrix* L, int* B, matrix* D, int* Tuc, int TucCount, int* Tuo, int TuoCount, int* m0)
{
	mroadm_r result;
	memset(&result, 0, sizeof(mroadm_r));
	if (! CheckParams(L, B, D, &Tuc, &TucCount, &Tuo, &TuoCount, m0)) return result;
	
	if (! NumberOfRows(*L))
	{
		result.how = HOW_OK;
		display("Empty set of constraints!", "", "", 1, 1);		
		return result;
	}
	if (! NumberOfRows(*D))
	{
		result.how = HOW_OK;
		result.ba = tcalloc(NumberOfRows(*L), sizeof(int));
		memcpy(result.ba, B, NumberOfRows(*L) * sizeof(int));
		AllocateMatrixType(2, &result.La, NumberOfRows(*L), NumberOfColumns(*L));
		CopyMatrix(L, &result.La);
		return result;
	}
	
	mroadm_d data = CreateData(L, B, D, Tuc, TucCount, Tuo, TuoCount, m0);

	/*Get the list of constraints that need to be converted and convert them.*/
	if (FindConstraintsToConvert(&data))
	{
		BuildM(&data);
		if (educ(&data))
		{
			zduo(&data);
		}
		subtest(&data);
		
		/*Reduce the constraint-related rows of M. We'll need a copy of the last part
		of a row of M, the part that was originally an identity. Allocate space for
		it now.*/
		matrix mrow;
		AllocateMatrixType(1, &mrow, 1, NumberOfColumns(data.M) - data.TucCount - data.TuoCount - 1);
		int i, j;
		for (i = 0; i < data.conversionCount; i++)
		{
			CopyBlock(1, NumberOfColumns(data.M) - data.TucCount - data.TuoCount - 1, &data.M, i + data.places, data.TucCount + data.TuoCount + 1, &mrow, 0, 0);
			int d = gcdv(&mrow, -1, -1);
			for (j = 0; j < NumberOfColumns(data.M); j++)
			{
				SetMatrixEl(&data.M, i + data.places, j, GetMatrixEl(&data.M, i + data.places, j) / d);
			}
		}
		DeallocateMatrix(&mrow);
		
		/*The constraint-related parts of M now become parts of the transformation 
		matrices.*/
		for (i = 0; i < data.conversionCount; i++)
		{
			CopyBlock(1, NumberOfColumns(data.R1), &data.M, i + data.places, data.TucCount + data.TuoCount + 1, &data.R1, data.conversionList[i], 0);
			SetMatrixEl(&data.R2, data.conversionList[i], data.conversionList[i], GetMatrixEl(&data.M, data.places + i, data.TucCount + data.TuoCount + data.places + i + 1));
		}
	}
	/*Get rid of intermediate variables.*/
	DeallocateMatrix(&data.M);
	free(data.conversionList);
	
	/*Cleanup output.*/
	if (is_verbose() >= VRB_MROADM)
	{
		printf("\n");
	}

	/*Build the solution.*/
	return BuildSolution(&data);	
}

/*******************************************************************************
CheckParams checks to make sure the parameters are valid. Returns a logical
value, nonzero for valid and zero for invalid.*/
int CheckParams(matrix* L, int* B, matrix* D, int** Tuc, int* TucCount, int** Tuo, int* TuoCount, int* m0)
{
	/*D, L, and B are all required.*/
	if (! (D && L))
	{
		merror(0, "MROADM: A required pointer was null!");
		return 0;
	}
	/*The constraint matrix must have as many columns as there are places.*/
	if (NumberOfColumns(*L) != NumberOfRows(*D))
	{
		merror(0, "MROADM: The dimensions of the constraint and incidence matrices are inconsistent");
		return 0;
	}
	/*Make sure that null pointers and counters at zero agree.*/
	if (! *Tuc) *TucCount = 0;
	else if (! *TucCount) *Tuc = 0;
	if (! *Tuo) *TuoCount = 0;
	else if (! *TuoCount) *Tuo = 0;
	return 1;
}

/*******************************************************************************
CreateData creates the initial parts of the mroadm_d data structure, which holds
information about the problem and is passed to various subroutines of the
problem. CreateData allocates space for some of the matrices and sets various
counters.*/
mroadm_d CreateData(matrix* L, int* B, matrix* D, int* Tuc, int TucCount, int* Tuo, int TuoCount, int *m0)
{
	mroadm_d data;
	memset(&data, 0, sizeof(mroadm_d));
	
	data.places = NumberOfRows(*D);
	data.transitions = NumberOfColumns(*D);
	data.TucCount = TucCount;
	data.Tuc = Tuc;
	data.TuoCount = TuoCount;
	data.Tuo = Tuo;
	data.L = *L;
	data.D = *D;
	data.B = B;
	data.m0 = m0;
	
	/*R1 will have as many rows as there are constraints and columns as there are
	places.*/
	AllocateMatrixType(2, &data.R1, NumberOfRows(*L), data.places);
	/*R2 defaults to an identity with as many rows and columns as there are 
	constraints. Setup the per-constraint status messages to default to OK while
	we're in the loop.*/
	AllocateMatrixType(2, &data.R2, NumberOfRows(*L), NumberOfRows(*L));
	data.dhow = tcalloc(NumberOfRows(*L), sizeof(char*));
	int i;
	for (i = 0; i < NumberOfRows(*L); i++)
	{
		SetMatrixEl(&data.R2, i, i, 1);
		data.dhow[i] = HOW_OK;
	}
	return data;
}

/*******************************************************************************
FindConstraintsToConvert fills in the list of which constraints need to be
converted. It returns the number of constraints that need to be converted.*/
int FindConstraintsToConvert(mroadm_d* d)
{
	/*Allocate space for the list of constraints that need to be converted.*/
	d->conversionList = tcalloc(NumberOfRows(d->L), sizeof(int));
	
	/*Iterate through constraints.*/
	int i, j, ucFlag, uoFlag, feasible;
	for (i = 0; i < NumberOfRows(d->L); i++)
	{
		/*If m0 has been provided, make sure the constraint works with it.*/
		feasible = 1;
		if (d->m0)
		{
			if (MultiplyVector(&d->L, (matrix*) d->m0, i, MULTIPLYVECTOR_ARRAY, 0) > (d->B ? d->B[i] : 0))
			{
				feasible = 0;
			}
		}		
		/*Make sure there exist no uncontrollable transitions such that the current
		constraint row times the incidence column is greater than 0.*/
		ucFlag = 1;
		for (j = 0; j < d->TucCount; j++)
		{
			if (MultiplyVector(&d->L, &d->D, i, d->Tuc[j], 0) > 0)
			{
				ucFlag = 0;
				break;
			}
		}
		/*Make sure there exist no unobservable transitions such that the current
		constraint row times the incidence column is nonzero.*/
		uoFlag = 1;
		for (j = 0; j < d->TuoCount; j++)
		{
			if (MultiplyVector(&d->L, &d->D, i, d->Tuo[j], 0))
			{
				uoFlag = 0;
				break;
			}
		}
		
		if (! feasible)
		{
			d->dhow[i] = HOW_INFEASIBLE;
			display("The transformation is infeasible due to the unitial marking","",".",i,NumberOfRows(d->L));
		}
		else if (ucFlag && uoFlag)
		{
			d->dhow[i] = HOW_OK;
			display("No transformation is necessary","",".",i,NumberOfRows(d->L));
		}
		else
		{
			/*Transformation is necessary.*/
			d->conversionList[d->conversionCount++] = i;
		}
	}
	return d->conversionCount;			
}

/*******************************************************************************
BuildM fills in the matrix M, which is used for processing by the mroadm
algorithm.*/
void BuildM(mroadm_d* d)
{
	/*Build M. In matlab:
	Lz = L(conversionList,:)
	mm = [m0; Lz * m0 - b(conversionList) - ones(conversionCount,1)]
	M = [[Duc, Duo, Lz*Duc, Lz*Duo] mm eye(places + conversionCount)]
	Allocate space.*/
	AllocateMatrixType(2, &d->M, d->places + d->conversionCount, d->TucCount + d->TuoCount + 1 + d->places + d->conversionCount);
	/*Copy in [Duc; Lz*Duc]*/
	int i, j;
	for (i = 0; i < d->TucCount; i++)
	{
		CopyBlock(d->places, 1, &d->D, 0, d->Tuc[i], &d->M, 0, i);
		for (j = 0; j < d->conversionCount; j++)
		{
			SetMatrixEl(&d->M, d->places + j, i, MultiplyVector(&d->L, &d->D, d->conversionList[j], d->Tuc[i], 0));
		}
	}
	for (i = 0; i < d->TuoCount; i++)
	{
		CopyBlock(d->places, 1, &d->D, 0, d->Tuo[i], &d->M, 0, i + d->TucCount);
		for (j = 0; j < d->conversionCount; j++)
		{
			SetMatrixEl(&d->M, d->places + j, i + d->TucCount, MultiplyVector(&d->L, &d->D, d->conversionList[j], d->Tuo[i], 0));
		}
	}
	/*Build mm*/
	if (d->m0)
	{
		for (i = 0; i < d->places; i++)
		{
			SetMatrixEl(&d->M, i, d->TucCount + d->TuoCount, d->m0[i]);
		}
		for (i = 0; i < d->conversionCount; i++)
		{
			SetMatrixEl(&d->M, d->places + i, d->TucCount + d->TuoCount,
			MultiplyVector(&d->L, (matrix*) d->m0, d->conversionList[i], 
			MULTIPLYVECTOR_ARRAY, 0) - (d->B ? d->B[d->conversionList[i]] - 1 : 0));
		}
	}
	else
	{
		for (i = 0; i < d->conversionCount; i++)
		{
			SetMatrixEl(&d->M, d->places + i, d->TucCount + d->TuoCount, -1);
		}
	}
	/*Fill in the identity.*/
	for (i = 0; i < d->places + d->conversionCount; i++)
	{
		SetMatrixEl(&d->M, i, d->TucCount + d->TuoCount + 1 + i, 1);
	}
}

/*******************************************************************************
educ eliminates positive elements from Duc.*/
int educ(mroadm_d* d)
{
	int success = 1, i, j, upperlimit;
	upperlimit = d->places < d->TucCount ? d->places : d->TucCount;
	for (i = 0; i < upperlimit; i++)
	{
		/*See if there are any elements below the main diagonal less than 0.*/
		for (j = i; j < d->places; j++)
		{
			if (GetMatrixEl(&d->M, j, i) < 0) break;
		}
		if (j < d->places)
		{
			if (j != i)
			{
				SwapRows(&d->M, &d->M, i, j);
			}
			czero(d, i, i);
		}
		else 
		{
			/*Find out if there exist any elements of M with certain characteristics.
			First, any elements in the constraint-related rows of M and in the current
			column such that they are greater than 0, and second, any elements in the
			same rows and in the mm column that are greater than or equal to 0?*/
			for (j = d->places; j < d->places + d->conversionCount; j++)
			{
				if (GetMatrixEl(&d->M, j, i) > 0) break;
				if (GetMatrixEl(&d->M, j, d->TucCount + d->TuoCount)) break;
			}
			if (j < d->places + d->conversionCount)
			{
				success = 0;
				break;
			}
		}
	}
	/*If we have not already failed, check another failure condition: Are there
	any elements in the constraint-related rows of M and the uncontrollable-
	transition columns that are greater than 0?*/
	if (success)
	{
		for (i = d->places; i < d->places + d->conversionCount; i++)
		{
			for (j = 0; j < d->TucCount; j++)
			{
				if (GetMatrixEl(&d->M, i, j) > 0) break;
			}
			if (j < d->TucCount) break;
		}
		if (i < d->places + d->conversionCount) success = 0;
	}
	return success;	
}

/*******************************************************************************
zduo performs the operations relevant to zeroing elements of Duo.*/
void zduo(mroadm_d* d)
{
	int success = 1, i, j, k, upperlimit;
	upperlimit = d->places < d->TucCount + d->TuoCount ? d->places : d->TucCount + d->TuoCount;
	for (i = 0; i < upperlimit; i++)
	{
		/*See if there are any elements below the main diagonal less than 0.*/
		for (k = i; k < d->places; k++)
		{
			if (GetMatrixEl(&d->M, k, i) < 0) break;
		}
		if (k < d->places && k != i)
		{
			SwapRows(&d->M, &d->M, i, k);
		}
		if (k < d->places) 
		{
			k = 1;
		}
		else
		{
			k = 0;
		}
		/*See if there are any elements below the main diagonal greater than 0.*/
		for (j = i; j < d->places; j++)
		{
			if (GetMatrixEl(&d->M, j, i) > 0) break;
		}
		if (j < d->places)
		{
			if (j != i)
			{
				SwapRows(&d->M, &d->M, i + k, j);
			}
			czero(d, i + k, i);
		}
		if (k)
		{
			czero(d, i, i);
		}
		/*Find out if there exist any elements of M with certain characteristics.
		First, any elements in the constraint-related rows of M and in the current
		column such that they are greater than 0, and second, any elements in the
		same rows and in the mm column that are greater than or equal to 0?
		If there are, fail.*/
		for (j = d->places; j < d->places + d->conversionCount; j++)
		{
			if (GetMatrixEl(&d->M, j, i) > 0) break;
			if (GetMatrixEl(&d->M, j, d->TucCount + d->TuoCount)) break;
		}
		if (j < d->places + d->conversionCount)
		{
			success = 0;
			break;
		}
	}
}

/*******************************************************************************
czero performs the operations related to column zeroing.*/
void czero(mroadm_d* d, int row, int col)
{
	/*Iterate through the places corresponding to the second group of rows in M
	(the ones related to constraints to be converted)*/
	int i, j, x, a, b;
	
	for (i = d->places; i < d->places + d->conversionCount; i++)
	{
		/*Get the two places of interest.*/
		/*If the places are of opposite sign, do the processing.*/
		if (GetMatrixEl(&d->M, i, col) * GetMatrixEl(&d->M, row, col) < 0)
		{
			/*Reduce the row (sort of)*/
			a = GetMatrixEl(&d->M, i, col);
			b = GetMatrixEl(&d->M, row, col);
			int c = gcdv(0, a, b);
			for (j = 0; j < NumberOfColumns(d->M); j++)
			{
				x = GetMatrixEl(&d->M, i, j) * abs(b) / c;
				x += GetMatrixEl(&d->M, row, j) * abs(a) / c;
				SetMatrixEl(&d->M, i, j, x); 
			}
		}
	}
	/*Now iterate through places from the one given as a parameter.*/
	for (i = row; i < d->places; i++)
	{
		/*Process if the places are of opposite sign.*/
		if (GetMatrixEl(&d->M, i, col) * GetMatrixEl(&d->M, row, col) < 0)
		{
			while (GetMatrixEl(&d->M, i, col))
			{
				a = GetMatrixEl(&d->M, i, col);
				b = GetMatrixEl(&d->M, row, col);				
				if (abs(b) > abs(a))
				{
					int c = (-b / a) - (! (b % a));
					for (j = 0; j < NumberOfColumns(d->M); j++)
					{
						x = GetMatrixEl(&d->M, row, j) + c * GetMatrixEl(&d->M, i, j);
						SetMatrixEl(&d->M, row, j, x);
					}
				}
				else
				{
					int c = -a / b;
					for (j = 0; j < NumberOfColumns(d->M); j++)
					{
						x = GetMatrixEl(&d->M, i, j) + c * GetMatrixEl(&d->M, row, j);
						SetMatrixEl(&d->M, i, j, x);
					}
				}
			}
		}
	}
}

/*******************************************************************************
subtest runs a series of tests on the constraint set based on the contents of M
and modifies the per-constraint status messages as appropriate.*/
void subtest(mroadm_d* d)
{
	int i, j, flag;
	/*Iterate through the places to transform.*/
	for (i = 0; i < d->conversionCount; i++)
	{
		/*According to the Matlab comments, this tests the marking.*/
		if (GetMatrixEl(&d->M, d->places + i, d->TucCount + d->TuoCount)) flag = 1;
		/*Test to see if there are any elements in constraint*Duc > 0.*/
		flag = 0;
		for (j = 0; j < d->TucCount && ! flag; j++)
		{
			if (GetMatrixEl(&d->M, d->places + i, j) > 0)
			{
				flag = 1;
			}
		}
		/*Test to see if there are any elements in constraint*Duo != 0.*/
		for (j = 0; j < d->TuoCount && ! flag; j++)
		{
			if (GetMatrixEl(&d->M, d->places + i, d->TucCount + j))
			{
				flag = 1;
			}
		}
		if (flag)
		{
			d->dhow[d->conversionList[i]] = HOW_NOTSOLVED;
		}
		else
		{
			d->dhow[d->conversionList[i]] = HOW_OK;
		}
	}		
}

/*******************************************************************************
BuildSolution builds a final solution given the data from the problem.*/
mroadm_r BuildSolution(mroadm_d* d)
{
	mroadm_r res;
	memset(&res, 0, sizeof(mroadm_r));
	
	res.dhow = d->dhow;
	/*The overall status message.*/
	int i, j, k;
	res.how = HOW_OK;
	for (i = 0; i < NumberOfRows(d->L); i++)
	{
		if (! strcmp(d->dhow[i], HOW_NOTSOLVED))
		{
			res.how = HOW_NOTSOLVED;
		}
		if (! strcmp(d->dhow[i], HOW_INFEASIBLE))
		{
			res.how = HOW_IMPOSSIBLE;
			break;
		}
	}
	/*R1 and R2.*/
	res.R1 = d->R1;
	res.R2 = d->R2;
	/*La*/
	/*Do the multiplication. This can be simplified by the fact that R2 is a
	diagonal matrix.*/
	matrix R2L;
	AllocateMatrixType(2, &R2L, NumberOfRows(d->R2), NumberOfColumns(d->L));
	for (i = 0; i < NumberOfRows(R2L); i++)
	{
		for (j = 0; j < NumberOfColumns(R2L); j++)
		{
			k = GetMatrixEl(&d->R2, i, i) * GetMatrixEl(&d->L, i, j);
			SetMatrixEl(&R2L, i, j, k);
		}
	}
	/*Do the addition.*/
	res.La = AddMatrix(&d->R1, &R2L, (matrix*) 2);
	DeallocateMatrix(&R2L);
	/*Build the new B.*/
	res.ba = tcalloc(NumberOfRows(res.La), sizeof(int));
	for (i = 0; i < NumberOfRows(res.La); i++)
	{
		res.ba[i] = (GetMatrixEl(&d->R2, i, i) * (d->B ? d->B[i] + 1 : 1)) - 1;
	}
	return res;
}

/*******************************************************************************
display does some display (if the verbosity threshold is high enough)*/
void display(char* beginning, char* middle, char* end, int i, int p)
{
	if (is_verbose() >= VRB_MROADM)
	{
		if (p <= 1)
		{
			printf("MROADM: %s%s%s\n", beginning, middle, end);
		}
		else
		{
			printf("MROADM, constraint %d: %s%s%s\n", i, beginning, middle, end);
		}
	}
}

/*******************************************************************************
Finds the absolute value of a number.*/
int abs(int a)
{
	return (a < 0) ? -a : a;
}
