export type Pt = [number, number];

export function cubicBezierCommand(pts: Pt[], p1: Pt, p2: Pt, p3: Pt){
    const p0 = pts[pts.length - 1];
    if (!p0) return pts;

    const [b1p0, b1p1, b1p2] = getMidPoints(p0, p1, p2, p3);
    const [b2p0, b2p1] = getMidPoints(b1p0, b1p1, b1p2);
    const [b3p0] = getMidPoints(b2p0, b2p1);

    const a1 = Math.atan2(b3p0[1] - p0[1], b3p0[0] - p0[0]);
    const a2 = Math.atan2(p3[1] - b3p0[1], p3[0] - b3p0[0]);
    const d = a2 - a1 - Math.round((a2 - a1) / Math.PI / 2) * Math.PI * 2;
    if (isNaN(d)) {
        console.error('NaN found', { d, a2, a1, p0, p1, p2, p3 });
        return pts;
    }

    const d03 = sqdist(p0, p3);
    if (d * d * d03 < Math.PI * Math.PI / 18 / 18) {
        pts.push(p3);
        return pts;
    }

    cubicBezierCommand(pts, b1p0, b2p0, b3p0);
    pts.push(b3p0);
    cubicBezierCommand(pts, b2p1, b1p2, p3);

    return pts;
}

function sqdist(pt: Pt, to: Pt) {
    const x = pt[0] - to[0];
    const y = pt[1] - to[1];
    return x * x + y * y;
}

function getMidPoints(...pts: Pt[]){
    const mps = [] as typeof pts;

    for(let i = 1; i < pts.length; i ++){
        mps[i-1] = [
            (pts[i][0] + pts[i-1][0])/2,
            (pts[i][1] + pts[i-1][1])/2,
        ];
    }

    return mps;
}