import Foundation
import simd


public extension FloatingPoint
{
	@inline(__always) var saturate: Self { min(max(self , 0), 1) }

	@inline(__always) static func lerp(_ a: Self, _ b: Self, _ x: Self) -> Self { a * (1 - x) + b * x }

	@inline(__always) static func deg(fromRad: Self) -> Self { fromRad * (180 / Self.pi) }
	@inline(__always) static func rad(fromDeg: Self) -> Self { fromDeg * (Self.pi / 180) }

	fileprivate func axisDeadzone(_ min: Self, _ max: Self) -> Self
	{
		let xabs = abs(self)
		return if xabs <= min { 0 }
		else if xabs >= max { Self(signOf: self, magnitudeOf: 1) }
		else { Self(signOf: self, magnitudeOf: xabs - min) / (max - min) }
	}
}

public extension SIMD2 where Scalar: FloatingPoint
{
	@inline(__always) var len2: Scalar { x * x + y * y }
	//@inline(__always) var len2: Scalar { simd_dot(self, self) }
	@inline(__always) var len: Scalar { len2.squareRoot() }
	@inline(__always) var normalised: Self { self / len }

	@inline(__always) func dot(_ b: Self) -> Scalar { x * b.x + y * b.y }
	@inline(__always) func reflect(_ n: Self) -> Self { self - (n * 2 * self.dot(n)) }
	@inline(__always) func project(_ n: Self) -> Self { n * self.dot(n) }

	@inline(__always) func cross(_ b: Self) -> Scalar { x * b.y - y * b.x }

	@inline(__always) func lerp(_ b: Self, _ x: Scalar) -> Self
	{
		let invX = 1 - x
		return Self(self.x * invX + b.x * x, self.y * invX + b.y * x)
	}

	@inline(__always) func distance(_ b: Self) -> Scalar { return (b - self).len }

	func cardinalDeadzone(min: Scalar, max: Scalar) -> Self
	{
		Self(self.x.axisDeadzone(min, max), self.y.axisDeadzone(min, max))
	}

	func radialDeadzone(min: Scalar, max: Scalar) -> Self
	{
		let magnitude = self.len
		if magnitude == .zero || magnitude < min { return Self.zero }
		if magnitude > max { return self / magnitude }
		let rescale = (magnitude - min) / (max - min)
		return self / magnitude * rescale
	}
}

public extension SIMD3 where Scalar: FloatingPoint
{
	@inline(__always) static var X: Self { Self(1, 0, 0) }
	@inline(__always) static var Y: Self { Self(0, 1, 0) }
	@inline(__always) static var Z: Self { Self(0, 0, 1) }

	@inline(__always) static var up: Self      {  Y }
	@inline(__always) static var down: Self    { -Y }
	@inline(__always) static var left: Self    { -X }
	@inline(__always) static var right: Self   {  X }
	@inline(__always) static var forward: Self {  Z }
	@inline(__always) static var back: Self    { -Z }

	@inline(__always) var len2: Scalar { x * x + y * y + z * z }
	@inline(__always) var len: Scalar { len2.squareRoot() }
	@inline(__always) var normalised: Self { self / len }

	@inline(__always) mutating func normalise() { self /= len }

	@inline(__always) func lerp(_ b: Self, _ x: Scalar) -> Self
	{
		let invX = 1 - x
		return Self(self.x * invX + b.x * x, self.y * invX + b.y * x, self.z * invX + b.z * x)
	}

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

	@inline(__always) func project(_ n: Self) -> Self { n * self.dot(n) }
}

public extension SIMD4 where Scalar: FloatingPoint
{
	@inline(__always) static var X: Self { Self(1, 0, 0, 0) }
	@inline(__always) static var Y: Self { Self(0, 1, 0, 0) }
	@inline(__always) static var Z: Self { Self(0, 0, 1, 0) }
	@inline(__always) static var W: Self { Self(0, 0, 0, 1) }
}

public extension simd_float4x4
{
	@inline(__always) static var identity: Self { Self(diagonal: .one) }

	@inline(__always) static func translate(_ v: Vec3f) -> Self
	{
		Self(
			.init(  1,   0,   0, 0),
			.init(  0,   1,   0 ,0),
			.init(  0,   0,   1, 0),
			.init(v.x, v.y, v.z, 1))
	}

	@inline(__always) static func scale(_ v: Vec3f) -> Self
	{
		Self(
			.init(v.x,   0,   0,   0),
			.init(  0,   v.y, 0,   0),
			.init(  0,   0,   v.z, 0),
			.init(  0,   0,   0,   1))
	}

	@inline(__always) static func scale(scalar v: Float) -> Self
	{
		Self(
			.init(v, 0, 0, 0),
			.init(0, v, 0, 0),
			.init(0, 0, v, 0),
			.init(0, 0, 0, 1))
	}

	static func rotate(axis v: Vec3f, angle theta: Float) -> Self
	{
		//FIXME: THIS IS FUCKED UP FOR EVERYTHING OTHER THAN X AXIS ROTATION LOL
		/*
		let vv = v * v
		let xy = v.x * v.y, xz = v.x * v.z, yz = v.y * v.z
		let ts = sin(angle), tc = cos(angle)
		return Self(
			.init(
				vv.x + (tc * (1 - vv.x)),
				(xz - (tc * xy)) + (ts * v.z),
				(xz - (tc * xz)) - (ts * v.y), 0),
			.init(
				(xy - (tc * xy)) - (ts * v.z),
				vv.y + (tc * (1 - vv.y)),
				(xz - (tc * xz)) + (ts * v.x), 0),
			.init(
				(xz - (tc * xz)) + (ts * v.y),
				(xz - (tc * xz)) - (ts * v.y),
				vv.z + (tc * (1 - vv.z)), 1),
			.init(0, 0, 0, 1))
		*/
		let vxx = v.x * v.x, vxy = v.x * v.y, vxz = v.x * v.z
		let vyy = v.y * v.y, vyz = v.y * v.z
		let vzz = v.z * v.z
		let ts = sin(theta), tc = cos(theta)
		let ic = 1 - tc
		return Self(
			.init(
				ic * vxx + tc,
				ic * vxy - v.z * ts,
				ic * vxz + v.z * ts,
				0),
			.init(
				ic * vxy + v.z * ts,
				ic * vyy + tc,
				ic * vyz - v.x * ts,
				0),
			.init(
				ic * vxz - v.y * ts,
				ic * vyz + v.x * ts,
				ic * vzz + tc,
				0),
			.init(0, 0, 0, 1))
	}

	@inline(__always) static func rotate(yawPitch: Vec2f) -> Self { return rotate(yaw: yawPitch.x, pitch: yawPitch.y) }

	static func rotate(yaw ytheta: Float, pitch xtheta: Float) -> Self
	{
		let xc = cos(xtheta), xs = sin(xtheta)
		let yc = cos(ytheta), ys = sin(ytheta)

		return Self(
			.init(yc, ys *  xs, -ys * xc, 0),
			.init( 0,       xc,       xs, 0),
			.init(ys, yc * -xs,  yc * xc, 0),
			.init( 0,        0,        0, 1))
	}

	static func rotate(yaw ytheta: Float, pitch xtheta: Float, roll ztheta: Float) -> Self
	{
		//FIXME: this doesn't null against control
		let xc = cos(xtheta), xs = sin(xtheta)
		let yc = cos(ytheta), ys = sin(ytheta)
		let zc = cos(ztheta), zs = sin(ztheta)

		let ysxs = ys * xs, ycxs = yc * xs

		let result = Mat4f(
			.init(yc * zc + ysxs * zs, yc * -zs + ysxs * zc, -ys * xc, 0),
			.init(            xc * zs,              xc * zc,       xs, 0),
			.init(ys * zc - ycxs * zs, ys * -zs - ycxs * zc,  yc * xc, 0),
			.init(                  0,                    0,        0, 1))
		let shouldBe = .rotate(z: ztheta) * .rotate(x: xtheta) * .rotate(y: ytheta)

		let epsilon: Float = .ulpOfOne
		if (result != shouldBe)
		{
			assert(abs(result[0][0] - shouldBe[0][0]) <= epsilon) // epsilon
			assert(result[1][0] == shouldBe[1][0])
			assert(abs(result[2][0] - shouldBe[2][0]) <= epsilon) // epsilon
			assert(result[3][0] == shouldBe[3][0])
			assert(abs(result[0][1] - shouldBe[0][1]) <= epsilon) // epsilon
			assert(result[1][1] == shouldBe[1][1])
			assert(abs(result[2][1] - shouldBe[2][1]) <= epsilon) // epsilon
			assert(result[3][1] == shouldBe[3][1])
			assert(result[0][2] == shouldBe[0][2])
			assert(result[1][2] == shouldBe[1][2])
			assert(result[2][2] == shouldBe[2][2])
			assert(result[3][2] == shouldBe[3][2])
			assert(result[0][3] == shouldBe[0][3])
			assert(result[1][3] == shouldBe[1][3])
			assert(result[2][3] == shouldBe[2][3])
			assert(result[3][3] == shouldBe[3][3])
		}
		return result
	}

	@inline(__always) static func rotate(x theta: Float) -> Self
	{
		let c = cos(theta), s = sin(theta)
		return Self(
			.init(1,  0, 0, 0),
			.init(0,  c, s, 0),
			.init(0, -s, c, 0),
			.init(0,  0, 0, 1))
	}

	@inline(__always) static func rotate(y theta: Float) -> Self
	{
		let c = cos(theta), s = sin(theta)
		return Self(
			.init(c, 0, -s, 0),
			.init(0, 1,  0, 0),
			.init(s, 0,  c, 0),
			.init(0, 0,  0, 1))
	}

	@inline(__always) static func rotate(z theta: Float) -> Self
	{
		let c = cos(theta), s = sin(theta)
		return Self(
			.init(c, -s, 0, 0),
			.init(s,  c, 0, 0),
			.init(0,  0, 1, 0),
			.init(0,  0, 0, 1))
	}

	static func perspective(fovY: Float, aspect: Float, zNear: Float, zFar: Float) -> Self
	{
		let h = 1 / tanf(fovY * 0.5)
		let w = h / aspect
		let invClipRange = 1 / (zFar - zNear)
		let z = -(zFar + zNear) * invClipRange
		let z2 = -(2 * zFar * zNear) * invClipRange
		return simd_matrix(
			.init(w, 0,  0,  0),
			.init(0, h,  0,  0),
			.init(0, 0,  z, -1),
			.init(0, 0, z2,  0))
	}

	static func lookAt(from: Vec3f = .zero, to: Vec3f, up: Vec3f = .up) -> Self
	{
		let forward   = (to - from).normalised
		let bitangent = forward.cross(up).normalised
		let tangent   = bitangent.cross(forward).normalised
		let normal    = -forward
		return simd_matrix(
			.init(bitangent.x, tangent.x, normal.x, 0.0),
			.init(bitangent.y, tangent.y, normal.y, 0.0),
			.init(bitangent.z, tangent.z, normal.z, 0.0),
			.init(        0.0,       0.0,      0.0, 1.0))
	}
}


public typealias Vec2f = SIMD2<Float>
public typealias Vec2d = SIMD2<Double>
public typealias Vec3f = SIMD3<Float>
public typealias Vec3d = SIMD3<Double>
public typealias Vec4f = SIMD4<Float>
public typealias Vec4d = SIMD4<Double>
public typealias Mat4f = simd_float4x4