export type VectorLike = {
	x: number;
	y: number;
	z: number;
};

export type PalmAimInput = {
	handedness?: string | null;
	indexMetacarpal?: VectorLike | null;
	middleMetacarpal?: VectorLike | null;
	pinkyMetacarpal?: VectorLike | null;
	ringMetacarpal?: VectorLike | null;
	wrist?: VectorLike | null;
};

export type PalmAimRay = {
	direction: VectorLike;
	origin: VectorLike;
};

export type TimedPalmAimRay = PalmAimRay & {
	timestamp: number;
};

export type PalmAimLatch = {
	isSelecting: boolean;
	selectedRay: TimedPalmAimRay | null;
	stableRay: TimedPalmAimRay | null;
};

const MIN_AXIS_LENGTH_SQ = 0.000001;
const PALM_AIM_FORWARD_TILT_DEGREES = 40;
const PALM_SURFACE_OFFSET_METERS = 0.035;
export const DEFAULT_PALM_AIM_LATCH_MAX_AGE_MS = 300;

export function computePalmAimRay(input: PalmAimInput): PalmAimRay | null {
	const { indexMetacarpal, pinkyMetacarpal, wrist } = input;
	if (!indexMetacarpal || !pinkyMetacarpal || !wrist) {
		return null;
	}

	const knuckleCenter = averageVectors([
		indexMetacarpal,
		input.middleMetacarpal,
		input.ringMetacarpal,
		pinkyMetacarpal
	]);
	if (!knuckleCenter) {
		return null;
	}

	const fingerAxis = normalize(subtract(knuckleCenter, wrist));
	const acrossPalmAxis = normalize(subtract(pinkyMetacarpal, indexMetacarpal));
	if (!fingerAxis || !acrossPalmAxis) {
		return null;
	}

	const direction = normalize(getTiltedPalmDirection(
		getPalmNormal(fingerAxis, acrossPalmAxis, input.handedness),
		fingerAxis
	));
	if (!direction) {
		return null;
	}

	const palmCenter = lerp(wrist, knuckleCenter, 0.62);
	const origin = add(palmCenter, scale(direction, PALM_SURFACE_OFFSET_METERS));
	return { direction, origin };
}

export function createPalmAimLatch(): PalmAimLatch {
	return {
		isSelecting: false,
		selectedRay: null,
		stableRay: null
	};
}

export function recordStablePalmAimRay(
	latch: PalmAimLatch | null | undefined,
	ray: PalmAimRay,
	timestamp: number
): void {
	if (!latch) {
		return;
	}

	if (latch.isSelecting) {
		return;
	}

	latch.stableRay = withTimestamp(ray, timestamp);
}

export function beginPalmAimSelection(
	latch: PalmAimLatch | null | undefined,
	timestamp: number,
	maxAgeMs = DEFAULT_PALM_AIM_LATCH_MAX_AGE_MS
): PalmAimRay | null {
	if (!latch) {
		return null;
	}

	latch.isSelecting = true;
	const stableRay = getFreshTimedRay(latch.stableRay, timestamp, maxAgeMs);
	latch.selectedRay = stableRay ? cloneTimedRay(stableRay) : null;
	return latch.selectedRay ? clonePalmAimRay(latch.selectedRay) : null;
}

export function endPalmAimSelection(latch: PalmAimLatch | null | undefined): void {
	if (!latch) {
		return;
	}

	latch.isSelecting = false;
	latch.selectedRay = null;
}

export function getPalmAimSelectionRay(
	latch: PalmAimLatch | null | undefined,
	timestamp: number,
	maxAgeMs = DEFAULT_PALM_AIM_LATCH_MAX_AGE_MS
): PalmAimRay | null {
	if (!latch) {
		return null;
	}

	if (latch.isSelecting && latch.selectedRay) {
		return clonePalmAimRay(latch.selectedRay);
	}

	const stableRay = getFreshTimedRay(latch.stableRay, timestamp, maxAgeMs);
	return stableRay ? clonePalmAimRay(stableRay) : null;
}

function getPalmNormal(fingerAxis: VectorLike, acrossPalmAxis: VectorLike, handedness?: string | null): VectorLike {
	if (handedness === 'left') {
		return cross(acrossPalmAxis, fingerAxis);
	}

	return cross(fingerAxis, acrossPalmAxis);
}

function getTiltedPalmDirection(palmNormal: VectorLike, fingerAxis: VectorLike): VectorLike {
	const tiltRadians = (PALM_AIM_FORWARD_TILT_DEGREES * Math.PI) / 180;
	return add(
		scale(palmNormal, Math.cos(tiltRadians)),
		scale(fingerAxis, Math.sin(tiltRadians))
	);
}

function withTimestamp(ray: PalmAimRay, timestamp: number): TimedPalmAimRay {
	return {
		...clonePalmAimRay(ray),
		timestamp
	};
}

function cloneTimedRay(ray: TimedPalmAimRay): TimedPalmAimRay {
	return {
		...clonePalmAimRay(ray),
		timestamp: ray.timestamp
	};
}

function clonePalmAimRay(ray: PalmAimRay): PalmAimRay {
	return {
		direction: cloneVector(ray.direction),
		origin: cloneVector(ray.origin)
	};
}

function cloneVector(vector: VectorLike): VectorLike {
	return {
		x: vector.x,
		y: vector.y,
		z: vector.z
	};
}

function getFreshTimedRay(
	ray: TimedPalmAimRay | null,
	timestamp: number,
	maxAgeMs: number
): TimedPalmAimRay | null {
	if (!ray) {
		return null;
	}

	const ageMs = Math.max(0, timestamp - ray.timestamp);
	return ageMs <= maxAgeMs ? ray : null;
}

function averageVectors(vectors: Array<VectorLike | null | undefined>): VectorLike | null {
	const usableVectors = vectors.filter(Boolean) as VectorLike[];
	if (usableVectors.length === 0) {
		return null;
	}

	const total = usableVectors.reduce(
		(sum, vector) => add(sum, vector),
		{ x: 0, y: 0, z: 0 }
	);
	return scale(total, 1 / usableVectors.length);
}

function normalize(vector: VectorLike): VectorLike | null {
	const lengthSq = vector.x * vector.x + vector.y * vector.y + vector.z * vector.z;
	if (lengthSq < MIN_AXIS_LENGTH_SQ) {
		return null;
	}

	const length = Math.sqrt(lengthSq);
	return scale(vector, 1 / length);
}

function add(a: VectorLike, b: VectorLike): VectorLike {
	return {
		x: a.x + b.x,
		y: a.y + b.y,
		z: a.z + b.z
	};
}

function subtract(a: VectorLike, b: VectorLike): VectorLike {
	return {
		x: a.x - b.x,
		y: a.y - b.y,
		z: a.z - b.z
	};
}

function scale(vector: VectorLike, scalar: number): VectorLike {
	return {
		x: vector.x * scalar,
		y: vector.y * scalar,
		z: vector.z * scalar
	};
}

function lerp(a: VectorLike, b: VectorLike, amount: number): VectorLike {
	return {
		x: a.x + (b.x - a.x) * amount,
		y: a.y + (b.y - a.y) * amount,
		z: a.z + (b.z - a.z) * amount
	};
}

function cross(a: VectorLike, b: VectorLike): VectorLike {
	return {
		x: a.y * b.z - a.z * b.y,
		y: a.z * b.x - a.x * b.z,
		z: a.x * b.y - a.y * b.x
	};
}