import Foundation
import simd


public struct Quaternion<T: FloatingPoint>: Equatable
{
	public var w: T, x: T, y: T, z: T

	public init() { self.x = 0; self.y = 0; self.z = 0; self.w = 1 }
	public init(_ w: T, _ x: T, _ y: T, _ z: T) { self.w = w; self.x = x; self.y = y; self.z = z }

	public static var identity: Self { .init(1, 0, 0, 0) }
}

public extension Quaternion where T: SIMDScalar
{
	init(real w: T, imaginary i: Vector3<T>) { self.w = w; self.x = i.x; self.y = i.y; self.z = i.z }
}

#if USE_SIMD

public extension Quaternion where T == Float
{
	init(_ q: simd_quatf)
	{
		self.init(real: q.real, imaginary: q.imag)
	}

	static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
	{
		let m3 = Matrix3x3.lookAt(from: from, to: to, up: up)
		let et_blee_eve_nyu = simd_quatf(m3)
		return Self(real: et_blee_eve_nyu.real, imaginary: .init(
			et_blee_eve_nyu.imag.x,
			et_blee_eve_nyu.imag.y,
			et_blee_eve_nyu.imag.z))
	}
}

#else

public extension Quaternion where T == Float
{
	init(_ m3: Matrix3x3<T>)
	{
		if m3.m22 < 0
		{
			if m3.m00 > m3.m11
			{
				let t = 1 + m3.m00 - m3.m11 - m3.m22
				self.x = t.squareRoot() * 0.5
				if m3.m21 < m3.m12 { self.x = -self.x }
				let ix4 = 1 / (4 * self.x)
				self.w = (m3.m21 - m3.m12) * ix4
				self.y = (m3.m10 + m3.m01) * ix4
				self.z = (m3.m02 + m3.m20) * ix4
				if t == 1 && self.w == 0 && self.y == 0 && self.z == 0 { self.x = 1 }
			}
			else
			{
				let t = 1 - m3.m00 + m3.m11 - m3.m22
				self.y = t.squareRoot() * 0.5
				if m3.m02 < m3.m20 { self.y = -self.y }
				let iy4 = 1 / (4 * self.y)
				self.w = (m3.m02 - m3.m20) * iy4
				self.x = (m3.m10 + m3.m01) * iy4
				self.z = (m3.m21 + m3.m12) * iy4
				if t == 1 && self.w == 0 && self.x == 0 && self.z == 0 { self.y = 1 }
			}
		}
		else
		{
			if m3.m00 < -m3.m11
			{
				let t = 1 - m3.m00 - m3.m11 + m3.m22
				self.z = t.squareRoot() * 0.5
				if m3.m10 < m3.m01 { self.z = -self.z }
				let iz4 = 1 / (4 * self.z)
				self.w = (m3.m10 - m3.m01) * iz4
				self.x = (m3.m02 + m3.m20) * iz4
				self.y = (m3.m21 + m3.m12) * iz4
				if t == 1 && self.w == 0 && self.x == 0 && self.y == 0 { self.z = 1 }
			}
			else
			{
				let t = 1 + m3.m00 + m3.m11 + m3.m22
				self.w = t.squareRoot() * 0.5
				let iw4 = 1 / (4 * self.w)
				self.x = (m3.m21 - m3.m12) * iw4
				self.y = (m3.m02 - m3.m20) * iw4
				self.z = (m3.m10 - m3.m01) * iw4
				if t == 1 && self.x == 0 && self.y == 0 && self.z == 0 { self.w = 1 }
			}
		}

		/*
		let m3 = m3FUCK.transpose
		if m3.m22 < 0
		{
			if m3.m00 > m3.m11
			{
				let t = 1 + m3.m00 - m3.m11 - m3.m22
				self.x = t.squareRoot() * 0.5
				if m3.m12 < m3.m21 { self.x = -self.x }
				let ix4 = 1 / (4 * self.x)
				self.w = (m3.m12 - m3.m21) * ix4
				self.y = (m3.m01 + m3.m10) * ix4
				self.z = (m3.m20 + m3.m02) * ix4
				if t == 1 && self.w == 0 && self.y == 0 && self.z == 0 { self.x = 1 }
			}
			else
			{
				let t = 1 - m3.m00 + m3.m11 - m3.m22
				self.y = t.squareRoot() * 0.5
				if m3.m20 < m3.m02 { self.y = -self.y }
				let iy4 = 1 / (4 * self.y)
				self.w = (m3.m20 - m3.m02) * iy4
				self.x = (m3.m01 + m3.m10) * iy4
				self.z = (m3.m12 + m3.m21) * iy4
				if t == 1 && self.w == 0 && self.x == 0 && self.z == 0 { self.y = 1 }
			}
		}
		else
		{
			if m3.m00 < -m3.m11
			{
				let t = 1 - m3.m00 - m3.m11 + m3.m22
				self.z = t.squareRoot() * 0.5
				if m3.m01 < m3.m10 { self.z = -self.z }
				let iz4 = 1 / (4 * self.z)
				self.w = (m3.m01 - m3.m10) * iz4
				self.x = (m3.m20 + m3.m02) * iz4
				self.y = (m3.m12 + m3.m21) * iz4
				if t == 1 && self.w == 0 && self.x == 0 && self.y == 0 { self.z = 1 }
			}
			else
			{
				let t = 1 + m3.m00 + m3.m11 + m3.m22
				self.w = t.squareRoot() * 0.5
				let iw4 = 1 / (4 * self.w)
				self.x = (m3.m12 - m3.m21) * iw4
				self.y = (m3.m20 - m3.m02) * iw4
				self.z = (m3.m01 - m3.m10) * iw4
				if t == 1 && self.x == 0 && self.y == 0 && self.z == 0 { self.w = 1 }
			}
		}
		*/


		/*
		self.w = (1 + m3.m00 + m3.m11 + m3.m22).squareRoot() * 0.5
		let iw4 = 1 / (4 * self.w)
		self.x = (m3.m21 - m3.m12) * iw4
		self.y = (m3.m02 - m3.m20) * iw4
		self.z = (m3.m10 - m3.m01) * iw4
		*/
		/*
		if m3.m22 < 0
		{
			if m3.m00 > m3.m11
			{
				self.w = (1 + m3.m00 - m3.m11 - m3.m22).squareRoot() * 0.5
				let iw4 = 1 / (4 * self.w)
				//self.x = (m3.m01 + m3.m10) * iw4
				//self.y = (m3.m20 + m3.m02) * iw4
				//self.z = (m3.m12 - m3.m21) * iw4
				self.x = (m3.m21 - m3.m12) * iw4
				self.y = (m3.m02 - m3.m20) * iw4
				self.z = (m3.m10 - m3.m01) * iw4
			}
			else
			{
				self.x = (1 - m3.m00 + m3.m11 - m3.m22).squareRoot() * 0.5
				let ix4 = 1 / (4 * self.x)
				//self.w = (m3.m01 + m3.m10) * ix4
				//self.y = (m3.m12 + m3.m21) * ix4
				//self.z = (m3.m20 - m3.m02) * ix4
				self.z = (m3.m02 - m3.m20) * ix4
				self.y = (m3.m21 - m3.m12) * ix4
				self.w = (m3.m10 - m3.m01) * ix4
			}
		}
		else
		{
			if m3.m00 < -m3.m11
			{
				self.y = (1 - m3.m00 - m3.m11 + m3.m22).squareRoot() * 0.5
				let iy4 = 1 / (4 * self.y)
				//self.w = (m3.m20 + m3.m02) * iy4
				//self.x = (m3.m12 + m3.m21) * iy4
				//self.z = (m3.m01 - m3.m10) * iy4
				self.z = (m3.m10 - m3.m01) * iy4
				self.x = (m3.m21 - m3.m12) * iy4
				self.w = (m3.m02 - m3.m20) * iy4
			}
			else
			{
				self.z = (1 + m3.m00 + m3.m11 + m3.m22).squareRoot() * 0.5
				let iz4 = 1 / (4 * self.z)
				//self.w = (m3.m12 - m3.m21) * iz4
				//self.x = (m3.m20 - m3.m02) * iz4
				//self.y = (m3.m01 - m3.m10) * iz4
				self.y = (m3.m10 - m3.m01) * iz4
				self.x = (m3.m02 - m3.m20) * iz4
				self.w = (m3.m21 - m3.m12) * iz4
			}
		}
		*/
	}

	static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
	{
		Self(Matrix3x3<T>.lookAt(from: from, to: to, up: up))
	}
}

#endif


public extension Quaternion where T == Float
{
	init(axis v: Vector3<T>, angle theta: T)
	{
		let tc = (theta * 0.5).cosine, ts = (theta * 0.5).sine
		self.init(real: tc, imaginary: v * ts)
	}

	@inline(__always) func slerp(_ rhs: Self, _ theta: T) -> Self
	{
		var b = rhs
		var cosHalfTheta = self.dot(b)
		if abs(cosHalfTheta) >= 1
		{
			// if a == b or a == -b then theta == 0
			return self
		}
		else if cosHalfTheta < 0
		{
			b = -b
			cosHalfTheta = -cosHalfTheta
		}
		let halfTheta = acos(cosHalfTheta)
		let sinHalfTheta = (1 - cosHalfTheta * cosHalfTheta).squareRoot()
		if abs(sinHalfTheta) <= .ulpOfOne
		{
			// if theta == 180 degrees then result is not fully defined
			// we could rotate around any axis normal to a or b
			return (self + b) * 0.5
		}
		let ratioA = ((1 - theta) * halfTheta).sine / sinHalfTheta
		let ratioB = (theta * halfTheta).sine / sinHalfTheta
		return self * ratioA + b * ratioB
		//let piss = simd_quatf(ix: x, iy: y, iz: z, r: w)
		//let shit = simd_quatf(ix: b.x, iy: b.y, iz: b.z, r: b.w)
		//let cunt = simd_slerp(piss, shit, x)
		//return .init(real: cunt.real, imaginary: cunt.imag)
	}

	// Because swift type checking brain injury
	@inline(__always) static func * (a: Self, b: Self) -> Self
	{
		.init(
			a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
			a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
			a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
			a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w)
	}
}

public extension Quaternion where T: FloatingPoint
{
	@inline(__always) static prefix func - (this: Self) -> Self
	{
		.init(-this.w, -this.x, -this.y, -this.z)
	}

	@inline(__always) static func + (lhs: Self, rhs: Self) -> Self
	{
		.init(lhs.w + rhs.w, lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z)
	}

	@inline(__always) static func * (lhs: Self, rhs: T) -> Self
	{
		.init(lhs.w * rhs, lhs.x * rhs, lhs.y * rhs, lhs.z * rhs)
	}

	@inline(__always) mutating func normalise()
	{
		let invMag = 1 / self.dot(self).squareRoot()
		self.w *= invMag
		self.x *= invMag
		self.y *= invMag
		self.z *= invMag
	}

	@inline(__always) var normalised: Self
	{
		let invMag = 1 / self.dot(self).squareRoot()
		return self * invMag
	}

	@inline(__always) func dot(_ b: Self) -> T { x * b.x + y * b.y + z * b.z + w * b.w }
}


#if USE_SIMD

public extension Vector3 where Scalar == Float
{
	@inline(__always) static func * (v: Self, q: Quaternion<Scalar>) -> Self
	{
		return v * simd_float3x3(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w))
	}
}

#else

public extension Vector3 where T: BinaryFloatingPoint
{
	@inline(__always) static func * (v: Self, q: Quaternion<T>) -> Self
	{
		let qw = q.w, qv = Self(q.x, q.y, q.z)

		var out = qv * 2.0 * qv.dot(v)
		out += v * (qw * qw - qv.dot(qv))
		return out + qv.cross(v) * 2.0 * qw
	}
}

#endif

public typealias Quatf = Quaternion<Float>
public typealias Quatd = Quaternion<Double>