// Input detector — turns the periodic state event into discrete
// Direction events. Mirrors the Python InputDetector state machine:
//
//   NEUTRAL → TILTED (held for HOLD_MS) → REGISTERED → back to NEUTRAL
//
// Thresholds match the Python constants. The state event arrives at
// whatever rate main.js sets via robot._stateRefreshInterval; we bump
// that to ~20 Hz to keep input latency reasonable.

import { Direction, flatMatrixToRpyDeg } from "./util.js";

const TILT_THRESHOLD = 12;        // degrees
const NEUTRAL_THRESHOLD = 8;      // degrees
const BODY_YAW_THRESHOLD = 20;    // degrees
const ANTENNA_THRESHOLD = 0.3;    // radians
const HOLD_MS = 100;
const COOLDOWN_MS = 100;

export function createInputDetector() {
    let difficulty = 1;
    let phase = "NEUTRAL";        // NEUTRAL | TILTED | REGISTERED
    let currentDir = null;
    let tiltStart = 0;
    let lastInput = 0;

    function setDifficulty(d) { difficulty = d; }
    function reset() {
        phase = "NEUTRAL";
        currentDir = null;
        tiltStart = 0;
    }

    function detectDirection(roll, pitch, bodyYawDeg, antennas) {
        // Antennas (difficulty 3+). antennas = [rightRad, leftRad].
        if (difficulty >= 3 && Array.isArray(antennas)) {
            if (antennas[1] >  ANTENNA_THRESHOLD) return Direction.LEFT_ANTENNA_LEFT;
            if (antennas[1] < -ANTENNA_THRESHOLD) return Direction.LEFT_ANTENNA_RIGHT;
            if (antennas[0] >  ANTENNA_THRESHOLD) return Direction.RIGHT_ANTENNA_LEFT;
            if (antennas[0] < -ANTENNA_THRESHOLD) return Direction.RIGHT_ANTENNA_RIGHT;
        }
        // Body yaw (difficulty 2+).
        if (difficulty >= 2) {
            if (bodyYawDeg >  BODY_YAW_THRESHOLD) return Direction.BODY_LEFT;
            if (bodyYawDeg < -BODY_YAW_THRESHOLD) return Direction.BODY_RIGHT;
        }
        // Head tilts.
        if (Math.abs(roll) < NEUTRAL_THRESHOLD && Math.abs(pitch) < NEUTRAL_THRESHOLD) {
            return null;
        }
        if (Math.abs(pitch) > Math.abs(roll)) {
            if (pitch < -TILT_THRESHOLD) return Direction.UP;
            if (pitch >  TILT_THRESHOLD) return Direction.DOWN;
        } else {
            if (roll >  TILT_THRESHOLD) return Direction.LEFT;
            if (roll < -TILT_THRESHOLD) return Direction.RIGHT;
        }
        return null;
    }

    // Returns a Direction (string) if one was registered on this update,
    // otherwise null. Call from the SDK "state" event handler.
    function update(rs) {
        if (!rs) return null;
        const { roll, pitch } = flatMatrixToRpyDeg(rs.head);
        const bodyYawDeg = ((rs.body_yaw ?? 0) * 180) / Math.PI;
        const antennas = Array.isArray(rs.antennas) ? rs.antennas : [0, 0];
        const detected = detectDirection(roll, pitch, bodyYawDeg, antennas);
        const now = performance.now();
        if (phase === "NEUTRAL") {
            if (detected != null) {
                phase = "TILTED";
                currentDir = detected;
                tiltStart = now;
            }
        } else if (phase === "TILTED") {
            if (detected !== currentDir) {
                phase = "NEUTRAL"; currentDir = null; tiltStart = 0;
            } else if (now - tiltStart >= HOLD_MS &&
                       now - lastInput >= COOLDOWN_MS) {
                phase = "REGISTERED";
                lastInput = now;
                return currentDir;
            }
        } else if (phase === "REGISTERED") {
            if (detected == null) { phase = "NEUTRAL"; currentDir = null; }
        }
        return null;
    }

    return { setDifficulty, reset, update };
}

// Debounced antenna-as-button: fires once per push, re-arms once the
// antenna returns close to rest. Used for cycling difficulty (left
// antenna) and starting the game (right antenna).
export function createAntennaButton({
    triggerThreshold = 0.4,
    releaseThreshold = 0.1,
} = {}) {
    let armed = true;
    return {
        // Returns true exactly once per push.
        update(angleRad) {
            const a = Math.abs(angleRad);
            if (a > triggerThreshold && armed) {
                armed = false;
                return true;
            }
            if (a < releaseThreshold) armed = true;
            return false;
        },
    };
}