#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 B whose first element is non-zero */
int find_nonzero_column(float A[][N], int m)
{
	int i;

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

/* Returns the first row of B whose first element is non-zero */
int find_nonzero_row(float A[][N], int m)
{
	int i;

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


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

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


/* Sets matrix A of size n to identity */
void set_to_identity(float A[][N], int n)
{
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
			if (i == j)
				A[i][j] = 1;
			else
				A[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.
 */
void copy_matrix(float A[][N], int u, float B[][N], int v, int m)
{
	for (int i = 0; i < m; i++)
		for (int j = 0; j < m; j++)
			A[i+u][j+u] = B[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 square matrices A and B */
void multiply_matrix(float A[][N], float B[][N], int n, float C[][N])
{
	float D[N][N]; // temporary storage

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

	copy_matrix(C, 0, D, 0, n); // copy D to C
}



/* 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;
}