#if USE_SIMD
import simd
#endif


#if USE_SIMD

public typealias Matrix4x4 = simd_float4x4
public extension Matrix4x4
{
	typealias T = Float
}

#else

public struct Matrix4x4<T: FloatingPoint>: Equatable
{
	public typealias Scalar = T

	public var m00: T, m01: T, m02: T, m03: T
	public var m10: T, m11: T, m12: T, m13: T
	public var m20: T, m21: T, m22: T, m23: T
	public var m30: T, m31: T, m32: T, m33: T

	public init()
	{
		self.m00 = 1; self.m01 = 0; self.m02 = 0; self.m03 = 0
		self.m10 = 0; self.m11 = 1; self.m12 = 0; self.m13 = 0
		self.m20 = 0; self.m21 = 0; self.m22 = 1; self.m23 = 0
		self.m30 = 0; self.m31 = 0; self.m32 = 0; self.m33 = 1
	}

	public init(
		_ a00: T, _ a01: T, _ a02: T, _ a03: T,
		_ a10: T, _ a11: T, _ a12: T, _ a13: T,
		_ a20: T, _ a21: T, _ a22: T, _ a23: T,
		_ a30: T, _ a31: T, _ a32: T, _ a33: T)
	{
		self.m00 = a00; self.m01 = a01; self.m02 = a02; self.m03 = a03
		self.m10 = a10; self.m11 = a11; self.m12 = a12; self.m13 = a13
		self.m20 = a20; self.m21 = a21; self.m22 = a22; self.m23 = a23
		self.m30 = a30; self.m31 = a31; self.m32 = a32; self.m33 = a33
	}
}

public extension Matrix4x4
{
	init(diagonal d: Vector4<T>)
	{
		self.m00 = d.x; self.m01 =   0; self.m02 =   0; self.m03 =   0
		self.m10 =   0; self.m11 = d.y; self.m12 =   0; self.m13 =   0
		self.m20 =   0; self.m21 =   0; self.m22 = d.z; self.m23 =   0
		self.m30 =   0; self.m31 =   0; self.m32 =   0; self.m33 = d.w
		
	}

	init(_ row0: Vector4<T>, _ row1: Vector4<T>, _ row2: Vector4<T>, _ row3: Vector4<T>)
	{
		self.m00 = row0.x; self.m01 = row0.y; self.m02 = row0.z; self.m03 = row0.w
		self.m10 = row1.x; self.m11 = row1.y; self.m12 = row1.z; self.m13 = row1.w
		self.m20 = row2.x; self.m21 = row2.y; self.m22 = row2.z; self.m23 = row2.w
		self.m30 = row3.x; self.m31 = row3.y; self.m32 = row3.z; self.m33 = row3.w
	}

	/*
	init(_ col0: Vector4<T>, _ col1: Vector4<T>, _ col2: Vector4<T>, _ col3: Vector4<T>)
	{
		self.m00 = col0.x; self.m01 = col1.x; self.m02 = col2.x; self.m03 = col3.x
		self.m10 = col0.y; self.m11 = col1.y; self.m12 = col2.y; self.m13 = col3.y
		self.m20 = col0.z; self.m21 = col1.z; self.m22 = col2.z; self.m23 = col3.z
		self.m30 = col0.w; self.m31 = col1.w; self.m32 = col2.w; self.m33 = col3.w
	}
	*/

	subscript(index: Int) -> Vector4<T>
	{
		get
		{
			switch index
			{
			case 0: rows.0
			case 1: rows.1
			case 2: rows.2
			case 3: rows.3
			default: fatalError()
			}
		}
	}

	var rows: (Vector4<T>, Vector4<T>, Vector4<T>, Vector4<T>)
	{(
		.init(m00, m01, m02, m03),
		.init(m10, m11, m12, m13),
		.init(m20, m21, m22, m23),
		.init(m30, m31, m32, m33))
	}

	var columns: (Vector4<T>, Vector4<T>, Vector4<T>, Vector4<T>)
	{(
		.init(m00, m10, m20, m30),
		.init(m01, m11, m21, m31),
		.init(m02, m12, m22, m32),
		.init(m03, m13, m23, m33))
	}

	var transpose: Self
	{
		Self(
			m00, m10, m20, m30,
			m01, m11, m21, m31,
			m02, m12, m22, m32,
			m03, m13, m23, m33)
	}

	static func * (lhs: Self, rhs: Self) -> Self
	{
		let l = rhs, r = lhs
		return Self(
			l.m00 * r.m00 + l.m01 * r.m10 + l.m02 * r.m20 + l.m03 * r.m30,
			l.m00 * r.m01 + l.m01 * r.m11 + l.m02 * r.m21 + l.m03 * r.m31,
			l.m00 * r.m02 + l.m01 * r.m12 + l.m02 * r.m22 + l.m03 * r.m32,
			l.m00 * r.m03 + l.m01 * r.m13 + l.m02 * r.m23 + l.m03 * r.m33,
			l.m10 * r.m00 + l.m11 * r.m10 + l.m12 * r.m20 + l.m13 * r.m30,
			l.m10 * r.m01 + l.m11 * r.m11 + l.m12 * r.m21 + l.m13 * r.m31,
			l.m10 * r.m02 + l.m11 * r.m12 + l.m12 * r.m22 + l.m13 * r.m32,
			l.m10 * r.m03 + l.m11 * r.m13 + l.m12 * r.m23 + l.m13 * r.m33,
			l.m20 * r.m00 + l.m21 * r.m10 + l.m22 * r.m20 + l.m23 * r.m30,
			l.m20 * r.m01 + l.m21 * r.m11 + l.m22 * r.m21 + l.m23 * r.m31,
			l.m20 * r.m02 + l.m21 * r.m12 + l.m22 * r.m22 + l.m23 * r.m32,
			l.m20 * r.m03 + l.m21 * r.m13 + l.m22 * r.m23 + l.m23 * r.m33,
			l.m30 * r.m00 + l.m31 * r.m10 + l.m32 * r.m20 + l.m33 * r.m30,
			l.m30 * r.m01 + l.m31 * r.m11 + l.m32 * r.m21 + l.m33 * r.m31,
			l.m30 * r.m02 + l.m31 * r.m12 + l.m32 * r.m22 + l.m33 * r.m32,
			l.m30 * r.m03 + l.m31 * r.m13 + l.m32 * r.m23 + l.m33 * r.m33)
	}
}

#endif

#if USE_SIMD

public extension Matrix4x4
{
	init(quat q: Quaternion<T>)
	{
		self = .init(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w)).transpose
		/*
		let FUCK = simd_matrix4x4(simd_quatf(ix: q.x, iy: q.y, iz: q.z, r: q.w))
		self = .init(
			FUCK.columns.0.x, FUCK.columns.1.x, FUCK.columns.2.x, FUCK.columns.3.x,
			FUCK.columns.0.y, FUCK.columns.1.y, FUCK.columns.2.y, FUCK.columns.3.y,
			FUCK.columns.0.z, FUCK.columns.1.z, FUCK.columns.2.z, FUCK.columns.3.z,
			FUCK.columns.0.w, FUCK.columns.1.w, FUCK.columns.2.w, FUCK.columns.3.w).transpose
		*/
	}
}

#else

public extension Matrix4x4 where T == Float
{
	init(_ m3: Matrix3x3<T>)
	{
		self.m00 = m3.m00; self.m01 = m3.m01; self.m02 = m3.m02; self.m03 = 0
		self.m10 = m3.m10; self.m11 = m3.m11; self.m12 = m3.m12; self.m13 = 0
		self.m20 = m3.m20; self.m21 = m3.m21; self.m22 = m3.m22; self.m23 = 0
		self.m30 =      0; self.m31 =      0; self.m32 =      0; self.m33 = 1
	}

	init(quat q: Quaternion<T>)
	{
		/*
		let qq = Quaternion<T>(q.w * q.w, q.x * q.x, q.y * q.y, q.z * q.z)
		return .init(
			.init(
				qq.w + qq.x - qq.y - qq.z,
				2.0 * (q.x * q.y - q.w * q.z),
				2.0 * (q.x * q.z + q.w * q.y),
				0.0),
			.init(
				2.0 * (q.x * q.y + q.w * q.z),
				qq.w - qq.x + qq.y - qq.z,
				2.0 * (q.y * q.z - q.w * q.x),
				0.0),
			.init(
				2.0 * (q.x * q.z - q.w * q.y),
				2.0 * (q.y * q.z + q.w * q.x),
				qq.w - qq.x - qq.y + qq.z,
				0.0),
			.init(0.0, 0.0, 0.0, 1.0))
		*/
		/*
		let qww = q.w * q.w
		let qxx = q.x * q.x
		let qyy = q.y * q.y
		let qzz = q.z * q.z
		.init(
			.init(
				qqw + qqx - qqy - qqz,
				2.0 * (q.x * q.y - q.w * q.z),
				2.0 * (q.x * q.z + q.w * q.y),
				0.0),
			.init(
				2.0 * (q.x * q.y + q.w * q.z),
				qqw - qqx + qqy - qqz,
				2.0 * (q.y * q.z - q.w * q.x),
				0.0),
			.init(
				2.0 * (q.x * q.z - q.w * q.y),
				2.0 * (q.y * q.z + q.w * q.x),
				qqw - qqx - qqy + qqz,
				0.0),
			.init(0.0, 0.0, 0.0, 1.0))
		*/
		let qww = q.w * q.w, qxx = q.x * q.x, qyy = q.y * q.y, qzz = q.z * q.z
		let qxy = q.x * q.y, qxz = q.x * q.z, qyz = q.y * q.z
		let qwx = q.w * q.x, qwy = q.w * q.y, qwz = q.w * q.z
		self = .init(
			.init(qww + qxx - qyy - qzz,       2 * (qxy - qwz),       2 * (qxz + qwy), 0),
			.init(      2 * (qxy + qwz), qww - qxx + qyy - qzz,       2 * (qyz - qwx), 0),
			.init(      2 * (qxz - qwy),       2 * (qyz + qwx), qww - qxx - qyy + qzz, 0),
			.init(                    0,                     0,                     0, 1)).transpose
	}

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

#endif


public extension Matrix4x4
{
	// what the fuck lmao
	@inline(__always) static func * (lhs: Self, rhs: Vector3<T>) -> Vector3<T>
	{
		return .init(
			lhs.columns.0.x * rhs.x + lhs.columns.0.y * rhs.y + lhs.columns.0.z * rhs.z,
			lhs.columns.1.x * rhs.x + lhs.columns.1.y * rhs.y + lhs.columns.1.z * rhs.z,
			lhs.columns.2.x * rhs.x + lhs.columns.2.y * rhs.y + lhs.columns.2.z * rhs.z)
	}
}

#if USE_SIMD

public extension Matrix4x4
{
	@inline(__always) static func translate(_ v: Vector3<T>) -> 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: Vector3<T>) -> 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: T) -> Self
	{
		Self(
			.init(v, 0, 0, 0),
			.init(0, v, 0, 0),
			.init(0, 0, v, 0),
			.init(0, 0, 0, 1))
	}

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

	static func rotate(yaw ytheta: T, pitch xtheta: T) -> Self
	{
		let xc = xtheta.cosine, xs = xtheta.sine
		let yc = ytheta.cosine, ys = ytheta.sine

		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))
	}

	@inline(__always) static func rotate(x theta: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T, aspect: T, zNear: T, zFar: T) -> Self
	{
		let h = 1 / (fovY * T(0.5)).tangent
		let w = h / aspect
		let invClipRange = 1 / (zFar - zNear)
		let z = -(zFar + zNear) * invClipRange
		let z2 = -(2 * zFar * zNear) * invClipRange
		return Self(
			.init(w, 0,  0,  0),
			.init(0, h,  0,  0),
			.init(0, 0,  z, -1),
			.init(0, 0, z2,  0))
	}

	static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
	{
		let forward   = (to - from).normalised
		let bitangent = forward.cross(up).normalised
		let tangent   = bitangent.cross(forward).normalised
		let normal    = -forward

		return Self(
			.init(bitangent, 0),
			.init(  tangent, 0),
			.init(   normal, 0),
			.init(0,  0,  0, 1))
	}
}

#else

public extension Matrix4x4 where T == Float
{
	@inline(__always) static func translate(_ v: Vector3<T>) -> 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: Vector3<T>) -> 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: T) -> 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: Vector3<T>, angle theta: T) -> 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(theta), tc = cos(theta)
		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 = theta.sine, tc = theta.cosine
		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: Vector2<T>) -> Self { return rotate(yaw: yawPitch.x, pitch: yawPitch.y) }

	static func rotate(yaw ytheta: T, pitch xtheta: T) -> Self
	{
		let xc = xtheta.cosine, xs = xtheta.sine
		let yc = ytheta.cosine, ys = ytheta.sine

		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: T, pitch xtheta: T, roll ztheta: T) -> Self
	{
		//FIXME: this doesn't null against control
		let xc = xtheta.cosine, xs = xtheta.sine
		let yc = ytheta.cosine, ys = ytheta.sine
		let zc = ztheta.cosine, zs = ztheta.sine

		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: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T) -> Self
	{
		let c = theta.cosine, s = theta.sine
		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: T, aspect: T, zNear: T, zFar: T) -> Self
	{
		let h = 1 / (fovY * 0.5).tangent
		let w = h / aspect
		let invClipRange = 1 / (zFar - zNear)
		let z = -(zFar + zNear) * invClipRange
		let z2 = -(2 * zFar * zNear) * invClipRange
		return Self(
			.init(w, 0,  0,  0),
			.init(0, h,  0,  0),
			.init(0, 0,  z, -1),
			.init(0, 0, z2,  0))
	}

	static func lookAt(from: Vector3<T> = .zero, to: Vector3<T>, up: Vector3<T> = .up) -> Self
	{
		let forward   = (to - from).normalised
		let bitangent = forward.cross(up).normalised
		let tangent   = bitangent.cross(forward).normalised
		let normal    = -forward

		return Self(
			.init(bitangent, 0),
			.init(  tangent, 0),
			.init(   normal, 0),
			.init(0,  0,  0, 1))
	}
}

#endif

#if USE_SIMD
public typealias Mat4f = Matrix4x4
#else
public typealias Mat4f = Matrix4x4<Float>
public typealias Mat4d = Matrix4x4<Double>
#endif