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

import simd
import ARKit	//	for HandAnchor

enum PlayersHand: CaseIterable {
	case left
	case right
}

struct VirtualJointPoses {

	let playersHand: PlayersHand
	let spaceGeometry: SpaceGeometry

	//	We'll compute the virtualPoses in whatever coordinate system
	//	the virtualHeadsetPose is given in.
	let virtualPoses: [HandSkeleton.JointName: simd_double4x4]


	init?(
		for handAnchor: HandAnchor,
		spaceGeometry: SpaceGeometry,
		physicalHeadsetPose: simd_double4x4,	//	in physical world coordinates
		virtualHeadsetPose: simd_double4x4?
	) {
	
		guard let theVirtualHeadsetPose = virtualHeadsetPose
		else { return nil }

		if !handAnchor.isTracked {
			return nil
		}

		guard let theSkeleton = handAnchor.handSkeleton
		else { return nil }

		switch handAnchor.chirality {
		case .left:  playersHand = .left
		case .right: playersHand = .right
		}
		self.spaceGeometry = spaceGeometry
		
		let theHandPoseInWorldCoordinates
			= convertFloat4x4toDouble4x4(handAnchor.originFromAnchorTransform)

		virtualPoses = HandSkeleton.JointName.allCases.reduce(into: [:]) {
			virtualPosesDictionary, jointName in
			
			let theJoint = theSkeleton.joint(jointName)
			
			//	If theJoint.isTracked is true, it means that the Vision Pro
			//	sees the joint and ARKit reports a reasonably precise pose.
			//	If theJoint.isTracked is false, it means that the Vision Pro
			//	doesn't currently see the joint and ARKit makes an educated guess
			//	about where it thinks the joint is.
			//	We're happy either way.
		
			//	anchorFromJointTransform() returns a pose with the z-axis
			//	pointing to the left (for the player's left and right hands both).
			let thePhysicalJointPoseInHandCoordinates = convertFloat4x4toDouble4x4(
				theJoint.anchorFromJointTransform)
			
			let thePhysicalJointPoseInWorldCoordinates
				= theHandPoseInWorldCoordinates
				* thePhysicalJointPoseInHandCoordinates

			let thePhysicalJointPoseInHeadsetCoordinates
				= physicalHeadsetPose.inverse
				* thePhysicalJointPoseInWorldCoordinates

			let theVirtualJointPoseInHeadsetCoordinates
				= virtualPose(
					correspondingToPhysicalPose: thePhysicalJointPoseInHeadsetCoordinates,
					spaceGeometry: spaceGeometry)
			
			let theVirtualJointPoseInWorldCoordinates
				= theVirtualHeadsetPose
				* theVirtualJointPoseInHeadsetCoordinates

			virtualPosesDictionary[jointName] = theVirtualJointPoseInWorldCoordinates
		}
	}
	
	subscript(jointName: HandSkeleton.JointName) -> simd_double4x4 {
	
		//	Our init() returns nil if it can't set a virtual pose
		//	for each JointName, so this Dictionary lookup should never fail.
		
		return virtualPoses[jointName]
				?? matrix_identity_double4x4
	}

	func position(
		_ jointName: HandSkeleton.JointName
	) -> SIMD4<Double> {
	
		let thePose = self[jointName]
		let thePosition = thePose * spaceGeometry.northPole
		
		return thePosition
	}
}


let ggVRHandMeshIndices: [UInt16] = [
	 0,  1,  6,
	 6,  1,  2,
	 6,  2,  7,
	 7,  2,  3,
	 7,  3,  4,
	 7,  4,  8,
	 8,  4,  9,
	 4,  5,  9,
	 0,  6, 15,
	15,  6, 10,
	15, 10, 11,
	15, 11, 17,
	15, 17, 16,
	18, 17, 11,
	18, 11, 12,
	18, 12, 13,
	18, 13, 19,
	19, 13, 20,
	13, 14, 20,
	21, 15, 16,
	 0, 15, 25,
	27, 26, 25,
	27, 25, 15,
	27, 15, 21,
	27, 21, 22,
	27, 22, 28,
	22, 29, 28,
	22, 23, 29,
	23, 24, 29,
	29, 24, 30,
	25, 26, 31,
	 0, 25, 35,
	31, 35, 25,
	31, 37, 35,
	31, 38, 37,
	31, 32, 38,
	38, 32, 33,
	38, 33, 39,
	39, 33, 40,
	33, 34, 40,
	35, 37, 36,
	35, 36, 41,
	 0, 35, 45,
	35, 46, 45,
	35, 41, 46,
	46, 41, 42,
	46, 42, 47,
	42, 48, 47,
	42, 43, 48,
	43, 44, 48,
	48, 44, 49
]

func ggvrHandMeshVertices(
	virtualJointPoses: VirtualJointPoses	//	in world coordinates
) -> [SIMD4<Double>] {	//	Returns [SIMD4<Double>] for a mesh in whatever
						//		coordinates the virtualJointPoses are using.
	
	let theSpaceGeometry = virtualJointPoses.spaceGeometry
	let theGeometryType = theSpaceGeometry.geometryType
	let theSpaceRadius = theSpaceGeometry.spaceRadius
	
	//	We want the left and right hand's meshes to be mirror images
	//	of each other, so they can share the same ggVRHandMeshIndices.
	//	But the poses reported by anchorFromJointTransform above
	//	always use a joint coordinate system whose z axis points
	//	to the left, no matter which hand we're considering.
	//	So let's define a function that consistently shifts
	//	a joint's centerpoint inward towards the thumb or
	//	outward towards the pinky.
	func shiftJoint(
		_ jointName: HandSkeleton.JointName,
		Δz: Double,			//	thumbward offset in meters
		Δy: Double = 0.0,	//	back-of-hand-ward offset in meters
		Δx: Double = 0.0	//	wristward offset in meters
	) -> SIMD4<Double> {	//	returns shifted point, still in world coordinates

		var ΔzInRadians = Δz / theSpaceRadius
		var ΔyInRadians = Δy / theSpaceRadius
		var ΔxInRadians = Δx / theSpaceRadius

		//	Apple's documentation doesn't say what coordinate system
		//	anchorFromJointTransform uses. It turns out to be kinda weird
		//	and needlessly inconsistent between the left and right hand.
		//	I can see why they didn't want to document it. Let's hope
		//	they clean it up in the future.
		//
		//		Update: Apple's page
		//
		//			https://developer.apple.com/documentation/arkit/rigging-a-model-for-motion-capture
		//
		//		says to imagine the person in the T-pose (shown on that page).
		//		That explains my earlier observations that for a right hand,
		//		the x axis points wristward, the y axis points back-of-hand-ward
		//		and the z axis points thumbward.
		//
		//		As for the left hand, the Vision Pro Engineer's reply at
		//
		//			https://forums.developer.apple.com/forums/thread/752795
		//
		//		seems to imply (but doesn't quite say directly) that
		//		the idea is that you can start by visualizing an intended
		//		left hand as a right hand, apply some rotations, and
		//		then mirror it when you map it into place. The only possible
		//		reason I can think of for why they invert all three coordinates
		//		rather than none of them is that once they map that hand
		//		into its final left-handed position, they want to end up
		//		with a chirality-preserving transformation (det > 0)
		//		rather than a mirrored transformation (det < 0).
		//		Even still, by inverting coordinates it seems like
		//		they'd end up rotating stuff in the wrong direction.
		//		Oh well, somebody must have had some reason for these conventions.
		//
		if virtualJointPoses.playersHand == .left {
			ΔzInRadians = -ΔzInRadians
			ΔyInRadians = -ΔyInRadians
			ΔxInRadians = -ΔxInRadians
		}

		let theShift = geodesicTranslation(
			v: SIMD3<Double>(ΔxInRadians, ΔyInRadians, ΔzInRadians),
			geometryType: theGeometryType)
		let theShiftedPositionInJointCoordinates
			= theShift * theSpaceGeometry.northPole
		let theVirtualJointPose = virtualJointPoses[jointName]
		let theShiftedPositionInWorldCoordinates = theVirtualJointPose
												 * theShiftedPositionInJointCoordinates

		return theShiftedPositionInWorldCoordinates
	}

	let theDistanceFunction = theSpaceGeometry.spaceDistanceFunction
	
	//	Base the virtual finger widths on the separation
	//	between the player's knuckles. Note that the finger diameter
	//	is slightly less than the knuckle separation.
	let δ₁ = 0.5 * 0.9 * theDistanceFunction(
			virtualJointPoses.position(.indexFingerKnuckle),
			virtualJointPoses.position(.middleFingerKnuckle) )
	let δ₂ = 0.5 * 1.0 * theDistanceFunction(
			virtualJointPoses.position(.middleFingerKnuckle),
			virtualJointPoses.position(.ringFingerKnuckle) )
	let δ₃ = 0.5 * 0.9 * theDistanceFunction(
			virtualJointPoses.position(.ringFingerKnuckle),
			virtualJointPoses.position(.littleFingerKnuckle) )

	//	Even when the types are all 100% consistent,
	//	the Swift compiler tends to choke on any sort of non-trivial expression
	//	and times out with the error message
	//
	//		The compiler is unable to type-check this expression
	//		in reasonable time; try breaking up the expression
	//		into distinct sub-expressions
	//
	//	To keep it happy, let's predefine the "web" vertices
	//	(meaning the web of the player's hand, not the internet!)
	//	and also the "intra-knuckle" vertices. This also keeps
	//	the code cleaner, so is probably a good idea in any case.
	//
	//	Hmmm... now it's willing to compile those expressions
	//	without timing out, but it's still super slow
	//	(86ms+110ms, I'm not sure why it gives two separate errors).
	//	Explicitly casting the individual terms to SIMD4<Double>
	//	reduces the time to 13ms+7ms. That's much better,
	//	although I still don't understand why it should take
	//	even that long. BTW, casting 0.25 to Double(0.25)
	//	offers little further improvement.
	//
	//	For an illuminating discussion of this slow type-checking problem,
	//	see
	//		https://danielchasehooper.com/posts/why-swift-is-slow/
	//
	let webIndexMiddle = theSpaceGeometry.normalizeToSpaceRadius(
		  SIMD4<Double>( 0.25 * virtualJointPoses.position(.indexFingerKnuckle)				)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.middleFingerKnuckle)			)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.indexFingerIntermediateBase)	)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.middleFingerIntermediateBase)	)
	)
	let webMiddleRing = theSpaceGeometry.normalizeToSpaceRadius(
		  SIMD4<Double>( 0.25 * virtualJointPoses.position(.middleFingerKnuckle)			)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.ringFingerKnuckle)				)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.middleFingerIntermediateBase)	)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.ringFingerIntermediateBase)		)
	)
	let webRingLittle = theSpaceGeometry.normalizeToSpaceRadius(
		  SIMD4<Double>( 0.25 * virtualJointPoses.position(.ringFingerKnuckle)				)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.littleFingerKnuckle)			)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.ringFingerIntermediateBase)		)
		+ SIMD4<Double>( 0.25 * virtualJointPoses.position(.littleFingerIntermediateBase)	)
	)

	let intraknuckleIndexMiddle = theSpaceGeometry.normalizeToSpaceRadius(
		0.5 * ( virtualJointPoses.position(.indexFingerKnuckle )
			  + virtualJointPoses.position(.middleFingerKnuckle) )
	)
	let intraknuckleMiddleRing = theSpaceGeometry.normalizeToSpaceRadius(
		0.5 * ( virtualJointPoses.position(.middleFingerKnuckle)
			  + virtualJointPoses.position(.ringFingerKnuckle  ) )
	)
	let intraknuckleRingLittle = theSpaceGeometry.normalizeToSpaceRadius(
		0.5 * ( virtualJointPoses.position(.ringFingerKnuckle  )
			  + virtualJointPoses.position(.littleFingerKnuckle) )
	)
	
	let ε₀ = -0.003	//	fingertip's longways offset
	let ε₁ =  0.003	//	fingertip's inward offset

	let theVertices = [
		shiftJoint(.wrist,							Δz: -0.02, Δx: -0.02),			//	 0
		shiftJoint(.thumbKnuckle,					Δz:  0.0),						//	 1
		shiftJoint(.thumbIntermediateBase,			Δz:  0.014),					//	 2
		shiftJoint(.thumbIntermediateTip,			Δz:  0.012),					//	 3
		shiftJoint(.thumbTip,						Δz:  0.008),					//	 4
		shiftJoint(.thumbTip,						Δz:  0.008 - ε₁, Δx: ε₀),		//	 5
		shiftJoint(.thumbIntermediateBase,			Δz: -0.018),					//	 6
		shiftJoint(.thumbIntermediateTip,			Δz: -0.012),					//	 7
		shiftJoint(.thumbTip,						Δz: -0.008),					//	 8
		shiftJoint(.thumbTip,						Δz: -0.008 + ε₁, Δx: ε₀),		//	 9
		shiftJoint(.indexFingerKnuckle,				Δz:  0.01),						//	10
		shiftJoint(.indexFingerIntermediateBase,	Δz:  0.8 * δ₁),					//	11
		shiftJoint(.indexFingerIntermediateTip,		Δz:  0.7 * δ₁),					//	12
		shiftJoint(.indexFingerTip,					Δz:  0.6 * δ₁),					//	13
		shiftJoint(.indexFingerTip,					Δz:  0.6 * δ₁ - ε₁, Δx: ε₀),		//	14
		intraknuckleIndexMiddle,													//	15
		webIndexMiddle,																//	16
		shiftJoint(.indexFingerIntermediateBase,	Δz: -0.8 * δ₁),					//	17
		shiftJoint(.indexFingerIntermediateTip,		Δz: -0.7 * δ₁),					//	18
		shiftJoint(.indexFingerTip,					Δz: -0.6 * δ₁),					//	19
		shiftJoint(.indexFingerTip,					Δz: -0.6 * δ₁ + ε₁, Δx: ε₀),		//	20
		shiftJoint(.middleFingerIntermediateBase,	Δz:  0.8 * δ₁),					//	21
		shiftJoint(.middleFingerIntermediateTip,	Δz:  0.7 * δ₁),					//	22
		shiftJoint(.middleFingerTip,				Δz:  0.6 * δ₁),					//	23
		shiftJoint(.middleFingerTip,				Δz:  0.6 * δ₁ - ε₁, Δx: ε₀),		//	24
		intraknuckleMiddleRing,														//	25
		webMiddleRing,																//	26
		shiftJoint(.middleFingerIntermediateBase,	Δz: -0.8 * δ₂),					//	27
		shiftJoint(.middleFingerIntermediateTip,	Δz: -0.7 * δ₂),					//	28
		shiftJoint(.middleFingerTip,				Δz: -0.6 * δ₂),					//	29
		shiftJoint(.middleFingerTip,				Δz: -0.6 * δ₂ + ε₁, Δx: ε₀),		//	30
		shiftJoint(.ringFingerIntermediateBase,		Δz:  0.8 * δ₂),					//	31
		shiftJoint(.ringFingerIntermediateTip,		Δz:  0.7 * δ₂),					//	32
		shiftJoint(.ringFingerTip,					Δz:  0.6 * δ₂),					//	33
		shiftJoint(.ringFingerTip,					Δz:  0.6 * δ₂ - ε₁, Δx: ε₀),		//	34
		intraknuckleRingLittle,														//	35
		webRingLittle,																//	36
		shiftJoint(.ringFingerIntermediateBase,		Δz: -0.8 * δ₃),					//	37
		shiftJoint(.ringFingerIntermediateTip,		Δz: -0.7 * δ₃),					//	38
		shiftJoint(.ringFingerTip,					Δz: -0.6 * δ₃),					//	39
		shiftJoint(.ringFingerTip,					Δz: -0.6 * δ₃ + ε₁, Δx: ε₀),		//	40
		shiftJoint(.littleFingerIntermediateBase,	Δz:  0.8 * δ₃),					//	41
		shiftJoint(.littleFingerIntermediateTip,	Δz:  0.7 * δ₃),					//	42
		shiftJoint(.littleFingerTip,				Δz:  0.6 * δ₃),					//	43
		shiftJoint(.littleFingerTip,				Δz:  0.6 * δ₃ - ε₁, Δx: ε₀),		//	44
		shiftJoint(.littleFingerKnuckle,			Δz: -1.0 * δ₃),					//	45
		shiftJoint(.littleFingerIntermediateBase,	Δz: -0.8 * δ₃),					//	46
		shiftJoint(.littleFingerIntermediateTip,	Δz: -0.7 * δ₃),					//	47
		shiftJoint(.littleFingerTip,				Δz: -0.6 * δ₃),					//	48
		shiftJoint(.littleFingerTip,				Δz: -0.6 * δ₃ + ε₁, Δx: ε₀)		//	49	
	]
		
	return theVertices
}


enum PlayerHandedness: String, CaseIterable, Identifiable {
	
	case leftHanded
	case rightHanded

	var id: String { self.rawValue }
	
	var hand: String {
	
		switch self {
		case .leftHanded:	return "left"
		case .rightHanded:	return "right"
		}
	}
	
	mutating func toggle() {
	
		switch self {
		case .leftHanded:	self = .rightHanded
		case .rightHanded:	self = .leftHanded
		}
	}
}
