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 = newQuat(0F, 0F, 0F, 0F);
	public static final Quaternion IDENTITY = newQuat();
	
	public static Quaternion newQuat(){return new QuaternionImmutable();}
	public static Quaternion newQuat(Quaternion source){return newQuat(source.getX(), source.getY(), source.getZ(), source.getW());}
	public static Quaternion newQuat(float x, float y, float z, float w){return new QuaternionImmutable(x, y, z, w);}
	public static Quaternion newAxisRadQuat(float x, float y, float z, float angle){return ZERO.setAxisRad(x, y, z, angle);}
	public static Quaternion newAxisDegQuat(float x, float y, float z, float angle){return ZERO.setAxisDeg(x, y, z, angle);}
	
	public static Quaternion newMutable(){return new QuaternionMutable();}
	public static Quaternion newMutable(Quaternion source){return newMutable(source.getX(), source.getY(), source.getZ(), source.getW());}
	public static Quaternion newMutable(float x, float y, float z, float w){return new QuaternionMutable(x, y, z, w);}
	public static Quaternion newMutableRadAxis(float x, float y, float z, float angle){return newMutable().setAxisRad(x, y, z, angle);}
	public static Quaternion newMutableDegAxis(float x, float y, float z, float angle){return newMutable().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 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.getX(), value.getY(), value.getZ(), value.getW());}
	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.newMutable());}
	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.newMutable());}
	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 : newQuat(this);}
	public default Quaternion asImmutable(){return isMutable() ? newMutable(this) : this;}
	public default Quaternion copyAsMutable(){return newMutable(this);}
	public default Quaternion copyAsImmutable(){return newQuat(this);}
}