import { assert } from "../../assert.js";
import { spline3_hermite } from "./spline3_hermite.js";
import { spline3_hermite_derivative } from "./spline3_hermite_derivative.js";

/**
 * Subdivides a Hermite curve into two hermite curves.
 * The result is written in form [a_p0, a_p1, a_m0, a_m1, b_p0, b_p1, b_m0, b_m1], note the stride and offset.
 * @param {number[]} output where to write the result
 * @param {number} output_offset where to start writing
 * @param {number} output_stride what should be the step between result values, useful when working in higher dimensions. Typically, this will be 1.
 * @param {number} p0
 * @param {number} p1
 * @param {number} m0 tangent at p0
 * @param {number} m1 tangent at p1
 * @param {number} [t=0.5] where to split, normalized position, must be value between 0 and 1
 */
export function spline3_hermite_subdivide(
    output,
    output_offset,
    output_stride,
    p0, p1, m0, m1,
    t = 0.5
) {
    assert.isNonNegativeInteger(output_offset, 'output_offset');
    assert.isNonNegativeInteger(output_stride, 'output_stride');
    assert.greaterThanOrEqual(output_stride, 1, 'output_stride must be at least 1');

    // see https://github.com/krishauser/Klampt/blob/0ed16608a3eceee59d04383a17c207ebc33a399f/Python/klampt/math/spline.py#L63

    const xm = spline3_hermite(t, p0, p1, m0, m1);
    const vm = spline3_hermite_derivative(t, p0, p1, m0, m1);


    // first segment
    output[output_offset] = p0;
    output[output_offset + output_stride] = xm;
    output[output_offset + output_stride * 2] = m0 * t;
    output[output_offset + output_stride * 3] = vm * t;

    // second segment
    output[output_offset + output_stride * 4] = xm;
    output[output_offset + output_stride * 5] = p1;
    output[output_offset + output_stride * 6] = vm * (1 - t);
    output[output_offset + output_stride * 7] = m1 * (1 - t);

}