/**
 * Robot transmitter: claps the two antennas together on schedule so each
 * onset produces an audible click. Consumes the same onset schedule from
 * wire.js that the speaker Synth uses, so robot and device speak one wire code.
 *
 * ── Antenna order: a bug worth remembering ───────────────────────────────
 * Both the Python and JS SDKs order antennas as [right, left] (the init pose
 * is [-0.1745, +0.1745] = [right, left]). When calibrating I drove the robot
 * from a Python script and wrote `antennas=[a, b]` thinking index 0 was the
 * LEFT antenna — it is actually the RIGHT. So the motion I recorded clean
 * decodes from was: RIGHT antenna slams (0 → +54°), LEFT antenna held (-39°).
 * The first version of this file called `setAntennasDeg(right, left)` with the
 * fixed value first and the moving value second, which slammed the LEFT
 * antenna instead — the mirror image (still collided + clicked, but the wrong
 * antenna moved). Fixed here: the RIGHT antenna (first arg) is the slammer, the
 * LEFT (second arg) is held, matching the validated motion exactly.
 *
 * Physics (from marionette/tests/test_antenna_collision.py): low-PID antennas
 * stall at contact (no damage) and make a crisp click. All geometry/timing is
 * configurable so it can be calibrated on hardware without touching logic.
 */

export const DEFAULT_TAP_PROFILE = Object.freeze({
    heldDeg: -39, // LEFT antenna, held still (≈ -0.68 rad)
    slamRestDeg: 0, // RIGHT antenna resting position
    collisionDeg: 54, // RIGHT antenna commanded past contact (~34°) so it stalls + clicks
    approachMs: 80, // ramp rest → collision
    holdMs: 70, // dwell at collision (stall against the held antenna)
    returnMs: 80, // ramp back to rest
    rateHz: 50, // command rate
    leadMs: 0, // advance whole timeline to offset command→sound latency
    settleMs: 500, // quiet hold after the last clap before resolving (anti-ring)
    prerollMs: 500, // gentle move into the rest pose before the first clap
});

export class RobotTapper {
    constructor(reachy, profile = {}) {
        this.reachy = reachy;
        this.p = { ...DEFAULT_TAP_PROFILE, ...profile };
        this._timer = null;
    }

    /** Slamming (RIGHT) antenna angle (deg) at time `t` ms within the windows. */
    _slamAngleAt(t, onsets) {
        const { slamRestDeg, collisionDeg, approachMs, holdMs } = this.p;
        for (const T of onsets) {
            const a0 = T - approachMs; // start approaching
            const hEnd = T + holdMs; // end of hold
            const rEnd = hEnd + this.p.returnMs; // back to rest
            if (t < a0 || t > rEnd) continue;
            if (t <= T) {
                const f = approachMs > 0 ? (t - a0) / approachMs : 1;
                return slamRestDeg + (collisionDeg - slamRestDeg) * easeIn(f);
            }
            if (t <= hEnd) return collisionDeg;
            const f = (t - hEnd) / this.p.returnMs;
            return collisionDeg + (slamRestDeg - collisionDeg) * easeOut(f);
        }
        return slamRestDeg;
    }

    _send(slam, held) {
        // setAntennasDeg(right, left): RIGHT is the slammer, LEFT is held.
        try { this.reachy.setAntennasDeg(slam, held); } catch { /* transient send errors are non-fatal */ }
    }

    /**
     * Run the clap timeline.
     * @param {number[]} onsetsMs
     * @param {object} [o]
     * @param {(i:number)=>void} [o.onTap] fires near each impact (UI highlight)
     * @param {AbortSignal} [o.signal]
     * @returns {Promise<void>} resolves when the sequence completes/aborts
     */
    tap(onsetsMs, { onTap, signal } = {}) {
        const preroll = this.p.prerollMs;
        // Shift the schedule after the preroll, and apply the latency lead.
        const onsets = onsetsMs.map((t) => t + preroll - this.p.leadMs);
        const tapMarks = onsetsMs.map((t) => t + preroll); // for UI highlight
        const period = Math.max(10, Math.round(1000 / this.p.rateHz));
        const endMs = (onsets.length ? onsets[onsets.length - 1] : preroll)
            + this.p.holdMs + this.p.returnMs + this.p.settleMs;

        return new Promise((resolve) => {
            const t0 = performance.now();
            let nextTapIdx = 0;
            const finish = () => {
                clearInterval(this._timer);
                this._timer = null;
                this._send(this.p.slamRestDeg, this.p.heldDeg); // settle at rest
                resolve();
            };
            signal?.addEventListener("abort", finish, { once: true });

            this._timer = setInterval(() => {
                const t = performance.now() - t0;
                if (t > endMs || signal?.aborted) return finish();
                if (t < preroll) {
                    // Ease the held (LEFT) antenna into place; slammer waits at rest.
                    const f = preroll > 0 ? t / preroll : 1;
                    this._send(this.p.slamRestDeg, this.p.heldDeg * easeOut(f));
                } else {
                    this._send(this._slamAngleAt(t, onsets), this.p.heldDeg);
                }
                while (nextTapIdx < tapMarks.length && t >= tapMarks[nextTapIdx]) {
                    onTap?.(nextTapIdx);
                    nextTapIdx += 1;
                }
            }, period);
        });
    }

    /** One calibration clap right now (no schedule). */
    async testClap() {
        await this.tap([0]);
    }

    stop() {
        if (this._timer) {
            clearInterval(this._timer);
            this._timer = null;
        }
        this._send(this.p.slamRestDeg, this.p.heldDeg);
    }
}

function easeIn(x) {
    const c = Math.min(1, Math.max(0, x));
    return c * c; // accelerate into the impact
}
function easeOut(x) {
    const c = Math.min(1, Math.max(0, x));
    return 1 - (1 - c) * (1 - c);
}