/* 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 mat33_fp_init_zero(mat33_fp_t A)
{
	memset(A, 0, sizeof(mat33_fp_t));
}

void mat33_fp_init_diagonal(mat33_fp_t A, fp_t x)
{
	const size_t N = 3;
	size_t i;

	mat33_fp_init_zero(A);

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

void mat33_fp_scalar_mul(mat33_fp_t A, fp_t c)
{
	const size_t N = 3;
	size_t i;

	for (i = 0; i < N; ++i) {
		size_t j;
		for (j = 0; j < N; ++j)
			A[i][j] = fp_mul(A[i][j], c);
	}
}

void mat33_fp_swap_rows(mat33_fp_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) {
		fp_t 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_fp_get_eigenbasis(mat33_fp_t S, fpv3_t e_vals,
			     mat33_fp_t e_vecs)
{
	const size_t N = 3;
	sizev3_t ind;
	size_t i, j, k, l, m;

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

	mat33_fp_init_diagonal(e_vecs, FLOAT_TO_FP(1.0f));

	for (;;) {
		fp_t y, t, s, c, p, sum;

		m = 0;
		for (k = 1; k + 1 < N; ++k)
			if (fp_abs(S[k][ind[k]]) > fp_abs(S[m][ind[m]]))
				m = k;

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

		/*
		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
		 * fixed-point(with 16 fp bits). The minimum positive value is
		 * 1 which is approximately 1.52E-5, so the
		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
		 */
		if (fp_abs(p) <= FLOAT_TO_FP(K_EPSILON))
			break;

		y = fp_mul(e_vals[l] - e_vals[k], FLOAT_TO_FP(0.5f));

		t = fp_abs(y) + fp_sqrtf(fp_sq(p) + fp_sq(y));
		s = fp_sqrtf(fp_sq(p) + fp_sq(t));
		c = fp_div_dbz(t, s);
		s = fp_div_dbz(p, s);
		t = fp_div_dbz(fp_sq(p), t);

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

		S[k][l] = FLOAT_TO_FP(0.0f);

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

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

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

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

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

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

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

		/*
		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
		 * fixed-point(with 16 fp bits). The minimum positive value is
		 * 1 which is approximately 1.52E-5, so the
		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
		 */
		if (sum <= FLOAT_TO_FP(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) {
			fp_t tmp = e_vals[k];
			e_vals[k] = e_vals[m];
			e_vals[m] = tmp;

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

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

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

	return m;
}

void mat33_fp_rotate(mat33_fp_t A, fp_t c, fp_t s,
		     size_t k, size_t l, size_t i, size_t j)
{
	fp_t tmp = fp_mul(c, A[k][l]) - fp_mul(s, A[i][j]);
	A[i][j] = fp_mul(s, A[k][l]) + fp_mul(c, A[i][j]);
	A[k][l] = tmp;
}