/* Copyright 2015 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "common.h"
#include "mat33.h"
#include "math.h"
#include "util.h"

#define K_EPSILON 1E-5f

void init_zero_matrix(mat33_t A)
{
	memset(A, 0, sizeof(mat33_t));
}

void init_diagonal_matrix(mat33_t A, float x)
{
	size_t i;
	init_zero_matrix(A);

	for (i = 0; i < 3; ++i)
		A[i][i] = x;
}

void mat33_scalar_mul(mat33_t A, float c)
{
	size_t i;
	for (i = 0; i < 3; ++i) {
		size_t j;
		for (j = 0; j < 3; ++j)
			A[i][j] *= c;
	}
}

void mat33_swap_rows(mat33_t A, const size_t i, const size_t j)
{
	const size_t N = 3;
	size_t k;

	if (i == j)
		return;

	for (k = 0; k < N; ++k) {
		float tmp = A[i][k];
		A[i][k] = A[j][k];
		A[j][k] = tmp;
	}
}

/*
 * Returns the eigenvalues and corresponding eigenvectors of the _symmetric_
 * matrix.
 * The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
 * "eigenvecs".
 */
void mat33_get_eigenbasis(mat33_t S, vec3_t e_vals, mat33_t e_vecs)
{
	const size_t N = 3;
	size3_t ind;
	size_t i, j, k, l, m;

	for (k = 0; k < N; ++k) {
		ind[k] = mat33_maxind(S, k);
		e_vals[k] = S[k][k];
	}

	init_diagonal_matrix(e_vecs, 1.0f);

	for (;;) {
		float y, t, s, c, p, sum;
		m = 0;
		for (k = 1; k + 1 < N; ++k) {
			if (fabsf(S[k][ind[k]]) >
			    fabsf(S[m][ind[m]])) {
				m = k;
			}
		}

		k = m;
		l = ind[m];
		p = S[k][l];

		if (fabsf(p) < K_EPSILON)
			break;

		y = (e_vals[l] - e_vals[k]) * 0.5f;

		t = fabsf(y) + sqrtf(p * p + y * y);
		s = sqrtf(p * p + t * t);
		c = t / s;
		s = p / s;
		t = p * p / t;

		if (y < 0.0f) {
			s = -s;
			t = -t;
		}

		S[k][l] = 0.0f;

		e_vals[k] -= t;
		e_vals[l] += t;

		for (i = 0; i < k; ++i)
			mat33_rotate(S, c, s, i, k, i, l);

		for (i = k + 1; i < l; ++i)
			mat33_rotate(S, c, s, k, i, i, l);

		for (i = l + 1; i < N; ++i)
			mat33_rotate(S, c, s, k, i, l, i);

		for (i = 0; i < N; ++i) {
			float tmp = c * e_vecs[k][i] - s * e_vecs[l][i];
			e_vecs[l][i] = s * e_vecs[k][i] + c * e_vecs[l][i];
			e_vecs[k][i] = tmp;
		}

		ind[k] = mat33_maxind(S, k);
		ind[l] = mat33_maxind(S, l);

		sum = 0.0f;
		for (i = 0; i < N; ++i)
			for (j = i + 1; j < N; ++j)
				sum += fabsf(S[i][j]);

		if (sum < K_EPSILON)
			break;
	}

	for (k = 0; k < N; ++k) {
		m = k;
		for (l = k + 1; l < N; ++l)
			if (e_vals[l] > e_vals[m])
				m = l;

		if (k != m) {
			float tmp = e_vals[k];
			e_vals[k] = e_vals[m];
			e_vals[m] = tmp;

			mat33_swap_rows(e_vecs, k, m);
		}
	}
}

/* index of largest off-diagonal element in row k */
size_t mat33_maxind(mat33_t A, size_t k)
{
	const size_t N = 3;
	size_t i, m = k + 1;

	for (i = k + 2; i < N; ++i)
		if (fabsf(A[k][i]) > fabsf(A[k][m]))
			m = i;

	return m;
}

void mat33_rotate(mat33_t A, float c, float s,
		  size_t k, size_t l, size_t i, size_t j)
{
	float tmp = c * A[k][l] - s * A[i][j];
	A[i][j] = s * A[k][l] + c * A[i][j];
	A[k][l] = tmp;
}