/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  CityDoctor2 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 Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.math;

/**
 * A Quaternion class storing a quaternion.
 * 
 * @author Matthias Betz
 *
 */
public class Quaternion {

	private static final double PI2 = Math.PI * 2d;

	private double x;
	private double y;
	private double z;
	private double w;

	private static final Quaternion IDENTITY = new Quaternion(0d, 0d, 0d, 1d);

	/**
	 * Create a new quaternion containing the rotation necessary to rotate the from
	 * vector to the to vector
	 * 
	 * @param from the from vector
	 * @param to   the to vector
	 * @return a Quaternion containing the rotation
	 */
	public static Quaternion fromToRotation(Vector3d from, Vector3d to) {
		double dot = from.dot(to);
		if (dot > 1d) {
			dot = 1d;
		} else if (dot < -1d) {
			dot = -1d;
		}
		double angle = Math.acos(dot);
		double x = from.getY() * to.getZ() - from.getZ() * to.getY();
		double y = from.getZ() * to.getX() - from.getX() * to.getZ();
		double z = from.getX() * to.getY() - from.getY() * to.getX();

		double d = Math.sqrt(x * x + y * y + z * z);

		if (d == 0d) {
			return IDENTITY;
		}
		d = 1d / d;

		double ang;
		if (angle < 0) {
			ang = PI2 - (-angle % PI2);
		} else {
			ang = angle % PI2;
		}
		ang = ang / 2d;
		double sin = Math.sin(ang);
		double cos = Math.cos(ang);

		Quaternion quad = new Quaternion(d * x * sin, d * y * sin, d * z * sin, cos);
		quad.normalize();
		return quad;
	}

	private Quaternion(double x, double y, double z, double w) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
	}

	/**
	 * 
	 * @return the square length of the quaternion
	 */
	public double squareLength() {
		return x * x + y * y + z * z + w * w;
	}

	/**
	 * Normalizes the quaternion by dividing with the length.
	 */
	public void normalize() {
		double length = squareLength();
		if (length != 0d && length != 1d) {
			length = Math.sqrt(length);
			x /= length;
			y /= length;
			z /= length;
			w /= length;
		}
	}

	/**
	 * Multiply this * other, stored in this, no new instances created
	 * 
	 * @param other the other Quaternion
	 * @return this * other
	 */
	public Quaternion mult(Quaternion other) {
		final double newX = w * other.x + x * other.w + y * other.z - z * other.y;
		final double newY = w * other.y + y * other.w + z * other.x - x * other.z;
		final double newZ = w * other.z + z * other.w + x * other.y - y * other.x;
		final double newW = w * other.w - x * other.x - y * other.y - z * other.z;
		this.x = newX;
		this.y = newY;
		this.z = newZ;
		this.w = newW;
		return this;
	}

	/**
	 * Converts the quaternion to a rotation matrix which is faster for multiplying
	 * with vectors to apply rotations.
	 * 
	 * @return a new 3x3 matrix containing the rotation described by this
	 *         quaternion.
	 */
	public Matrix3x3d toRotationMatrix() {
		double[][] rot = new double[3][3];
		final double xx = x * x;
		final double xy = x * y;
		final double xz = x * z;
		final double xw = x * w;
		final double yy = y * y;
		final double yz = y * z;
		final double yw = y * w;
		final double zz = z * z;
		final double zw = z * w;

		// Set matrix from quaternion
		rot[0][0] = 1 - 2 * (yy + zz);
		rot[0][1] = 2 * (xy - zw);
		rot[0][2] = 2 * (xz + yw);
		rot[1][0] = 2 * (xy + zw);
		rot[1][1] = 1 - 2 * (xx + zz);
		rot[1][2] = 2 * (yz - xw);
		rot[2][0] = 2 * (xz - yw);
		rot[2][1] = 2 * (yz + xw);
		rot[2][2] = 1 - 2 * (xx + yy);
		return new Matrix3x3d(rot);
	}
}
