
#include <stdio.h>
#include "matrix.h"

/* This file defines a number of matrix operations:
 * 	Matrix multiplication
 * 	Find a column whose first row is non-zero
 * 	Find a row whose first column is non-zero
 * 	Swap two columns
 * 	Set a matrix to identity
 * 	Copy a matrix to another
 * 	Add a row to another after multiplying with a constant
 *
 * It also includes a function to check if a floating point value
 * is very close to zero.
 */


/* Returns the first column of A whose first element is non-zero */
int find_nonzero_column(Matrix A)
{
	int i;

	for (i = 0; i < A.cols; i++)
		if (!is_zero(A.elements[0][i]))
			return i;
	
	return i;
}

/* Returns the first row of A whose first element is non-zero */
int find_nonzero_row(Matrix A)
{
	int i;

	for (i = 0; i < A.rows; i++)
		if (!is_zero(A.elements[i][0]))
			return i;
	
	return i;
}


/* Swaps the two columns #i and #j of matrix A */
void swap_column(Matrix A, int i, int j)
{
	float t; // temporary storage

	for (int k = 0; k < A.rows; k++) {
		t = A.elements[k][i];
		A.elements[k][i] = A.elements[k][j];
		A.elements[k][j] = t;
	}
}


/* Sets matrix A to identity. if A is not a square matrix,
 * it is set to a partial identity matrix.
 */
void set_to_identity(Matrix A)
{
	for (int i = 0; i < A.rows; i++)
		for (int j = 0; j < A.cols; j++)
			if (i == j)
				A.elements[i][j] = 1;
			else
				A.elements[i][j] = 0;
}


/* Copies a submatrix of B to A by dropping the first v rows and columns of
 * B. The submatrix is copied in A from element (u, u) onwards (in effect,
 * the first u rows and columns of A are left untouched.
 * The size of submatrix is m x n.
 */
void copy_matrix(Matrix A, int u, Matrix B, int v, int m, int n)
{
	for (int i = 0; i < m; i++)
		for (int j = 0; j < n; j++)
			A.elements[i+u][j+u] = B.elements[i+v][j+v];
}

/* Adds row_2 to row_1 by first multiplying it with factor. The
 * size of rows is m.
 */
void add_row(float *row_1, float *row_2, float factor, int m)
{
	for (int i = 0; i < m; i++)
		row_1[i] = row_1[i] + factor * row_2[i];
}


/* Calculates *C = A * B for matrices A and B. returns -1 if columns of A
 * does not equal rows of B.
 */
int multiply_matrix(Matrix A, Matrix B, Matrix *C)
{
	Matrix D; // temporary storage

	if (A.cols != B.rows) // error
		return -1;

	D = allocate_matrix(A.rows, B.cols);

	for (int i = 0; i < A.rows; i++)
		for (int j = 0; j < B.cols; j++) {
			D.elements[i][j] = 0; // initialize
			for (int k = 0; k < A.cols; k++)
				D.elements[i][j] = D.elements[i][j] + A.elements[i][k] * B.elements[k][j];
		}

	copy_matrix(*C, 0, D, 0, D.rows, D.cols); // copy D to C
	C->rows = D.rows;
	C->cols = D.cols;

	free_matrix(D);
	return 1;
}



/* Checks if the input floating point value is close to zero.
 * Often, this is required for zero-testing since a floating point
 * value may not be exactly zero due to truncation errors.
 */
int is_zero(float value)
{
	if ((value < 0.0000001) && (value > -0.0000001))
		return 1;
	else
		return 0;
}

