/****************************************************************************
matrixmath.c
Written in 2009 by Stephen Camp
Updated 19 May 2009
Updated 13 July 2009
****************************************************************************/

#include "../pnheaders/general.h"
#include "../pnheaders/matrix.h"
#include "matrixmath.h"

/*This function is an internal function used by many of the matrixmath functions.
It examines the dst pointer to determine if it points to a valid destination
or if it requests a destination be allocated. 
If the former, the function checks to make sure the destination is of the
correct size. If the latter, it does the allocation..*/
static int CheckDestination(matrix** dst, int dstRows, int dstCols, matrix* local)
{
	/*If *dst > 3 it is a valid pointer. Check to make sure the destination has
	the requested size.*/
	if ((long) *dst > 3)
	{
		if (NumberOfRows(**dst) != dstRows || NumberOfColumns(**dst) != dstCols)
		{
			return 0;
		}
		else
		{
			/*Return 1. This will indicate success with no allocation.*/
			return 1;
		}
	}
	/*If a specific type of allocation is is requested...*/
	else if (*dst)
	{
		AllocateMatrixType((long) *dst, local, dstRows, dstCols);
		*dst = local; // MVI: long cast above to avoid gcc warning
	}
	/*Otherwise, use the default allocated type.*/
	else	
	{
		AllocateMatrix(local, dstRows, dstCols);
		*dst = local;
	}
	/*Return -1. This will indicate success with allocation.*/
	return -1;
}

/*This function transposes the matrix passed in Matrix. Currently, this 
operation is very fast as it consists only of changing an internal flag in the 
matrix structure.*/
void TransposeMatrix(matrix* m)
{
	m->trans = ! m->trans;
}

/* This function adds two matrices together element-by-element and stores the 
result in the matrix pointed to by dst.*/
matrix AddMatrix(const matrix*  addA, const matrix*  addB, matrix* dst)
{
	int i, j, rows, cols;
	/*This will hold an empty matrix for error return or be allocated to be the 
	destination matrix.*/
	matrix local;
	//Zero the destination matrix to use as an error return value.
	AllocateMatrix(&local, 0, 0);
	
	//Check to make sure none of the parameters are null.
	if(! (addA && addB))
	{
		merror(0, "AddMatrix: One of the parameters is a null pointer");
		return local;
	}
	//Check to make sure the matrices are of the same dimension.
	rows = NumberOfRows(*addA);
	cols = NumberOfColumns(*addA);
	
	if (rows != NumberOfRows(*addB) || cols != NumberOfColumns(*addB))
	{
		merror(0, "AddMatrix: The operands do not have the same dimensions");
		return local;
	}
	
	//Arrange the destination matrix.
	if (! CheckDestination(&dst, rows, cols, &local))
	{
		merror(0, "AddMatrix: The destination matrix is the wrong size");
		return local;
	}
	
	//Add the matrices
	for (i = 0; i < rows; i++)	
	{
		for (j = 0; j < cols; j++)
		{
			SetMatrixEl(dst, i, j, GetMatrixEl(addA, i, j) + GetMatrixEl(addB, i, j));
		}
	}
	return *dst;
}		

/* SubtractMatrix does an element-by-element subtraction. The elements of the 
matrix pointed to by sub are subtracted from the elements of the matrix pointed 
to by add. The results are stored in the matrix pointed to by dst.*/
matrix SubtractMatrix(const matrix*  add, const matrix*  sub, matrix* dst)
{
	int i, j, rows, cols;
	/*This will hold an empty matrix for error return or be allocated to be the 
	destination matrix.*/
	matrix local;

	//Zero the destination matrix to use as an error return value.
	AllocateMatrix(&local, 0, 0);

	if(! (add && sub))
	{
		merror(0, "SubtractMatrix: One of the operands is a null pointer");
		return local;
	}
	//Check to make sure the matrices are of the same dimension.
	rows = NumberOfRows(*add);
	cols = NumberOfColumns(*add);
	
	if (rows != NumberOfRows(*sub) || cols != NumberOfColumns(*sub))
	{
		merror(0, "SubtractMatrix: The operands do not have the same dimensions");
		return local;
	}
	//Arrange the destination matrix.
	if (! CheckDestination(&dst, rows, cols, &local))
	{
		merror(0, "SubtractMatrix: The destination matrix has the wrong dimensions");
		return local;
	}
	//Subtract the matrices
	for (i = 0; i < rows; i++)	
	{	
		for (j = 0; j < cols; j++)
		{
			SetMatrixEl(dst, i, j, GetMatrixEl(add, i, j) - GetMatrixEl(sub, i, j));
		}
	}
	return *dst;
}

/*MultiplyMatrix does a matrix multiplication of the matrices pointed to by 
left and right and stores the results in the matrix pointed to by dst.*/
matrix MultiplyMatrix(const matrix*  left, const matrix* right, matrix* dst)
{
	int i, j, k, rows, cols, inner, currentsum;
	/*This will hold an empty matrix for error return or be allocated to be the 
	destination matrix.*/
	matrix local;
	
	//Zero the destination matrix to use as an error return value.
	AllocateMatrix(&local, 0, 0);
	
	//Check to make sure none of the operands are null.
	if(! (left && right))
	{
		merror(0, "MultiplyMatrix: One of the operands is a null pointer");
		return;
	}
	
	/*Get the number of rows and columns that will be in the result and the number 
	of columns/rows that are being added element-by-element (the inner dimensions 
	of the product)*/
	rows = NumberOfRows(*left);
	cols = NumberOfColumns(*right);
	inner = NumberOfColumns(*left);
	
	/*Make sure the inner dimensions (columns of left matrix vx rows of right 
	matrix) match*/
	if (inner != NumberOfRows(*right))
	{
		merror(0, "MultiplyMatrix: The matrices do not have matching inner dimensions");
		return local;
	}

	//Arrange the destination matrix.
	if (! CheckDestination(&dst, rows, cols, &local))
	{
		merror(0, "MultiplyMatrix: The destination matrix has the wrong dimensions");
		return local;
	}
	
	/*Now, take the matrix product. Each element of the destination is the sum of
	corresponding elements of *left by row and *right by column.*/
	for (i = 0; i < rows; i++)
	{
		for (j = 0; j < cols; j++)
		{
			currentsum = GetMatrixEl(left, i, 0) * GetMatrixEl(right, 0, j);			
			for (k = 1; k < inner; k++)
			{
				currentsum += (GetMatrixEl(left, i, k) * GetMatrixEl(right, k, j));			
			}			
			SetMatrixEl(dst, i, j, currentsum);
		}
	}
	return *dst;
}

void ShowMatrix(const matrix* m, const char* title)
{
	int i, j;
	if (! m) 
	{
		printf ("Matrix %s (null pointer).\n", title);
		return;
	}
	printf("Matrix %s (%d x %d of type %d):\n", title, NumberOfRows(*m), NumberOfColumns(*m), m->type);
	for (i = 0; i < NumberOfRows(*m); i++)
	{			
		for (j = 0; j < NumberOfColumns(*m); j++)
		{				
			printf("%d\t", GetMatrixEl(m, i, j));
		}
		printf("\n");
	}	
}

/*This function multiplies a particular row of the left matrix by a particular 
column of the right matrix. If the row or column index is a negative constaint, 
the pointer to the corresponding matrix is interpreted as a pointer to an array 
of integers. If both primary parameters are arrays of integers, then Length 
holds the length of the arrays. Otherwise it is ignored.*/
int MultiplyVector(matrix* left, matrix* right, int Row, int Col, int Length)
{
	int i, current = 0;
	if (Row == MULTIPLYVECTOR_ARRAY)
	{
		if (Col == MULTIPLYVECTOR_ARRAY)
		{
			for (i = 0; i < Length; i++)
			{
				current += (*(((int*) right) + i)) * (*(((int*) left) + i));
			}
			return current;
		}
		else
		{
			Length = NumberOfRows(*right);
			for (i = 0; i < Length; i++)
			{
				current += (*(((int*) left) + i)) * GetMatrixEl(right, i, Col);
			}
			return current;
		}
	}
	else
	{
		if (Col == MULTIPLYVECTOR_ARRAY)
		{
			Length = NumberOfColumns(*left);
			for (i = 0; i < Length; i++)
			{
				current += (*(((int*) right) + i)) * GetMatrixEl(left, Row, i);
			}
			return current;
		}
		else
		{
			Length = NumberOfColumns(*left);
			if (Length != NumberOfRows(*right))
			{
				merror(0, "MultiplyVector: The dimensions of the vectors do not agree");
				return 0;
			}
			for (i = 0; i < Length; i++)
			{
				current += GetMatrixEl(left, Row, i) * GetMatrixEl(right, i, Col);
			}
			return current;
		}
	}
}

int MatrixEqualMatrix(matrix* A, matrix* B)
{
	int i, j;
	if (! (A && B)) return 0;
	if ((NumberOfRows(*A) != NumberOfRows(*B)) || (NumberOfColumns(*A) != NumberOfColumns(*B))) return 0;
	for (i = 0; i < NumberOfRows(*A); i++)
	{
		for (j = 0; j < NumberOfColumns(*A); j++)
		{
			if (GetMatrixEl(A, i, j) != GetMatrixEl(B, i, j))
			{
				return 0;
				break;
			}
		}
	}
	return 1;
}
