package com.onthelevel.core.sensors

import kotlin.math.hypot

/** Shared two-axis motion gate for calibration, guidance, and Audio Assist. */
class SettlingDetector(
    private val enterRateDegreesPerSecond: Double = ENTER_RATE_DEGREES_PER_SECOND,
    private val exitRateDegreesPerSecond: Double = EXIT_RATE_DEGREES_PER_SECOND,
    private val settledDwellMillis: Long = SETTLED_DWELL_MILLIS,
) {
    data class Result(val isSettling: Boolean, val movementRateDegreesPerSecond: Double)

    private var lastPitchDegrees: Double? = null
    private var lastRollDegrees: Double? = null
    private var lastTimestampNanos: Long? = null
    private var settledSinceMillis: Long? = null
    private var isSettling = true

    fun update(pitchDegrees: Double, rollDegrees: Double, timestampNanos: Long): Result {
        val previousPitch = lastPitchDegrees
        val previousRoll = lastRollDegrees
        val previousTimestamp = lastTimestampNanos
        lastPitchDegrees = pitchDegrees
        lastRollDegrees = rollDegrees
        lastTimestampNanos = timestampNanos

        if (previousPitch == null || previousRoll == null || previousTimestamp == null) {
            return Result(isSettling = true, movementRateDegreesPerSecond = 0.0)
        }
        val dtSeconds = (timestampNanos - previousTimestamp) / 1e9
        if (dtSeconds <= 0.0) {
            reset()
            return Result(isSettling = true, movementRateDegreesPerSecond = 0.0)
        }

        val rate = hypot(pitchDegrees - previousPitch, rollDegrees - previousRoll) / dtSeconds
        val nowMillis = timestampNanos / 1_000_000
        when {
            rate > enterRateDegreesPerSecond -> {
                isSettling = true
                settledSinceMillis = null
            }
            isSettling && rate > exitRateDegreesPerSecond -> {
                // A mid-band hand nudge is not enough to leave Settling, but it is
                // enough to prove the signal has not been continuously quiet for
                // the required dwell window.
                settledSinceMillis = null
            }
            isSettling && rate <= exitRateDegreesPerSecond -> {
                val since = settledSinceMillis ?: nowMillis.also { settledSinceMillis = it }
                if (nowMillis - since >= settledDwellMillis) isSettling = false
            }
        }
        return Result(isSettling = isSettling, movementRateDegreesPerSecond = rate)
    }

    fun reset() {
        lastPitchDegrees = null
        lastRollDegrees = null
        lastTimestampNanos = null
        settledSinceMillis = null
        isSettling = true
    }

    companion object {
        const val ENTER_RATE_DEGREES_PER_SECOND = 1.0
        const val EXIT_RATE_DEGREES_PER_SECOND = 0.3
        const val SETTLED_DWELL_MILLIS = 500L
    }
}
