//	GGVRMatrices.swift
//
//	© 2026 by Jeff Weeks
//	See TermsOfUse.txt

import simd

//	Notes on matrices
//
//	--------------------------------
//	Note #1
//
//	You may interpret all matrices in Curved-Space Pool Hall
//	and in Curved Spaces VR
//
//	EITHER
//
//		as acting from left to right
//
//			    ----------------------->
//			            (  the   )(  the   )
//			(row vector)( first  )( second )
//			            ( matrix )( matrix )
//
//		with each matrix's elements listed in row-major order
//
//	OR
//
//		as acting from right to left
//
//			  <-----------------------
//			(  the   )(  the   )(  a   )
//			( second )( first  )(column)
//			( matrix )( matrix )(vector)
//
//		with each matrix's elements listed in column-major order.
//
//	--------------------------------
//	Note #2
//
//	All matrix entries are dimensionless. This is obvious
//	for motions of the 3-sphere and of hyperbolic 3-space.
//	It's less obvious -- but nevertheless true -- for motions
//	of Euclidean 3-space as well. For example, if a Euclidean
//	3-space has "radius" 2 meters -- meaning its points have
//	the form (x,y,z,2) -- then a translation of 3 meters
//	in the x direction would be realized by the product
//
//		         (  1  0  0  0  )
//		(x y z 2)(  0  1  0  0  )
//		         (  0  0  1  0  )
//		         ( 1.5 0  0  1  )
//
//	where all entries, including the entry
//
//		 3 meters
//		---------- = 1.5
//		 2 meters
//
//	are dimensionless.
//
//	--------------------------------



// MARK: -
// MARK: 4D matrices

func convertFloat4x4toDouble4x4(
	_ m: simd_float4x4
) -> simd_double4x4 {

	let theDouble4x4 = simd_double4x4(
		SIMD4<Double>(m[0]),
		SIMD4<Double>(m[1]),
		SIMD4<Double>(m[2]),
		SIMD4<Double>(m[3])
	)

	return theDouble4x4
}

func convertDouble4x4toFloat4x4(
	_ m: simd_double4x4
) -> simd_float4x4 {

	let theFloat4x4 = simd_float4x4(
		SIMD4<Float>(m[0]),
		SIMD4<Float>(m[1]),
		SIMD4<Float>(m[2]),
		SIMD4<Float>(m[3])
	)

	return theFloat4x4
}

func convertQuaternionDoubleToFloat(
	_ q: simd_quatd
) -> simd_quatf {

	let qf = simd_quatf(vector: SIMD4<Float>(q.vector))
	
	return qf
}


func axisAlignedRotation(
	from firstAxis: Int,		//	= 0, 1 or 2
	towards secondAxis: Int,	//	= 0, 1 or 2, ≠ firstAxis
	angle θ: Double				//	in radians
) -> simd_double4x4 {	//	dimensionless

	precondition(
				firstAxis >= 0 &&  firstAxis <= 2
			&& secondAxis >= 0 && secondAxis <= 2
			&&  firstAxis != secondAxis,
		"axisAlignedRotation() received an invalid axis")
	
	var theRotation = matrix_identity_double4x4
	
	theRotation[firstAxis ][firstAxis ] =  cos(θ)
	theRotation[firstAxis ][secondAxis] =  sin(θ)
	theRotation[secondAxis][firstAxis ] = -sin(θ)
	theRotation[secondAxis][secondAxis] =  cos(θ)
	
	return theRotation
}

func axisAlignedTranslation(
	axis: Int,						//	= 0, 1 or 2 for translation in x, y or z direction.
	distanceInRadians θ: Double,	//	In radians, even in the Euclidean case.
	geometryType: GeometryType
) -> simd_double4x4 {	//	dimensionless, even in the Euclidean case

	precondition(
		axis >= 0 && axis <= 2,
		"axisAlignedTranslation() received an invalid axis")
	
	//	Initialize theTranslation to the identity matrix.
	var theTranslation = matrix_identity_double4x4
	
	//	Rotate the w axis ( = axis 3 ) towards the given axis.
	switch geometryType {
	
	case .spherical:
		theTranslation[axis][axis] =  cos(θ)
		theTranslation[axis][ 3  ] = -sin(θ)
		theTranslation[ 3  ][axis] =  sin(θ)
		theTranslation[ 3  ][ 3  ] =  cos(θ)
		
	case .euclidean:
		//	Just as in the spherical and hyperbolic cases,
		//	in the Euclidean case too
		//
		//		θ = (the translation distance) / (the space radius)
		//
		//	A product like
		//
		//		         (  1     0     0    0 )
		//		(x y z ρ)(  0     1     0    0 )
		//		         (  0     0     1    0 )
		//		         ( Δx/ρ  Δy/ρ  Δz/ρ  1 )
		//
		//		where ρ = (the space radius),
		//
		//	gives the expected result.
		//
		theTranslation[axis][axis] = 1.0
		theTranslation[axis][ 3  ] = 0.0
		theTranslation[ 3  ][axis] =  θ
		theTranslation[ 3  ][ 3  ] = 1.0
		
	case .hyperbolic:
		theTranslation[axis][axis] = cosh(θ)
		theTranslation[axis][ 3  ] = sinh(θ)
		theTranslation[ 3  ][axis] = sinh(θ)
		theTranslation[ 3  ][ 3  ] = cosh(θ)
	}
	
	return theTranslation
}
	
func geodesicTranslation(
	v: SIMD3<Double>,	//	Translation vector with length in radians. That is,
						//		|v| = (the translation distance) / (the space radius)
						//	even in Euclidean geometry.
	geometryType: GeometryType
) -> simd_double4x4 {	//	dimensionless

	//	Even though the space has a radius of ρ meters,
	//	here we'll work in dimensionless radians,
	//	even in Euclidean geometry. For an explanation
	//	of radians in Euclidean geometry, see §4.1 of
	//
	//		Jeff Weeks,
	//		Body coherence in curved-space virtual reality games,
	//		Computers & Graphics 97 (2021) 28–41.
	//
	//		  https://www.sciencedirect.com/science/article/pii/S0097849321000443
	//		or
	//		  https://arxiv.org/abs/2011.00510
	//

	//	We'll compute a geodesic translation of the space
	//	moving the north pole (0,0,0,1) through θ = |v| radians
	//	in the direction (v₀, v₁, v₂, 0).
	
	//	The concept of an "angle" is most familiar in spherical geometry,
	//	but it applies equally well in hyperbolic and Euclidean geometry.
	//	In all three geometries it's the dimensionless ratio
	//
	//		    distance as measured in the space (in meters)
	//		θ = ---------------------------------------------
	//		          radius of the space (in meters)
	//
	let θ = length(v)	//	v is already in radians
	if θ < 1.0e-8 {
		return matrix_identity_double4x4
	}
	let u = v / θ	//	unit-speed velocity vector in direction of v

	//	For clarity in the documentation…
	let Δx = u.x
	let Δy = u.y
	let Δz = u.z

	//	Initialize the geometry-independent entries of the velocity matrix.
	//
	//			(  0   0   0   0 )
	//		m = (  0   0   0   0 )
	//			(  0   0   0   0 )
	//		    ( Δx  Δy  Δz   0 )
	//
	//	The geometry-dependent right-hand column will get set below.
	var m = simd_double4x4()	//	initializes to zero matrix
	m[3] = SIMD4<Double>(Δx, Δy, Δz, 0.0)
	
	let c₁: Double
	let c₂: Double
	switch geometryType {
	
	case .spherical:
		//	In the spherical case, the unit-speed velocity (Δx, Δy, Δz)
		//	defines a velocity matrix
		//
		//			(  0   0   0 -Δx )
		//		m = (  0   0   0 -Δy )
		//			(  0   0   0 -Δz )
		//		    ( Δx  Δy  Δz   0 )
		//
		//	Miraculously, m³ = -(Δx² + Δy² + Δz²)m = -m,
		//	so the following infinite series compresses
		//	to a quadratic polynomial in m
		//
		//		exp(θ m) = 1 + θ m + (θ m)²/(2!) + (θ m)³/(3!) + …
		//
		//				 = 1 + ( θ - θ³/(3!) + … ) m + ( θ²/(2!) - θ⁴/(4!) + … )m²
		//
		//				 = 1  +  sin(θ) m  +  (1 - cos(θ)) m²
		//
		
		m[0][3] = -Δx
		m[1][3] = -Δy
		m[2][3] = -Δz
		
		c₁ = sin(θ)
		c₂ = 1.0 - cos(θ)
		
	case .euclidean:
		//	In the flat case, the unit-speed velocity (Δx, Δy, Δz)
		//	defines a velocity matrix
		//
		//			(  0   0   0   0 )
		//		m = (  0   0   0   0 )
		//			(  0   0   0   0 )
		//		    ( Δx  Δy  Δz   0 )
		//
		//	Trivially m² = 0, so the infinite series for
		//
		//		exp(θ m) = 1 + θ m + (θ m)²/(2!) + (θ m)³/(3!) + …
		//
		//	compresses to the simple sum
		//
		//		exp(θ m) = 1 + θ m
		//
		
		m[0][3] = 0.0	//	redundant, but makes the meaning clear
		m[1][3] = 0.0
		m[2][3] = 0.0
		
		c₁ = θ
		c₂ = 0.0
		
	case .hyperbolic:
		//	Miraculously, m³ = (Δx² + Δy² + Δz²)m = m,
		//	so the following infinite series compresses
		//	to a quadratic polynomial in m
		//
		//		exp(θ m) = 1 + θ m + (θ m)²/(2!) + (θ m)³/(3!) + …
		//
		//				 = 1 + ( θ + θ³/(3!) + … ) m + ( θ²/(2!) + θ⁴/(4!) + … )m²
		//
		//				 = 1  +  sinh(θ) m  +  (cosh(θ) - 1) m²
		//
		
		m[0][3] = +Δx
		m[1][3] = +Δy
		m[2][3] = +Δz
		
		c₁ = sinh(θ)
		c₂ = cosh(θ) - 1.0
	}
	
	//	In all three geometries, the final
	//	translation matrix is 1 + c₁ m + c₂ m².
	//
	let theGeodesicTranslation
		= matrix_identity_double4x4
		+ c₁ * m
		+ c₂ * m * m
	
	return theGeodesicTranslation
}

func geometricInverse(
	of m: simd_double4x4,		//	dimensionless
	geometryType: GeometryType	//	Pass the geometryType explicitly, because m[3][3] could
								//		be 1.000001 or 0.999999 in the Euclidean case.
) -> simd_double4x4 {	//	dimensionless

	//	Invert a matrix in O(4), Isom(E³) or O(3,1).
	//	Work geometrically for better precision than row-reduction
	//	methods would provide. Probably better performance too.
	
	var theInverse: simd_double4x4

	switch geometryType {
	
	case .spherical:	//	m ∈ O(4)

		//	The inverse is the transpose.
		theInverse = m.transpose

	case .euclidean:	//	m ∈ Isom(E³)

		//	The upper-left 3×3 block is the transpose of the original.
		theInverse = m.transpose

		//	The right-most column is mostly zeros.
		for i in 0...2 {
			theInverse[i][3] = 0.0
		}

		//	The bottom row is the negative of a matrix product.
		for i in 0...2 {

			theInverse[3][i] = 0.0
			for j in 0...2 {
				theInverse[3][i] -= m[3][j] * m[i][j]
			}
		}

		//	The bottom right entry is 1.0, which is already included
		//	in m.transpose above, but let's set it anyhow in case m[3][3]
		//	contains some numerical error.
		theInverse[3][3] = 1.0
		
	case .hyperbolic:	//	m ∈ O(3,1)

		//	The inverse is the transpose,
		//	but with a few minus signs thrown in.
		
		theInverse = m.transpose
		
		for i in 0...2 {
			theInverse[i][3] = -theInverse[i][3]
			theInverse[3][i] = -theInverse[3][i]
		}
	}
	
	return theInverse
	
}

func applyGramSchmidt(
	to mIn: simd_double4x4,
	geometryType: GeometryType
) -> simd_double4x4 {

	//	Remove numerical errors, so the matrix stays
	//	in the required group ( O(4), Isom(E³) or O(3,1) ).

	var m = mIn

	let theMetricPair = geometryType.metricPair
	
	for i in (0...3).reversed() {	//	start with the last row,
									//	which may use a different metric

		let theMetric = theMetricPair[i == 3 ? 1 : 0]
		
		//	Normalize row i to unit length.
		//
		//		Because m is already approximately orthonormal,
		//		we know the arguments passed to the sqrt() function
		//		will always be close to 1.0, so no need to worry
		//		about checking for non-positive values.
		//
		let theLength = sqrt( dot(theMetric, m[i] * m[i]) )
		m[i] /= theLength
		
		//	Make all preceding rows orthogonal to row i.
		for j in 0 ..< i {

			let theInnerProduct = dot(theMetric, m[i] * m[j])
			
			m[j] -= theInnerProduct * m[i]
		}
	}

	return m
}

func randomQuaternion() -> simd_quatd {

	//	The 3-sphere may be parameterized using "toroidal coordinates" (χ,θ,φ)
	//
	//		x = cos(χ) cos(θ)
	//		y = cos(χ) sin(θ)
	//		z = sin(χ) cos(φ)
	//		w = sin(χ) sin(φ)
	//
	//	where
	//
	//		0 ≤ χ ≤ π/2
	//		0 ≤ θ ≤ 2π
	//		0 ≤ φ ≤ 2π
	//
	//	(For an illustration of toroidal coordinates,
	//	see Figure 1 of https://arxiv.org/abs/math/0202072 .)
	//
	//	But does this parameterization map the unit cube
	//	uniformly (that is, in a volume-preserving way)
	//	onto the 3-sphere?  The parameterization is clearly
	//	uniform in θ and φ, but it's not uniform in χ:
	//	the volume of the thickened torus bounded by
	//	χ₀ ≤ χ ≤ χ₀ + dχ depends on χ₀. Fortunately
	//	the fix is easy:  we just need to reparameterize
	//	using χ = asin(sqrt(τ)), so that
	//
	//		x = sqrt(1 - τ) cos(θ)
	//		y = sqrt(1 - τ) sin(θ)
	//		z = sqrt(  τ  ) cos(φ)
	//		w = sqrt(  τ  ) sin(φ)
	//
	//	where
	//
	//		0 ≤ τ ≤ 1
	//		0 ≤ θ ≤ 2π
	//		0 ≤ φ ≤ 2π
	//
	//	To compute the thickness dχ of a thickened torus,
	//	start with
	//
	//		χ = asin(sqrt(τ))
	//	=>
	//		sin(χ) = sqrt(τ)
	//
	//	and then take the derivative of both sides
	//	with respect to τ
	//
	//		cos(χ) dχ/dτ = (1/2) / sqrt(τ)
	//	=>
	//		dχ = dτ / (2 sqrt(τ) cos(χ))
	//		   = dτ / (2 sqrt(τ) sqrt(1 - τ))
	//
	//	The surface area of a toroidal slice is
	//
	//		A = (2π sqrt(1 - τ)) (2π sqrt(τ))
	//
	//	so the volume of a thickened slice is
	//
	//		dV = A dχ
	//		   = 2π sqrt(1 - τ) 2π sqrt(τ) dτ / (2 sqrt(τ) sqrt(1 - τ))
	//		   = 2π² dτ
	//
	//	As desired this last expression is independent of τ,
	//	thus proving that the (τ,θ,φ) coordinates parameterize
	//	the 3-sphere in a volume-preserving way. As an immediate
	//	we see that, because τ runs from 0 to 1, the hypersurface
	//	volume of the unit 3-sphere is 2π².

	let τ = Double.random(in: 0.0 ..< 1.0)
	let θ = Double.random(in: 0.0 ..< 2.0 * Double.pi)
	let φ = Double.random(in: 0.0 ..< 2.0 * Double.pi)

	let q = simd_quatd(vector: SIMD4<Double>(
		sqrt(1 - τ) * cos(θ),
		sqrt(1 - τ) * sin(θ),
		sqrt(  τ  ) * cos(φ),
		sqrt(  τ  ) * sin(φ)))
	
	return q
}

func safeAcos(
	_ x: Double
) -> Double {

	if x <= -1.0 { return .pi }
	if x >= +1.0 { return 0.0 }
	return acos(x)
}

func safeAsin(
	_ x: Double
) -> Double {

	if x <= -1.0 { return -0.5 * .pi }
	if x >= +1.0 { return +0.5 * .pi }
	return asin(x)
}

func safeAcosh(
	_ x: Double
) -> Double {

	if x <= +1.0 { return 0.0 }
	return acosh(x)
}

// MARK: -
// MARK: Map physical pose to virtual pose

func virtualPose(
	correspondingToPhysicalPose physicalPose: simd_double4x4,
	spaceGeometry: SpaceGeometry
) -> simd_double4x4 {

	//	Given a pose in (Euclidean) physical space,
	//	compute the corresponding virtual pose
	//	in the (often non-Euclidean) virtual space.
	//
	//	Speaking informally, we wrap the Euclidean space
	//	onto the non-Euclidean space in the "obvious way".
	//	The technical term for this wrapping is
	//	the "exponential map", but in my opinion that's
	//	a horribly confusing name for it.

	//	Factor the physicalPose as a rotation
	//	followed by a translation.
	
	let thePhysicalRotation = simd_double4x4(
		physicalPose[0],
		physicalPose[1],
		physicalPose[2],
		SIMD4<Double>(0.0, 0.0, 0.0, 1.0))
	
	//	The physical "space radius" is always 1 meter.
	let thePhysicalTranslationInMeters = SIMD3<Double>(
		1.0 * physicalPose[3][0],	//	(1 meter)*(m₃₀)
		1.0 * physicalPose[3][1],	//	(1 meter)*(m₃₁)
		1.0 * physicalPose[3][2])	//	(1 meter)*(m₃₂)

	//	The virtual pose's rotational part
	//	is the same as the physical pose's...
	let theVirtualRotation = thePhysicalRotation

	//	...but the translational part needs
	//	to get converted to a motion of the
	//	(often non-Euclidean) virtual space.
	let theTranslationVectorInMeters = thePhysicalTranslationInMeters
	let theTranslationVectorInRadians = theTranslationVectorInMeters
									  / spaceGeometry.spaceRadius
	let theVirtualTranslation = geodesicTranslation(
									v: theTranslationVectorInRadians,
									geometryType: spaceGeometry.geometryType)

	//	The final virtual pose is the virtual rotation
	//	followed by the virtual translation.
	let theVirtualPose
		= theVirtualTranslation	//	right-to-left matrix action
		* theVirtualRotation

	return theVirtualPose
}
