/**
 * @file SplineCurve.ts
 * @description A collection of functions for working with spline curves.
 * For more information on spline curves, see:
 * getObjectsAlongPath.ts
 **/
export const cardinalSplineCurve3D = (points, tension = 0.5, numSegments = 10, close = false) => {
    // Make sure we have at least two points
    if (points.length < 2) {
        throw new Error('Cannot create a curve with fewer than two points.');
    }
    if (close) {
        points.push(points[0]);
    }
    // Calculate the step size based on the number of segments
    const step = 1 / numSegments;
    // Create an array to hold the result points
    const result = [];
    // Calculate the spline curve for each segment
    for (let i = 1; i < points.length; i++) {
        // Get the four control points for this segment
        const p0 = i > 1 ? points[i - 2] : points[0];
        const p1 = i > 0 ? points[i - 1] : points[0];
        const p2 = points[i];
        const p3 = i < points.length - 1 ? points[i + 1] : points[points.length - 1];
        // Calculate the spline curve for this segment
        for (let t = 0; t <= 1; t += step) {
            const t2 = t * t;
            const t3 = t2 * t;
            const c1 = tension * (p2[0] - p0[0]);
            const c2 = tension * (p3[0] - p1[0]);
            const x = (2 * t3 - 3 * t2 + 1) * p1[0] + (t3 - 2 * t2 + t) * c1 + (t3 - t2) * c2 + (-2 * t3 + 3 * t2) * p2[0];
            const c3 = tension * (p2[1] - p0[1]);
            const c4 = tension * (p3[1] - p1[1]);
            const y = (2 * t3 - 3 * t2 + 1) * p1[1] + (t3 - 2 * t2 + t) * c3 + (t3 - t2) * c4 + (-2 * t3 + 3 * t2) * p2[1];
            const c5 = tension * (p2[2] - p0[2]);
            const c6 = tension * (p3[2] - p1[2]);
            const z = (2 * t3 - 3 * t2 + 1) * p1[2] + (t3 - 2 * t2 + t) * c5 + (t3 - t2) * c6 + (-2 * t3 + 3 * t2) * p2[2];
            result.push([x, y, z]);
        }
    }
    return result;
};
export const getPointOnCardinalSplineCurve = (curve, progress) => {
    if (curve.length < 2) {
        throw new Error('Cannot interpolate on a curve with fewer than two points.');
    }
    // Ensure that progress is within the valid range of 0 to 1
    progress = Math.max(0, Math.min(1, progress));
    // Handle the case where progress is equal to 1
    if (progress === 0) {
        return curve[0];
    }
    else if (progress === 1) {
        return curve[curve.length - 1];
    }
    // Calculate the total length of the curve
    const lengths = [0];
    let totalLength = 0;
    for (let i = 1; i < curve.length; i++) {
        const p1 = curve[i - 1];
        const p2 = curve[i];
        const dx = p2[0] - p1[0];
        const dy = p2[1] - p1[1];
        const dz = p2[2] - p1[2];
        const length = Math.sqrt(dx ** 2 + dy ** 2 + dz ** 2);
        totalLength += length;
        lengths.push(totalLength);
    }
    // Find the segment that corresponds to the specified progress value
    const targetLength = progress * totalLength;
    let segment = 0;
    while (segment < lengths.length - 1 && lengths[segment + 1] <= targetLength) {
        segment++;
    }
    // Interpolate along the segment to find the point at the specified progress value
    const segmentStart = curve[segment];
    const segmentEnd = curve[segment + 1];
    const segmentLength = lengths[segment + 1] - lengths[segment];
    const segmentProgress = (targetLength - lengths[segment]) / segmentLength;
    const x = segmentStart[0] + (segmentEnd[0] - segmentStart[0]) * segmentProgress;
    const y = segmentStart[1] + (segmentEnd[1] - segmentStart[1]) * segmentProgress;
    const z = segmentStart[2] + (segmentEnd[2] - segmentStart[2]) * segmentProgress;
    return [x, y, z];
};
export const getRotationBetweenPoints = (start, end) => {
    const dx = end[0] - start[0];
    const dy = end[1] - start[1];
    const dz = end[2] - start[2];
    const distance = Math.sqrt(dx ** 2 + dy ** 2 + dz ** 2);
    // Calculate yaw angle (rotation around y-axis)
    const yaw = (Math.atan2(dx, dy) * -180) / Math.PI;
    // Calculate pitch angle (rotation around x-axis)
    let pitch;
    if (yaw > 0) {
        pitch = (Math.atan2(dz, Math.sqrt(dx ** 2 + dy ** 2)) * 180) / Math.PI;
    }
    else {
        pitch = (Math.atan2(dz, Math.sqrt(dx ** 2 + dy ** 2)) * -180) / Math.PI;
    }
    // ALT 1
    let roll;
    if (yaw > 0) {
        roll = (Math.atan2(start[1] * dz - end[1] * dz, end[0] * dz - start[0] * dz) * 180) / Math.PI;
    }
    else {
        roll = (Math.atan2(start[1] * dz - end[1] * dz, end[0] * dz - start[0] * dz) * -180) / Math.PI;
    }
    // ALT 2
    // Calculate roll angle (rotation around z-axis)
    // const cosYaw = Math.cos((yaw * Math.PI) / -180);
    // const sinYaw = Math.sin((yaw * Math.PI) / -180);
    // const cosPitch = Math.cos((pitch * Math.PI) / 180);
    // const sinPitch = Math.sin((pitch * Math.PI) / 180);
    // const x = sinYaw * cosPitch;
    // const y = -cosYaw * cosPitch;
    // const roll =
    //   (Math.atan2(start[2] * x - end[2] * y, end[1] * x - start[1] * y) * -180) /
    //   Math.PI;
    return [roll, pitch, yaw];
};
//# sourceMappingURL=SplineCurve.js.map