/*
 * __________________________________________________________________
 *
 * Copyright (C) [2021] by InvenSense, Inc.
 * 
 * Permission to use, copy, modify, and/or distribute this software
 * for any purpose with or without fee is hereby granted.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
 * WARRANTIES  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
 * WARRANTIES OF MERCHANTABILITY  AND FITNESS. IN NO EVENT SHALL
 * THE AUTHOR BE LIABLE  FOR ANY SPECIAL, DIRECT, INDIRECT, OR
 * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
 * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR
 * IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 * __________________________________________________________________
 */

#include "InvBasicMath.h"

#include <limits.h>

unsigned int InvBasicMath_log2u(unsigned int val)
{
	unsigned int ret = UINT_MAX;

	while (val != 0) {
		val >>= 1;
		ret++;
	}

	return ret;
}

int InvBasicMath_isAnOrthonormalMatrix(const float matrix[9])
{
	// Check if matrix is orthogonal
	// Matrix is orthogonal if transpose(Matrix) x Matrix = Identity

	float transpose[9];
	float mult[9];
	int i, j;

	// Compute Transpose(matrix)
	for (i = 0; i < 3; i++) {
		for(j = 0; j < 3; j++) {
			transpose[i*3+j] = matrix[i+j*3];
		}
	}

	// Mulitply transpose x matrix
	mult[0] = transpose[0]*matrix[0] + transpose[1]*matrix[3] + transpose[2]*matrix[6];
	mult[1] = transpose[0]*matrix[1] + transpose[1]*matrix[4] + transpose[2]*matrix[7];
	mult[2] = transpose[0]*matrix[2] + transpose[1]*matrix[5] + transpose[2]*matrix[8];

	mult[3] = transpose[3]*matrix[0] + transpose[4]*matrix[3] + transpose[5]*matrix[6];
	mult[4] = transpose[3]*matrix[1] + transpose[4]*matrix[4] + transpose[5]*matrix[7];
	mult[5] = transpose[3]*matrix[2] + transpose[4]*matrix[5] + transpose[5]*matrix[8];

	mult[6] = transpose[6]*matrix[0] + transpose[7]*matrix[3] + transpose[8]*matrix[6];
	mult[7] = transpose[6]*matrix[1] + transpose[7]*matrix[4] + transpose[8]*matrix[7];
	mult[8] = transpose[6]*matrix[2] + transpose[7]*matrix[5] + transpose[8]*matrix[8];

	// Check that mult is identity
	for (i = 0; i < 3; i++) {
		for(j = 0; j < 3; j++) {
			if (i == j) {
				if (mult[i+j*3] != 1)
					return 0;
			}
			else {
				if (mult[i+j*3] != 0)
					return 0;
			}
		}
	}

	return 1;
}

float InvBasicMath_computeMatrixDeterminant(const float matrix[9])
{
	return matrix[0] * (matrix[4]*matrix[8] - matrix[7]*matrix[5])
	      -matrix[1] * (matrix[3]*matrix[8] - matrix[6]*matrix[5])
	      +matrix[2] * (matrix[3]*matrix[7] - matrix[4]*matrix[6]);
}


