package speiger.src.coreengine.math.vector.quaternion;

import java.nio.ByteBuffer;
import java.nio.FloatBuffer;

import speiger.src.coreengine.math.MathUtils;
import speiger.src.coreengine.math.vector.floats.Vec4f;
import speiger.src.coreengine.math.vector.matrix.Matrix4f;

public interface Quaternion {
	public static final Quaternion ZERO = of(0F, 0F, 0F, 0F);
	public static final Quaternion IDENTITY = of();
	
	public static Quaternion mutable() { return new QuaternionMutable(); }
	public static Quaternion mutable(Quaternion source) { return mutable(source.getX(), source.getY(), source.getZ(), source.getW()); }
	public static Quaternion mutable(float x, float y, float z, float w) { return new QuaternionMutable(x, y, z, w); }
	public static Quaternion mutableRad(float x, float y, float z, float angle) { return mutable().setAxisRad(x, y, z, angle); }
	public static Quaternion mutableDeg(float x, float y, float z, float angle) { return mutable().setAxisDeg(x, y, z, angle); }
	public static Quaternion of() { return new QuaternionImmutable(); }
	public static Quaternion of(Quaternion source) { return of(source.getX(), source.getY(), source.getZ(), source.getW()); }
	public static Quaternion of(float x, float y, float z, float w) { return new QuaternionImmutable(x, y, z, w); }
	public static Quaternion ofRad(float x, float y, float z, float angle) { return ZERO.setAxisRad(x, y, z, angle); }
	public static Quaternion ofDeg(float x, float y, float z, float angle) { return ZERO.setAxisDeg(x, y, z, angle); }
	public static double acos(double v) { return v < -1D ? 3.141592653589793D : (v > 1D ? 0D : Math.acos(v)); }
	
	public Quaternion setX(float x);
	public Quaternion setY(float y);
	public Quaternion setZ(float z);
	public Quaternion setW(float w);
	public float getX();
	public float getY();
	public float getZ();
	public float getW();
	
	public default float[] asArray() { return new float[] {getX(), getY(), getZ(), getW()}; }
	public default Quaternion negate() { return set(0F, 0F, 0F, 0F); }
	public default Quaternion invert() { return set(-getX(), -getY(), -getZ(), -getW()); }
	public default Quaternion normalize() { return scale(1.0F / length()); }
	public default Quaternion conjugate() { return set(-getX(), -getY(), -getZ(), getW()); }
	public default Quaternion setIdentity() { return set(0F, 0F, 0F, 1F); }
	public default Quaternion multiply(Quaternion other) { return set(getX() * other.getW() + getW() * other.getX() + getY() * other.getZ() - getZ() * other.getY(), getY() * other.getW() + getW() * other.getY() + getZ() * other.getX() - getX() * other.getZ(), getZ() * other.getW() + getW() * other.getZ() + getX() * other.getY() - getY() * other.getX(), getW() * other.getW() - getX() * other.getX() - getY() * other.getY() - getZ() * other.getZ()); }
	public default Quaternion scale(float scale) { return set(getX() * scale, getY() * scale, getZ() * scale, getW() * scale); }
	public default Quaternion rotateX(float angle) {
		angle = (float)Math.toRadians(angle);
		float sin = MathUtils.sin(angle * 0.5D);
		float cos = MathUtils.cos(angle * 0.5D);
		return set(getW() * sin + getX() * cos, getY() * cos + getZ() * sin, getZ() * cos - getY() * sin, getW() * cos - getX() * sin);
	}
	
	public default Quaternion rotateY(float angle) {
		angle = (float)Math.toRadians(angle);
		float sin = MathUtils.sin(angle * 0.5D);
		float cos = MathUtils.cos(angle * 0.5D);
		return set(getX() * cos - getZ() * sin, getW() * sin + getY() * cos, getX() * sin + getZ() * cos, getW() * cos - getY() * sin);
	}
	
	public default Quaternion rotateZ(float angle) {
		angle = (float)Math.toRadians(angle);
		float sin = MathUtils.sin(angle * 0.5D);
		float cos = MathUtils.cos(angle * 0.5D);
		return set(getX() * cos + getY() * sin, getY() * cos - getX() * sin, getW() * sin + getZ() * cos, getW() * cos - getZ() * sin);
	}
	
	public default Quaternion set(Vec4f value) { return set(value.x(), value.y(), value.z(), value.w()); }
	public default Quaternion set(Quaternion value) { return set(value.getX(), value.getY(), value.getZ(), value.getW()); }
	
	public default Quaternion set(Matrix4f matrix) {
		float tr = matrix.get(0, 0) + matrix.get(1, 1) + matrix.get(2, 2);
		if(tr >= 0.0D) {
			float s = (float)Math.sqrt(tr + 1.0D);
			float w = s * 0.5F;
			s = 0.5F / s;
			float x = (matrix.get(2, 1) - matrix.get(1, 2)) * s;
			float y = (matrix.get(0, 2) - matrix.get(2, 0)) * s;
			float z = (matrix.get(1, 0) - matrix.get(0, 1)) * s;
			return set(x, y, z, w);
		}
		else {
			float max = Math.max(Math.max(matrix.get(0, 0), matrix.get(1, 1)), matrix.get(2, 2));
			if(max == matrix.get(0, 0)) {
				float s = (float)Math.sqrt(matrix.get(0, 0) - (matrix.get(1, 1) + matrix.get(2, 2)) + 1.0D);
				float x = s * 0.5F;
				s = 0.5F / s;
				float y = (matrix.get(0, 1) + matrix.get(1, 0)) * s;
				float z = (matrix.get(2, 0) + matrix.get(0, 2)) * s;
				float w = (matrix.get(2, 1) - matrix.get(1, 2)) * s;
				return set(x, y, z, w);
			}
			else if(max == matrix.get(1, 1)) {
				float s = (float)Math.sqrt(matrix.get(1, 1) - (matrix.get(2, 2) + matrix.get(0, 0)) + 1.0D);
				float y = s * 0.5F;
				s = 0.5F / s;
				float z = (matrix.get(1, 2) + matrix.get(2, 1)) * s;
				float x = (matrix.get(0, 1) + matrix.get(1, 0)) * s;
				float w = (matrix.get(0, 2) - matrix.get(2, 0)) * s;
				return set(x, y, z, w);
			}
			else {
				float s = (float)Math.sqrt(matrix.get(2, 2) - (matrix.get(0, 0) + matrix.get(1, 1)) + 1.0D);
				float z = s * 0.5F;
				s = 0.5F / s;
				float x = (matrix.get(2, 0) + matrix.get(0, 2)) * s;
				float y = (matrix.get(1, 2) + matrix.get(2, 1)) * s;
				float w = (matrix.get(1, 0) - matrix.get(0, 1)) * s;
				return set(x, y, z, w);
			}
		}
	}
	
	public default Quaternion setAxisRad(float x, float y, float z, float angle) {
		float sin = MathUtils.sin(angle * 0.5D);
		float cos = MathUtils.cos(angle * 0.5D);
		return set(x * sin, y * sin, z * sin, cos);
	}
	
	public default Quaternion setAxisDeg(float x, float y, float z, float angle) {
		angle = (float)Math.toRadians(angle);
		float sin = MathUtils.sin(angle * 0.5D);
		float cos = MathUtils.cos(angle * 0.5D);
		return set(x * sin, y * sin, z * sin, cos);
	}
	
	public Quaternion set(float x, float y, float z, float w);
	public default float dot(Quaternion other) { return getX() * other.getX() + getY() * other.getY() + getZ() * other.getZ() + getW() * other.getW(); }
	public default Quaternion difference(Quaternion other) { return difference(other, this); }
	public default Quaternion difference(Quaternion other, Quaternion result) {
		float invNorm = 1.0F / (getX() * getX() + getY() * getY() + getZ() * getZ() + getW() * getW());
		float x = -getX() * invNorm;
		float y = -getY() * invNorm;
		float z = -getZ() * invNorm;
		float w = getW() * invNorm;
		return set(w * other.getX() + x * other.getW() + y * other.getZ() - z * other.getY(), w * other.getY() - x * other.getZ() + y * other.getW() + z * other.getX(), w * other.getZ() + x * other.getY() - y * other.getX() + z * other.getW(), w * other.getW() - x * other.getX() - y * other.getY() - z * other.getZ());
	}
	
	public default Quaternion lerp(Quaternion other, float progress) { return lerp(other, progress, this); }
	public default Quaternion lerp(Quaternion other, float progress, Quaternion result) {
		float cosom = getX() * other.getX() + getY() * other.getY() + getZ() * other.getZ() + getW() * other.getW();
		float absCosom = Math.abs(cosom);
		float scale1;
		float scale0;
		if(1.0F - absCosom > 1.0E-006F) {
			float sinSqr = 1.0F - absCosom * absCosom;
			float sinom = (float)(1.0D / Math.sqrt(sinSqr));
			float omega = (float)Math.atan2(sinSqr * sinom, absCosom);
			scale0 = MathUtils.sin((1.0D - progress) * omega) * sinom;
			scale1 = MathUtils.sin(progress * omega) * sinom;
		}
		else {
			scale0 = 1.0F - progress;
			scale1 = progress;
		}
		scale1 = cosom >= 0.0F ? scale1 : -scale1;
		return result.set(scale0 * getX() + scale1 * other.getX(), scale0 * getY() + scale1 * other.getY(), scale0 * getZ() + scale1 * other.getZ(), scale0 * getW() + scale1 * other.getW());
	}
	
	public default float length() { return (float)Math.sqrt(lengthSquared()); }
	public default double lengthSquared() { return (getX() * getX()) + (getY() * getY()) + (getZ() * getZ()) + (getW() * getW()); }
	public default Matrix4f asRotationMatrix() { return new Matrix4f().rotate(this); }
	public default Vec4f toAxisDegreeRotation() { return toAxisDegreeRotation(Vec4f.mutable()); }
	public default Vec4f toAxisDegreeRotation(Vec4f input) {
		double invSqrt = 1.0D / Math.sqrt(1.0D - getW() * getW());
		return input.set((float)(getX() * invSqrt), (float)(getY() * invSqrt), (float)(getZ() * invSqrt), (float)Math.toDegrees(acos(getW()) * 2F));
	}
	
	public default Vec4f toAxisRotation() { return toAxisRotation(Vec4f.mutable()); }
	public default Vec4f toAxisRotation(Vec4f input) {
		double invSqrt = 1.0D / Math.sqrt(1.0D - getW() * getW());
		return input.set((float)(getX() * invSqrt), (float)(getY() * invSqrt), (float)(getZ() * invSqrt), (float)acos(getW()) * 2F);
	}
	
	public default Quaternion store(ByteBuffer buffer) {
		buffer.putFloat(getX()).putFloat(getY()).putFloat(getZ()).putFloat(getW());
		return this;
	}
	
	public default Quaternion load(ByteBuffer buffer) { return set(buffer.getFloat(), buffer.getFloat(), buffer.getFloat(), buffer.getFloat()); }
	
	public default Quaternion store(FloatBuffer buffer) {
		buffer.put(getX()).put(getY()).put(getZ()).put(getW());
		return this;
	}
	
	public default Quaternion load(FloatBuffer buffer) { return set(buffer.get(), buffer.get(), buffer.get(), buffer.get()); }
	
	public Quaternion copy();
	public boolean isMutable();
	public default Quaternion asMutable() { return isMutable() ? this : of(this); }
	public default Quaternion asImmutable() { return isMutable() ? mutable(this) : this; }
	public default Quaternion copyAsMutable() { return mutable(this); }
	public default Quaternion copyAsImmutable() { return of(this); }
}
