/*
    2017: Distributed by Insight SiP
	
	THIS SOFTWARE IS PROVIDED BY INSIGHT SIP "AS IS" AND ANY EXPRESS
	OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
	OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
	DISCLAIMED. IN NO EVENT SHALL INSIGHT SIP OR CONTRIBUTORS BE
	LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
	GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
	OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/


#include "math.h"

// function sets the matrix A to the identity matrix
void fmatrix3_A_eq_I(float A[3][3])
{
	int i, j;		// loop counters

	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			A[i][j] = 0.0F;
		}
		A[i][i] = 1.0F;
	}
}

// function sets the matrix A to the identity matrix
void fmatrix4_A_eq_I(float A[4][4])
{
	int i, j;		// loop counters

	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 4; j++)
		{
			A[i][j] = 0.0F;
		}
		A[i][i] = 1.0F;
	}
}

/* function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ */
/* on exit, A is replaced with its inverse */
void fmatrix4_A_eqInv_A(float A[4][4])
{
	
	int i, j, k, l, m;					// index counters
	int ipivotrow = 0, ipivotcol = 0;	// row and column of pivot element
	float largest;						// largest element used for pivoting
	float scaling;						// scaling factor in pivoting
	float recippiv;						// reciprocal of pivot element
	float ftmp;							// temporary variable used in swaps
	int icolind[3][3], irowind[3][3], ipivot[3][3];

	// initialize the pivot array to 0
	for (j = 0; j < 4; j++)
	{
		ipivot[j][0] = 0;
	}

	// main loop i over the dimensions of the square matrix A
	for (i = 0; i < 4; i++)
	{
		// zero the largest element found for pivoting
		largest = 0.0F;
		// loop over candidate rows j
		for (j = 0; j < 4; j++)
		{
			// check if row j has been previously pivoted
			if (ipivot[j][0] != 1)
			{
				// loop over candidate columns k
				for (k = 0; k < 4; k++)
				{
					// check if column k has previously been pivoted
					if (ipivot[k][0] == 0)
					{
						// check if the pivot element is the largest found so far
						if (fabs(A[j][k]) >= largest)
						{
							// and store this location as the current best candidate for pivoting
							ipivotrow = j;
							ipivotcol = k;
							largest = fabs(A[ipivotrow][ipivotcol]);
						}
					}
					else if (ipivot[k][0] > 1)
					{
						// zero determinant situation: exit with identity matrix
						fmatrix4_A_eq_I(A);
						return;
					}
				}
			}
		}
		// increment the entry in ipivot to denote it has been selected for pivoting
		ipivot[ipivotcol][0]++;

		// check the pivot rows ipivotrow and ipivotcol are not the same before swapping
		if (ipivotrow != ipivotcol)
		{
			// loop over columns l 
			for (l = 0; l < 4; l++)
			{
				// and swap all elements of rows ipivotrow and ipivotcol
				ftmp = A[ipivotrow][l];
				A[ipivotrow][l] = A[ipivotcol][l];
				A[ipivotcol][l] = ftmp;
			}
		}

		// record that on the i-th iteration rows ipivotrow and ipivotcol were swapped
		irowind[i][0] = ipivotrow;
		icolind[i][0] = ipivotcol;

		// check for zero on-diagonal element (singular matrix) and return with identity matrix if detected
		if (A[ipivotcol][ipivotcol] == 0.0F)
		{
			// zero determinant situation: exit with identity matrix
			fmatrix4_A_eq_I(A);
			return;
		}

		// calculate the reciprocal of the pivot element knowing it's non-zero
		recippiv = 1.0F / A[ipivotcol][ipivotcol];
		// by definition, the diagonal element normalizes to 1
		A[ipivotcol][ipivotcol] = 1.0F;
		// multiply all of row ipivotcol by the reciprocal of the pivot element including the diagonal element
		// the diagonal element A[ipivotcol][ipivotcol] now has value equal to the reciprocal of its previous value
		for (l = 0; l < 4; l++)
		{
			A[ipivotcol][l] *= recippiv;
		}
		// loop over all rows m of A
		for (m = 0; m < 4; m++)
		{
			if (m != ipivotcol)
			{
				// scaling factor for this row m is in column ipivotcol
				scaling = A[m][ipivotcol];
				// zero this element
				A[m][ipivotcol] = 0.0F;
				// loop over all columns l of A and perform elimination
				for (l = 0; l < 4; l++)
				{
					A[m][l] -= A[ipivotcol][l] * scaling;
				}
			}
		}
	} // end of loop i over the matrix dimensions

	// finally, loop in inverse order to apply the missing column swaps 
	for (l = 4 - 1; l >= 0; l--)
	{
		// set i and j to the two columns to be swapped
		i = irowind[l][0];
		j = icolind[l][0];

		// check that the two columns i and j to be swapped are not the same
		if (i != j)
		{
			// loop over all rows k to swap columns i and j of A
			for (k = 0; k < 4; k++)
			{
				ftmp = A[k][i];
				A[k][i] = A[k][j];
				A[k][j] = ftmp;
			}
		}
	}
}

// function calculates the matrix product A = B x C
void fmatrix4_A_eq_B_x_C(float A[4][1], float B[4][4], float C[4][1])
{
	int i, j, k;	// counters

	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 1; j++)
		{
			A[i][j] = 0.0F;
			for (k = 0; k < 4; k++)
				A[i][j] += B[i][k] * C[k][j];
		}
	}
}
