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

supervis_r supervis(matrix* PlantIn, matrix* PlantOut, matrix* L)
{
	/*Variables: 
	Sizes of various matrices.
	Matrices: 
	constraints = the final constraint matrix.
	plantD = the incidence matrix of the plant.
	supervisorD = the incidence matrix of the supervisor.
	supervisorDm = the output matrix of the superivisor.
	supervisorDp = the input matrix of the supervisor.
	*/

	int m, n, k, m2;
	
	matrix plantD, supervisorD, supervisorDp, supervisorDm;
	matrix* constraints = 0;
	
	supervis_r toReturn;
	/*Initialize the matrices of toReturn to empty matrices to use it as an error 
	return value.*/
	AllocateMatrix(&toReturn.Dfm, 0, 0);
	AllocateMatrix(&toReturn.Dfp, 0, 0);	
	
	/*Check the parameters for validity.*/
	if (! CheckParams(PlantIn, PlantOut, L))
	{
		return toReturn;
	}
	
	//Get the size of the petri net input/output matrices
	m = NumberOfRows(*PlantIn);
	n = NumberOfColumns(*PlantIn);
	//Get the size of the constraint matrix.
	k = NumberOfRows(*L);
	m2 = NumberOfColumns(*L);
	/*Since we are going to be multiplying the constraints * the incidence matrix, 
	constraints must have the same number of columns as the incidence matrix does 
	rows. Pad if necesary.*/
	if (m > m2)
	{
		/*If there are more rows in the incidence matrix, we can just pad the 
		constraint matrix with extra columns of zeros. Since a new matrix is filled 
		with zeros when it is created, we can just create a new matrix of the 
		appropriate size and then block-copy the constraint matrix into it.
		Use type-2 matrices.*/
		constraints = tmalloc(sizeof(matrix));
		AllocateMatrixType(2, constraints, k, m);
		//Copy l to L(1:k, 1:m2)
		CopyBlock(k, m2, L, 0, 0, constraints, 0, 0);
	}
	else
	{
		constraints = L;
	}
	/*Now, constraints contains a constraint matrix of proper dimensions.*/
	
	/*Get the incidence matrix corresponding to the input/output matrices of the 
	plant. The result should be stored in a type-2 matrix.*/
	plantD = SubtractMatrix(PlantIn, PlantOut, (matrix*) 2);
	
	/*Now multiply the constraints matrix with the plant incidence matrix. The 
	result is a supervisor net stored as an incidence matrix in D. Store it as a type-2.*/
	supervisorD = MultiplyMatrix(constraints, &plantD, (matrix*) 2);
	
	/*The returned petri net should have input and output matrices that are the 
	top-to-bottom vertical concatenation of the input/output matrices of the 
	original plant net with the input/output matrices of the supervisor net 
	represented by the incidence matrix D.*/
	
	/*First, get the supervisor net's input and output matrices.*/
	AllocateMatrixType(2, &supervisorDm, k, n);
	AllocateMatrixType(2, &supervisorDp, k, n);
	d2dd(&supervisorD, &supervisorDm, &supervisorDp);	
	
	/*Add the rows of the supervisor to the end of the net matrices. The result 
	should be stored in a type-2 matrix.*/
	toReturn.Dfm = InsertRows(PlantOut, &supervisorDm, -1, 2);
	
	/*Concatenate the columns of the input matrices. The result should be stored 
	in a type-2 matrix.*/
	toReturn.Dfp = InsertRows(PlantIn, &supervisorDp, -1, 2);
	
	/*Free any memory that was allocated. If we allocated new space for 
	constraints, then a new matrix was allocated for it*/
	if (constraints != L)
	{
		DeallocateMatrix(constraints);
		free(constraints);
	}
	DeallocateMatrix(&plantD);
	DeallocateMatrix(&supervisorD);
	DeallocateMatrix(&supervisorDm);
	DeallocateMatrix(&supervisorDp);
	
	return toReturn;
}

int CheckParams(matrix* PlantIn, matrix* PlantOut, matrix* L)
{
	int m, n, k, m2;
	//All pointers are required.
	if (! (PlantIn && PlantOut && L))
	{
		merror(0, "SUPERVIS: A pointer to a required parameter was null");
		return 0;
	}
	//Get the size of the petri net input/output matrices
	m = NumberOfRows(*PlantIn);
	n = NumberOfColumns(*PlantIn);
	
	//The petri net matrices need to be the same size.
	if (NumberOfRows(*PlantOut) != m || NumberOfColumns(*PlantOut) != n)
	{
		merror(0, "SUPERVIS: The plant i/o matrices are not the same size");
		return 0;
	}
	//Get the size of the constraint matrix.
	k = NumberOfRows(*L);
	m2 = NumberOfColumns(*L);
	/*Since we are going to be multiplying the constraints * the incidence matrix, 
	constraints must have the same number of columns as the incidence matrix does 
	rows. We will be padding the constraint matrix if it has too few columns - 
	fail only if it has too many.*/
		
	if (m < m2)
	{
		merror(0, "Dimensions of incidence matrices and constraints do not agree");
		return 0;
	}
	return 1;
}
