//#include "matrix.h"

/*
Matrix

Author: Darcy Cook

Description: This is the file implements many functions that are used for matrix operations

*/





#include <stdlib.h>
#include <memory.h>
#include <math.h>

#ifdef CHECK_DIMENSIONS
#include <stdio.h>
#endif

void create_matrix(Matrix *matrix_ptr, unsigned int rows, unsigned int cols)
{
    // allocate memory for the matrix
    matrix_ptr->data = malloc(rows * cols * sizeof(Matrix_Unit));

    // set the rows and cols in the matrix struct
    matrix_ptr->rows = rows;
    matrix_ptr->cols = cols;
}

void free_matrix(Matrix *matrix_ptr)
{
    // free the data
    free(matrix_ptr->data);

    // set rows and cols to 0
    matrix_ptr->rows = 0;
    matrix_ptr->cols = 0;
}

void all_zeros(Matrix *matrix_ptr)
{
	unsigned int i, j;
	
	for (i = 0; i < matrix_ptr->rows; i++)
	{
		for (j = 0; j < matrix_ptr->cols; j++)
		{
			//set every element in the matrix to 0
			get_matrix_element(matrix_ptr, i, j) = 0;
		}
	} 
}

void set_all_elements(Matrix *matrix_ptr, Matrix_Unit value)
{
	unsigned int i, j;
	
	for (i = 0; i < matrix_ptr->rows; i++)
	{
		for (j = 0; j < matrix_ptr->cols; j++)
		{
			//set every element in the matrix to the specified value
			get_matrix_element(matrix_ptr, i, j) = value;
		}
	}
}

void set_block_elements(Matrix *matrix_ptr, Matrix_Unit value, int start_row, int start_col, int end_row, int end_col)
{
#ifdef CHECK_DIMENSIONS
	if ((start_row > end_row) || (start_col > end_col))
	{
		printf("Specified end row or end column is smaller than start row or start column for block set\n");
	}
	else if ((matrix_ptr->rows <= end_row) || (matrix_ptr->cols <= end_col))
	{
		printf("End row or column is out of range for set block elements\n");
		return;
	}		
#endif

	unsigned int i, j;
	
	for (i = start_row; i < end_row; i++)
	{
		for (j = start_col; j < end_col; j++)
		{
			//set every element in the specified block to the specified value
			get_matrix_element(matrix_ptr, i, j) = value;
		}
	}
}

void matrix_addition(Matrix *m1_ptr, Matrix *m2_ptr, Matrix *result_ptr)
{
	unsigned int i, j;
	
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows != m2_ptr->rows) || (m1_ptr->cols != m2_ptr->cols))
	{
		printf("Dimensions do not match in matrix addition\n");
		return;
	}
	else if ((m1_ptr->rows != result_ptr->rows) || (m1_ptr->cols != result_ptr->cols)) 
	{
		printf("Dimensions do not match in matrix addition\n");
		return;
	}	
#endif
	
	for (i = 0; i < m1_ptr->rows; i++)
	{
		for (j = 0; j < m1_ptr->cols; j++)
		{
			//add the matrix elements
			get_matrix_element(result_ptr, i, j) = get_matrix_element(m1_ptr, i, j) + get_matrix_element(m2_ptr, i, j);	
		}
	}	
}

void matrix_block_addition(int block_rows, int block_cols, 
						   	Matrix *m1_ptr, int m1_start_row, int m1_start_col, 
						 	Matrix *m2_ptr, int m2_start_row, int m2_start_col,
						 	Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j;	
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows < (m1_start_row+block_rows)) || (m1_ptr->cols < (m1_start_col+block_cols)))
	{
		printf("m1 dimensions are not sufficient for matrix addition\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+block_rows)) || (m2_ptr->cols < (m2_start_col+block_cols))) 
	{
		printf("m2 dimensions are not sufficient for matrix addition\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_rows)) || (result_ptr->cols < (result_start_col+block_cols))) 
	{
		printf("result dimensions are not sufficient for matrix addition\n");
		return;
	}	
#endif
	for (i = 0; i < block_rows; i++)
	{
		for (j = 0; j < block_cols; j++)
		{
			//add the matrix elements
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) 
				= get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+j) 
				  + get_matrix_element(m2_ptr, m2_start_row+i, m2_start_col+j);		
		}
	}
}

void matrix_block_subtraction(int block_rows, int block_cols, 
						   	Matrix *m1_ptr, int m1_start_row, int m1_start_col, 
						 	Matrix *m2_ptr, int m2_start_row, int m2_start_col,
						 	Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j;	
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows < (m1_start_row+block_rows)) || (m1_ptr->cols < (m1_start_col+block_cols)))
	{
		printf("m1 dimensions are not sufficient for matrix subtraction\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+block_rows)) || (m2_ptr->cols < (m2_start_col+block_cols))) 
	{
		printf("m2 dimensions are not sufficient for matrix subtraction\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_rows)) || (result_ptr->cols < (result_start_col+block_cols))) 
	{
		printf("result dimensions are not sufficient for matrix subtraction\n");
		return;
	}	
#endif
	for (i = 0; i < block_rows; i++)
	{
		for (j = 0; j < block_cols; j++)
		{
			//subtract the matrix elements
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) 
				= get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+j) 
				  - get_matrix_element(m2_ptr, m2_start_row+i, m2_start_col+j);		
		}
	}
}

void matrix_division(Matrix *m1_ptr, double divisor, Matrix *result_ptr)
{
	unsigned int i, j;
	#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows != result_ptr->rows) || (m1_ptr->cols != result_ptr->cols))
	{
		printf("Dimensions do not match in matrix addition\n");
		return;
	}	
	else if (divisor == 0)
	{
		printf("Divide by 0\n");
		return;
	}
	#endif
	for (i = 0; i < m1_ptr->rows; i++)
	{
		for (j = 0; j < m1_ptr->cols; j++)
		{
			if (divisor != 0)
			{
				//divide each element by the divisor
				get_matrix_element(result_ptr, i, j) = get_matrix_element(m1_ptr, i, j) / divisor;	
			}			
		}
	}	
}

void matrix_block_autocorrelation_sum(int block_rows, int block_cols, 
								  	Matrix *m1_ptr, int m1_start_row, int m1_start_col, 
								  	Matrix *m2_ptr, int m2_start_row, int m2_start_col,
						 		  	Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j;	
#ifdef CHECK_DIMENSIONS
	if (block_rows != block_cols)
	{
		printf("autocorrelation sum function must only be applied to square matricies\n");
		return;
	}
	else if ((m1_ptr->rows < (m1_start_row+block_rows)) || (m1_ptr->cols < (m1_start_col+block_cols)))
	{
		printf("m1 dimensions are not sufficient for matrix block autocorrelation sum\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+block_rows)) || (m2_ptr->cols < (m2_start_col+block_cols))) 
	{
		printf("m2 dimensions are not sufficient for matrix block autocorrelation sum\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_rows)) || (result_ptr->cols < (result_start_col+block_cols))) 
	{
		printf("result dimensions are not sufficient for matrix block autocorrelation sum\n");
		return;
	}	
#endif
	for (i = 0; i < block_rows; i++)
	{
		for (j = 0; j < block_cols; j++)
		{
			// this is the dot product of m1 and the transform of m1 added to m2
			// in Matlab notation this would be: result= m1 .* m1' + m2
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) 
				= (get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+j) * get_matrix_element(m1_ptr, m1_start_row+j, m1_start_col+i)) 
				  + get_matrix_element(m2_ptr, m2_start_row+i, m2_start_col+j);		
		}
	}
}		

void copy_matrix(Matrix *original_ptr, Matrix *copy_ptr)				 		  	
{
	unsigned int i, j;
	
#ifdef CHECK_DIMENSIONS
	if ((original_ptr->rows != copy_ptr->rows) || (original_ptr->cols != copy_ptr->cols))
	{
		printf("Dimensions do not match in matrix copy\n");
		return;
	}	
#endif
	
	for (i = 0; i < original_ptr->rows; i++)
	{
		for (j = 0; j < original_ptr->cols; j++)
		{
			//copy the matrix elements
			get_matrix_element(copy_ptr, i, j) = get_matrix_element(original_ptr, i, j);	
		}
	}	
}

void identity_matrix(Matrix *matrix_ptr)
{
	unsigned int i, j;
	
	for (i = 0; i < matrix_ptr->rows; i++)
	{
		for (j = 0; j < matrix_ptr->cols; j++)
		{
			if (i == j)
			{
				//set every element on the diagonal to 1
				get_matrix_element(matrix_ptr, i, j) = 1;
			}
			else
			{
				//set every element not on the diagonal to 0
				get_matrix_element(matrix_ptr, i, j) = 0;
			}
		}
	} 
}

void get_rotator_and_mult(Matrix *A, Matrix *Q, unsigned int i, unsigned int j)
{
	double c, s, val1, val2;
	int m;
		
#ifdef CHECK_DIMENSIONS
	if ((A->rows != Q->rows) || (A->cols != Q->cols))
	{
		printf("Dimensions do not match in get rotator\n");
		return;
	}	
	else if ((i >= A->rows) || (j >= A->cols))
	{
		printf("The rotator indicies are too large for the get rotator function\n");
		return;
	}
	else if (A->rows != A->cols)
	{
		printf("The matrix A and Q must be square for the rotator function\n");
		return;
	}
#endif
	if ((get_matrix_element(A, i, j) == 0) && (get_matrix_element(A, j, j) == 0))
	{
        c = 1;
        s = 0;
	}
    else
    {
	    //calculate the cosine rotator value
        c = get_matrix_element(A, j, j) 
            / sqrt(get_matrix_element(A, j, j)*get_matrix_element(A, j, j) + get_matrix_element(A, i, j)*get_matrix_element(A, i, j));
        //calculate the sine rotator value
        s = get_matrix_element(A, i, j) 
            / sqrt(get_matrix_element(A, j, j)*get_matrix_element(A, j, j) + get_matrix_element(A, i, j)*get_matrix_element(A, i, j));
	}
	
	//this is the result of multiplying the current value of Q by the rotator matrix
	//the actual rotator matrix does ot have to be created. 
	for (m = 0; m < Q->rows; m++)
	{
		val1 = get_matrix_element(Q, m, i);
		val2 = get_matrix_element(Q, m, j);
		get_matrix_element(Q, m, i) = val1*c+val2*(-s);
		get_matrix_element(Q, m, j) = val1*s+val2*c;
	}
}

void matrix_mult(Matrix *m1_ptr, Matrix *m2_ptr, Matrix *result_ptr)
{
	unsigned int i, j, k;
#ifdef CHECK_DIMENSIONS
	if (m1_ptr->cols != m2_ptr->rows)
	{
		printf("Inner dimensions of m1 and m2 do not agree in multiplication\n");
		return;
	}	
	else if ((result_ptr->rows != m1_ptr->rows) || (result_ptr->cols != m2_ptr->cols))
	{
		printf("The dimensions of the result matrix are not correct for multiplication\n");
		return;
	}	
#endif
	for (i = 0; i < m1_ptr->rows; i++)
	{
		for (j = 0; j < m2_ptr->cols; j++)
		{
			get_matrix_element(result_ptr, i, j) = 0;
			for (k = 0; k < m1_ptr->cols; k++)
			{
				get_matrix_element(result_ptr, i, j) += get_matrix_element(m1_ptr, i, k) * get_matrix_element(m2_ptr, k, j); 
			}
		}
	}
	
}

void matrix_transpose_mult(Matrix *m1_ptr, Matrix *m2_ptr, Matrix *result_ptr)
{
	unsigned int i, j, k;
#ifdef CHECK_DIMENSIONS
	if (m1_ptr->rows != m2_ptr->rows)
	{
		printf("Inner dimensions of transpose of m1 and m2 do not agree in transpose multiplication\n");
		return;
	}	
	else if ((result_ptr->rows != m1_ptr->cols) || (result_ptr->cols != m2_ptr->cols))
	{
		printf("The dimensions of the result matrix are not correct for transpose multiplication\n");
		return;
	}	
#endif
	for (i = 0; i < m1_ptr->cols; i++)
	{
		for (j = 0; j < m2_ptr->cols; j++)
		{
			get_matrix_element(result_ptr, i, j) = 0;
			for (k = 0; k < m1_ptr->rows; k++)
			{
				get_matrix_element(result_ptr, i, j) += get_matrix_element(m1_ptr, k, i) * get_matrix_element(m2_ptr, k, j); 
			}
		}
	}	
}

void square_matrix_block_transpose_mult(int block_dim, Matrix *m1_ptr, int m1_start_row, int m1_start_col, 
						 	Matrix *m2_ptr, int m2_start_row, int m2_start_col,
						 	Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j, k;
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows < (m1_start_row+block_dim)) || (m1_ptr->cols < (m1_start_col+block_dim)))
	{
		printf("m1 dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+block_dim)) || (m2_ptr->cols < (m2_start_col+block_dim))) 
	{
		printf("m2 dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_dim)) || (result_ptr->cols < (result_start_col+block_dim))) 
	{
		printf("result dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}	
#endif
	for (i = 0; i < block_dim; i++)
	{
		for (j = 0; j < block_dim; j++)
		{
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) = 0;
			for (k = 0; k < block_dim; k++)
			{
				get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) += get_matrix_element(m1_ptr, m1_start_row+k, m1_start_col+i) 
						* get_matrix_element(m2_ptr, m2_start_row+k, m2_start_col+j); 
			}
		}
	}	
}

void square_matrix_block_mult(int block_dim, Matrix *m1_ptr, int m1_start_row, int m1_start_col, 
						 	Matrix *m2_ptr, int m2_start_row, int m2_start_col,
						 	Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j, k;
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows < (m1_start_row+block_dim)) || (m1_ptr->cols < (m1_start_col+block_dim)))
	{
		printf("m1 dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+block_dim)) || (m2_ptr->cols < (m2_start_col+block_dim))) 
	{
		printf("m2 dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_dim)) || (result_ptr->cols < (result_start_col+block_dim))) 
	{
		printf("result dimensions are not sufficient for matrix block transpose mult\n");
		return;
	}	
#endif
	for (i = 0; i < block_dim; i++)
	{
		for (j = 0; j < block_dim; j++)
		{
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) = 0;
			for (k = 0; k < block_dim; k++)
			{
				get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) += get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+k) 
						* get_matrix_element(m2_ptr, m2_start_row+k, m2_start_col+j); 
			}
		}
	}	
}

void scalar_matrix_block_mult(Matrix_Unit scalar, Matrix *m1_ptr, int block_height, int block_width,
							  int m1_start_row, int m1_start_col, Matrix *result_ptr, 
							  int result_start_row, int result_start_col)
{
		unsigned int i, j;
#ifdef CHECK_DIMENSIONS
	if ((m1_ptr->rows < (m1_start_row+block_height)) || (m1_ptr->cols < (m1_start_col+block_width)))
	{
		printf("m1 dimensions are not sufficient for scalar-matrix block mult\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+block_height)) || (result_ptr->cols < (result_start_col+block_width)))
	{
		printf("result dimensions are not sufficient for scalar-matrix block mult\n");
		return;
	}		
#endif
	
	for (i = 0; i < block_height; i++)
	{
		for (j = 0; j < block_width; j++)
		{
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) = get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+j)*scalar;	
		}
	}
}

void matrix_block_mult(Matrix *m1_ptr, int m1_start_row, int m1_start_col, int m1_block_height, int m1_block_width,
					   Matrix *m2_ptr, int m2_start_row, int m2_start_col, int m2_block_height, int m2_block_width,
					   Matrix *result_ptr, int result_start_row, int result_start_col)
{
	unsigned int i, j, k;
#ifdef CHECK_DIMENSIONS
	if (m1_block_width != m2_block_height)
	{
		printf("Inner dimensions of m1 and m2 block do not agree in multiplication\n");
		return;
	}
	else if ((m1_ptr->rows < (m1_start_row+m1_block_height)) || (m1_ptr->cols < (m1_start_col+m1_block_width)))
	{
		printf("m1 dimensions are not sufficient for matrix block mult\n");
		return;
	}
	else if ((m2_ptr->rows < (m2_start_row+m2_block_height)) || (m2_ptr->cols < (m2_start_col+m2_block_width))) 
	{
		printf("m2 dimensions are not sufficient for matrix block mult\n");
		return;
	}
	else if ((result_ptr->rows < (result_start_row+m1_block_height)) || (result_ptr->cols < (result_start_col+m2_block_width))) 
	{
		printf("result dimensions are not sufficient for matrix block mult\n");
		return;
	}	
#endif
	for (i = 0; i < m1_block_height; i++)
	{
		for (j = 0; j < m2_block_width; j++)
		{
			get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) = 0;
			for (k = 0; k < m1_block_width; k++)
			{
				get_matrix_element(result_ptr, result_start_row+i, result_start_col+j) += 
							get_matrix_element(m1_ptr, m1_start_row+i, m1_start_col+k) * get_matrix_element(m2_ptr, m2_start_row+k, m2_start_col+j); 
			}
		}
	}
}

void print_matrix_block(Matrix* m_ptr, int start_row, int start_col, int num_rows, int num_cols)
{
	unsigned int i, j;
#ifdef CHECK_DIMENSIONS
	if ((m_ptr->rows < (start_row+num_rows)) || (m_ptr->cols < (start_col+num_cols)))
	{
		printf("Dimensions incorrect for matrix print\n");
		return;
	}		
#endif

	for (i = start_row; i < (start_row + num_rows); i++)
	{
		for (j = start_col; j < (start_col + num_cols); j++)
		{
			printf("%f ", get_matrix_element(m_ptr, i, j));
		}
		printf("\n");
	}
}
