import { Quaternion, Vector3 } from 'three';
export function getFrameTransformationQuaternion() {
    // MATCHING DART IMPLEMENTATION EXACTLY:
    // 1: Rotate -90° around Z-axis
    const rotZ = new Quaternion().setFromAxisAngle(new Vector3(0, 0, 1), -Math.PI / 2);
    // 2: Rotate 90° around X-axis
    const rotX = new Quaternion().setFromAxisAngle(new Vector3(1, 0, 0), Math.PI / 2);
    // Combine: Apply rotX first, then rotZ
    return rotZ.multiply(rotX);
}
/**
 * Calculates the delta position in Robot Frame
 */
export function calculatePositionTarget(currentControllerPos, referenceControllerPos, robotReferencePos, qTransform, scaleFactor) {
    // 1. Get delta in XR space (Meters)
    const deltaXR = currentControllerPos.clone().sub(referenceControllerPos);
    // 2. Convert to Robot Frame
    const deltaRobot = deltaXR.clone().applyQuaternion(qTransform);
    // 3. Scale (Meters -> Millimeters) and Apply
    const scaleMM = scaleFactor * 1000;
    return {
        x: robotReferencePos.x + deltaRobot.x * scaleMM,
        y: robotReferencePos.y + deltaRobot.y * scaleMM,
        z: robotReferencePos.z + deltaRobot.z * scaleMM,
    };
}
