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

import ARKit

extension VirtualJointPoses {	//	in virtual table coordinates

	//	Compute the point in virtual table coordinates where we
	//	want the cue stick's axis to pass through the player's hand.
	func cueStickGripPoint() -> SIMD4<Double> {
		
		//	When I write the following as a single expression,
		//	the stupid Swift compiler says
		//
		//		The compiler is unable to type-check this expression
		//		in reasonable time; try breaking up the expression
		//		into distinct sub-expressions.
		//
		//	Sigh.
		//
		let term0 = 0.2 * position(.thumbIntermediateBase)
		let term1 = 0.3 * position(.indexFingerKnuckle)
		let term2 = 0.5 * position(.indexFingerIntermediateTip)
		let theGripPoint
			= spaceGeometry.normalizeToSpaceRadius(term0 + term1 + term2)
		
		return theGripPoint
	}

	//	Compute the point in virtual table coordinates where we
	//	want the cue stick's axis to pass through the player's hand.
	func cueStickBridgePoint() -> SIMD4<Double> {
		
		//	See comments in cueStickGripPoint() above.
		let term0 = 0.4 * position(.thumbTip)
		let term1 = 0.6 * position(.indexFingerKnuckle)
		let theBridgePoint
			= spaceGeometry.normalizeToSpaceRadius(term0 + term1)
		
		return theBridgePoint
	}

	//	Compute the point in virtual table coordinates where
	//	an in-hand cue ball should sit in the player's hand.
	func cueBallInHandPoint() -> SIMD4<Double> {

		let term0 = 0.5 * position(.thumbTip)
		let term1 = 0.5 * position(.indexFingerTip)

		let theCueBallCenter
			= spaceGeometry.normalizeToSpaceRadius(term0 + term1)

		return theCueBallCenter
	}
}

extension NEBRenderer {

	func getVirtualCueStickPose(
		gripPoint: SIMD4<Double>?,		//	in virtual table coordinates
		bridgePoint: SIMD4<Double>?,	//	in virtual table coordinates
		cueBallIsInHand: Bool
	) -> simd_double4x4?
		//	Returns the cue stick's pose in table coordinates
		//	in the (typically non-Euclidean) virtual space,
		//	placed at the cue stick's tip, looking forwards (negative Z direction),
		//	with the stick extending backwards (positive Z direction).
	{

		//	When the player is holding the cue ball in hand,
		//	don't try to show the cue stick.
		if cueBallIsInHand {
			return nil
		}
		
		//	If ARKit isn't currently tracking both the player's hands,
		//	don't try to show the cue stick.
		guard let theGripPoint = gripPoint,
			  let theBridgePoint = bridgePoint
		else {
			return nil
		}

		let theGripToBridgeDistance = spaceGeometry.spaceDistanceFunction(
														theGripPoint,
														theBridgePoint)

		//	Are the player's hands too close together for good stability?
		//	(We need to protect against division-by-zero in any case.)
		if theGripToBridgeDistance < 0.1 {
			return nil
		}

		//	The player's hands are sufficiently far apart,
		//	so we may place the cue stick with its axis
		//	passing through theGripPoint and theBridgePoint.

		//	Start with the identity pose, which puts
		//	the cue stick's tip at the center of the table
		//	and also keeps its y-axis pointing up,
		//	which could be useful for lighting.
		
		let theIdentityPose = matrix_identity_double4x4

		//	Translate theIdentityPose to put
		//	the cue stick's tip at theGripPoint.
		
		let theOriginToGripDistance
			= spaceGeometry.spaceDistanceFunction(
								spaceGeometry.northPole,
								theGripPoint)
		let theRawGripDirection = SIMD3<Double>(
									theGripPoint.x,
									theGripPoint.y,
									theGripPoint.z)
		let theRawGripDistance = length(theRawGripDirection)
		let theGripTranslationVectorInMeters =
			theRawGripDistance > 0.001 ?	//	> 1 mm ?
				(theOriginToGripDistance / theRawGripDistance) * theRawGripDirection :
				SIMD3<Double>.zero
		let theGripTranslationVectorInRadians = theGripTranslationVectorInMeters
											  /	spaceGeometry.spaceRadius
		let theGripTranslation = geodesicTranslation(
									v: theGripTranslationVectorInRadians,
									geometryType: spaceGeometry.geometryType)
		let theTipAtGripPose = theGripTranslation * theIdentityPose
		
		
		//	Rotate theTipAtGripPose so the cue stick
		//	points towards theBridgePoint.
		
		let theBridgeInGripCoordinates = theGripTranslation.inverse * theBridgePoint
		let theBridgeInGripCoordinatesXYZ = SIMD3<Double>(
												theBridgeInGripCoordinates.x,
												theBridgeInGripCoordinates.y,
												theBridgeInGripCoordinates.z)

		//	We've already checked that
		//
		//		theGripToBridgeDistance ≥ 0.1
		//
		//	so the only way that theBridgeInGripCoordinatesXYZ
		//	could be the zero vector would be if the player were
		//	in a very small spherical space with one of his/her hands
		//	sitting antipodally opposite the other. Let's make sure
		//	that's not the case before we try to normalize
		//	theBridgeInGripCoordinatesXYZ to unit length.
		//
		if length(theBridgeInGripCoordinatesXYZ) < 0.01 {
			return nil
		}
		let theGripToBridgeDirection = normalize(
										SIMD3<Double>(
											theBridgeInGripCoordinates.x,
											theBridgeInGripCoordinates.y,
											theBridgeInGripCoordinates.z))

		//	simd_quatd(from:to:) handles antipodal vectors OK.
		//	Of the circle of acceptable quaternions, it returns an arbitary one of them.
		//	It does, however, require that the input vectors have unit length.
		//
		let theRotationAsQuaternion = simd_quatd(
										from: SIMD3<Double>(0.0, 0.0, -1.0), // forward direction
										to: theGripToBridgeDirection)	//	unit vector
		let theRotationAsMatrix = simd_double4x4(theRotationAsQuaternion)
		let theRotatedPose = theTipAtGripPose * theRotationAsMatrix	//	right-to-left matrix action
		
		//	Translate the cue stick along itself,
		//	to put its tip at the desired position.
		let theTranslationDistanceInMeters = -cueStickTipToGripDistance
		let theTranslationDistanceInRadians = theTranslationDistanceInMeters
												/ spaceGeometry.spaceRadius
		let theParallelTranslation = axisAlignedTranslation(
										axis: 2,
										distanceInRadians: theTranslationDistanceInRadians,
										geometryType: spaceGeometry.geometryType)
		let theVirtualCueStickPose = theRotatedPose * theParallelTranslation

		return theVirtualCueStickPose
	}

	func getVirtualInHandCueBallPose(
		cueBallCenterPoint: SIMD4<Double>?	//	in virtual table coordinates
	) -> simd_double4x4? {	//	Returns the cue ball's pose in virtual table coordinates,
							//	placed in the player's dominant hand. (It's a bit easier
							//	for the player to place the cue ball with their
							//	non-dominant hand, which then becomes the bridge hand,
							//	but that also makes it easier to accidentally mis-cue.)

		guard let theCueBallCenterPoint = cueBallCenterPoint
		else { return nil }
		
		let theOriginToCueBallDistance = spaceGeometry.spaceDistanceFunction(
											spaceGeometry.northPole,
											theCueBallCenterPoint)

		let theRawCueBallDirection = SIMD3<Double>(
										theCueBallCenterPoint.x,
										theCueBallCenterPoint.y,
										theCueBallCenterPoint.z)
		let theRawLength = length(theRawCueBallDirection)
		let theCueBallTranslationVectorInMeters =
			theRawLength > 0.001 ?	//	> 1 mm ?
				(theOriginToCueBallDistance / theRawLength) * theRawCueBallDirection :
				SIMD3<Double>.zero	//	good for north pole; south pole shouldn't occur
		let theCueBallTranslationVectorInRadians = theCueBallTranslationVectorInMeters
												 /	spaceGeometry.spaceRadius

		let theCueBallTranslation = geodesicTranslation(
										v: theCueBallTranslationVectorInRadians,
										geometryType: spaceGeometry.geometryType)

		return theCueBallTranslation
	}
}
