// quaternion.cpp (Quaternion implementation)
//
//  The WorldForge Project
//  Copyright (C) 2002  The WorldForge Project
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//  For information about WorldForge and its authors, please contact
//  the Worldforge Web Site at http://www.worldforge.org.
//

// Author: Ron Steinke

// Some code here was taken from the quaternion implementation is
// eris. Some of the other algorithms are based on information
// found here <http://www.cs.ualberta.ca/~andreas/math/matrfaq_latest.html>
// and here <http://www.cs.berkeley.edu/~laura/cs184/quat/quaternion.html>.

#include "quaternion.h"
#include "error.h"
#include "rotmatrix.h"

#include <cmath>

#include <cassert>

namespace WFMath {

static_assert(std::is_standard_layout<Quaternion>::value, "Quaternion should be standard layout.");
static_assert(std::is_trivially_copyable<Quaternion>::value, "Quaternion should be trivially copyable.");


Quaternion::Quaternion(CoordType w_in,
					   CoordType x_in,
					   CoordType y_in,
					   CoordType z_in)
		: m_w(0), m_vec(), m_valid(true), m_age(1) {
	CoordType norm = std::sqrt(w_in * w_in + x_in * x_in + y_in * y_in + z_in * z_in);

	m_w = w_in / norm;
	m_vec[0] = x_in / norm;
	m_vec[1] = y_in / norm;
	m_vec[2] = z_in / norm;
	m_vec.setValid();
}

const Quaternion& Quaternion::IDENTITY() {
	static Quaternion ident = Quaternion(Identity());
	return ident;
}


// The equality functions regard q and -q as equal, since they
// correspond to the same rotation matrix. We consider the form
// of the quaternion with w > 0 canonical.

bool Quaternion::isEqualTo(const Quaternion& q, CoordType epsilon) const {
	// Since the sum of squares is 1, the magnitude of the largest
	// element must be between 1 and 0.5, so we don't need to scale epsilon.

	assert(epsilon > 0);

	//If anyone is invalid they are never equal
	if (!q.m_valid || !m_valid) {
		return false;
	}

	if (std::fabs(m_w - q.m_w) <= epsilon) {
		int i;
		for (i = 0; i < 3; ++i)
			if (std::fabs(m_vec[i] - q.m_vec[i]) > epsilon)
				break; // try again with swapped signs
		if (i == 3) // got through the loop
			return true;
	}

	// This makes q == -q true
	if (std::fabs(m_w + q.m_w) <= epsilon) {
		for (int i = 0; i < 3; ++i)
			if (std::fabs(m_vec[i] + q.m_vec[i]) > epsilon)
				return false;
		return true;
	}

	return false;
}

// order of multiplication vs sense of rotation
//
// v.rotate(q1).rotate(q2) is the same as v.rotate(q1 * q2),
// the same as with matrices

Quaternion& Quaternion::operator*=(const Quaternion& rhs) {
	m_valid = m_valid && rhs.m_valid;
	m_age = m_age + rhs.m_age;
	checkNormalization();

	CoordType old_w = m_w;
	m_w = m_w * rhs.m_w - Dot(m_vec, rhs.m_vec);
	m_vec = old_w * rhs.m_vec + rhs.m_w * m_vec - Cross(m_vec, rhs.m_vec);

	return *this;
}

Quaternion& Quaternion::operator/=(const Quaternion& rhs) {
	m_valid = m_valid && rhs.m_valid;
	m_age = m_age + rhs.m_age;
	checkNormalization();

	CoordType old_w = m_w;
	m_w = m_w * rhs.m_w + Dot(m_vec, rhs.m_vec);
	m_vec = rhs.m_w * m_vec - old_w * rhs.m_vec + Cross(m_vec, rhs.m_vec);

	return *this;
}

bool Quaternion::fromRotMatrix(const RotMatrix<3>& m) {
	RotMatrix<3> m_tmp;
	bool not_flip = !m.parity();

	m_valid = m.isValid();
	m_vec.setValid(m.isValid());

	if (!not_flip)
		m_tmp = Prod(m, RotMatrix<3>().mirrorX());

	const RotMatrix<3>& m_ref = not_flip ? m : m_tmp;

	CoordType s;
	const int nxt[3] = {1, 2, 0};
	CoordType tr = m_ref.trace();

	// check the diagonal
	if (tr > 0.0) {
		s = std::sqrt(tr + 1.0f);
		m_w = (s / 2.0f);
		s = (0.5f / s);

		m_vec[0] = (m_ref.elem(2, 1) - m_ref.elem(1, 2)) * s;
		m_vec[1] = (m_ref.elem(0, 2) - m_ref.elem(2, 0)) * s;
		m_vec[2] = (m_ref.elem(1, 0) - m_ref.elem(0, 1)) * s;
	} else {
		// diagonal is negative
		int i = 0;

		if (m_ref.elem(1, 1) > m_ref.elem(0, 0)) i = 1;
		if (m_ref.elem(2, 2) > m_ref.elem(i, i)) i = 2;

		int j = nxt[i], k = nxt[j];

		s = std::sqrt(1.0f + m_ref.elem(i, i) - m_ref.elem(j, j) - m_ref.elem(k, k));
		m_vec[i] = -(s * 0.5f);

		assert("sqrt() returns positive" && s > 0.0);
		s = (0.5f / s);

		m_w = (m_ref.elem(k, j) - m_ref.elem(j, k)) * s;
		m_vec[j] = (m_ref.elem(i, j) + m_ref.elem(j, i)) * s;
		m_vec[k] = (m_ref.elem(i, k) + m_ref.elem(k, i)) * s;
	}

	m_age = m.age();

	return not_flip;
}

Quaternion Quaternion::inverse() const {
	Quaternion q(m_valid);
	q.m_w = m_w;
	q.m_vec = -m_vec;
	q.m_age = m_age; // no multiplication was done, so roundoff error does not increase
	return q;
}

Quaternion& Quaternion::rotate(const RotMatrix<3>& m) {
	// FIXME find a more efficient way to do this
	Quaternion tmp;
	tmp.fromRotMatrix(m);
	*this *= tmp;
	return *this;
}

Quaternion& Quaternion::rotation(int axis, CoordType angle) {
	if (axis < 0 || axis > 2) {
		m_valid = false;
		return *this;
	}

	CoordType half_angle = angle / 2;

	m_w = std::cos(half_angle);
	for (int i = 0; i < 3; ++i)
		// Note sin() only called once
		m_vec[i] = (i == axis) ? std::sin(half_angle) : 0;
	m_vec.setValid();

	m_valid = true;
	m_age = 1;

	return *this;
}

Quaternion& Quaternion::rotation(const Vector<3>& axis, CoordType angle) {
	CoordType axis_mag = axis.mag();
	CoordType half_angle = angle / 2;

	if (axis_mag < numeric_constants<CoordType>::epsilon()) {
		m_valid = false;
		return *this;
	}

	m_w = std::cos(half_angle);
	m_vec = axis * (std::sin(half_angle) / axis_mag);

	m_valid = axis.isValid();
	m_age = 1;

	return *this;
}

Quaternion& Quaternion::rotation(const Vector<3>& axis) {
	CoordType axis_mag = axis.mag();
	CoordType half_angle = axis_mag / 2;

	if (axis_mag < numeric_constants<CoordType>::epsilon()) {
		m_valid = false;
		return *this;
	}

	m_w = std::cos(half_angle);
	m_vec = axis * (std::sin(half_angle) / axis_mag);

	m_valid = axis.isValid();
	m_age = 1;

	return *this;
}

Quaternion& Quaternion::rotation(const Vector<3>& from, const Vector<3>& to) {
	CoordType mag_prod = std::sqrt(from.sqrMag() * to.sqrMag());
	CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;

	if (mag_prod < numeric_constants<CoordType>::epsilon()) {
		m_valid = false;
		return *this;
	}

	// antiparallel vectors
	if (ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) // same check as used in the RotMatrix function
		throw ColinearVectors<3>(from, to);

	// cosine of half the angle
	m_w = std::sqrt(ctheta_plus_1 / 2.f);

	// vector in direction of axis, magnitude of cross product is proportional to
	// the sin of the angle, divide to make the magnitude the sin of half the angle,
	// sin(x) = 2sin(x/2)cos(x/2), so sin(x/2) = sin(x)/(2cos(x/2))
	m_vec = Cross(from, to) / (2 * mag_prod * m_w);

	m_valid = from.isValid() && to.isValid();
	m_age = 1;

	return *this;
}

Quaternion& Quaternion::rotation(const Vector<3>& from, const Vector<3>& to, const Vector<3>& fallbackAxis) {
	CoordType mag_prod = std::sqrt(from.sqrMag() * to.sqrMag());
	CoordType ctheta_plus_1 = Dot(from, to) / mag_prod + 1;

	if (mag_prod < numeric_constants<CoordType>::epsilon()) {
		m_valid = false;
		return *this;
	}

	// antiparallel vectors
	if (ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) { // same check as used in the RotMatrix function
		rotation(fallbackAxis, numeric_constants<CoordType>::pi());
	} else {
		// cosine of half the angle
		m_w = std::sqrt(ctheta_plus_1 / 2.f);

		// vector in direction of axis, magnitude of cross product is proportional to
		// the sin of the angle, divide to make the magnitude the sin of half the angle,
		// sin(x) = 2sin(x/2)cos(x/2), so sin(x/2) = sin(x)/(2cos(x/2))
		m_vec = Cross(from, to) / (2 * mag_prod * m_w);
		m_valid = from.isValid() && to.isValid();
		m_age = 1;
	}

	return *this;
}

void Quaternion::normalize() {
	// Assume that we're not too far off, and compute the norm
	// only to linear order in the difference from 1.
	// If q.sqrMag() = 1 + x, q.mag() = 1 + x/2 = (q.SqrMag() + 1)/2
	// to linear order.
	CoordType norm = (m_w * m_w + m_vec.sqrMag() + 1) / 2;
	m_w /= norm;
	m_vec /= norm;
	m_age = 1;
}

}
