package com.onthelevel.core.sensors

import org.junit.Assert.assertFalse
import org.junit.Assert.assertTrue
import org.junit.Test

class LockDetectorTest {

    private fun detector() = LockDetector(
        enterThresholdDegrees = 0.2,
        exitThresholdDegrees = 0.35,
        dwellMillis = 175,
        settledDwellMillis = 400,
        slowRateThresholdDegPerSec = 0.3,
        feedbackDebounceMillis = 3_000,
    )

    private val slow = 0.1 // °/s, below the slow-rate threshold
    private val fast = 2.0 // °/s, a real center crossing

    @Test
    fun `easing slowly into center locks after the short dwell`() {
        val d = detector()
        assertFalse(d.update(0.1, slow, 0).isLocked)
        assertFalse(d.update(0.1, slow, 174).isLocked)
        val r = d.update(0.1, slow, 175)
        assertTrue(r.isLocked)
        assertTrue(r.fireFeedback)
    }

    @Test
    fun `flying across center never locks, however long it stays in the zone`() {
        val d = detector()
        // In the zone the whole time, but moving fast: the dwell never accumulates.
        for (t in 0..2_000 step 20) assertFalse(d.update(0.05, fast, t.toLong()).isLocked)
    }

    @Test
    fun `intermediate speed does not accumulate lock time`() {
        val d = detector()
        val intermediate = 0.5 // between slow (0.3) and a crossing
        for (t in 0..2_000 step 20) assertFalse(d.update(0.1, intermediate, t.toLong()).isLocked)
    }

    @Test
    fun `slowing to a stop inside the zone starts a fresh confirmation dwell`() {
        val d = detector()
        // In the zone but moving fast until t=300: no accumulation yet.
        d.update(0.1, fast, 0)
        d.update(0.1, fast, 300)
        // Now slow down: dwell starts fresh here, so it must NOT be locked at +100ms...
        assertFalse(d.update(0.1, slow, 320).isLocked)
        assertFalse(d.update(0.1, slow, 420).isLocked)
        // ...but locks 175ms after slowing.
        assertTrue(d.update(0.1, slow, 495).isLocked)
    }

    @Test
    fun `leaving the zone before dwell completes resets the timer`() {
        val d = detector()
        d.update(0.1, slow, 0)
        d.update(0.5, slow, 100) // bounced out of the zone
        d.update(0.1, slow, 150) // back in — dwell restarts
        assertFalse(d.update(0.1, slow, 300).isLocked) // only 150ms since re-entry
        assertTrue(d.update(0.1, slow, 325).isLocked)
    }

    @Test
    fun `hysteresis holds the lock between enter and exit thresholds`() {
        val d = detector()
        d.update(0.1, slow, 0)
        assertTrue(d.update(0.1, slow, 200).isLocked)
        // 0.3° is above enter (0.2) but below exit (0.35): still locked (velocity irrelevant once locked).
        assertTrue(d.update(0.3, fast, 300).isLocked)
        // 0.4° exceeds the exit threshold: unlocked.
        assertFalse(d.update(0.4, slow, 400).isLocked)
    }

    @Test
    fun `feedback fires once per lock and respects the debounce window`() {
        val d = detector()
        d.update(0.1, slow, 0)
        assertTrue(d.update(0.1, slow, 200).fireFeedback)
        assertFalse(d.update(0.1, slow, 300).fireFeedback) // still locked, no re-fire

        // Rock out and back quickly: re-lock at ~1500ms is inside the 3s debounce.
        d.update(0.5, fast, 900)
        d.update(0.1, slow, 1000)
        val relock = d.update(0.1, slow, 1500)
        assertTrue(relock.isLocked)
        assertFalse(relock.fireFeedback)

        // A re-lock after the debounce window fires again.
        d.update(0.5, fast, 4000)
        d.update(0.1, slow, 4100)
        assertTrue(d.update(0.1, slow, 4400).fireFeedback)
    }

    @Test
    fun `without a velocity signal it uses the fixed settled dwell (Edge)`() {
        val d = detector()
        assertFalse(d.update(0.1, null, 0).isLocked)
        assertFalse(d.update(0.1, null, 399).isLocked) // 175 has passed, but the no-rate path needs 400
        assertTrue(d.update(0.1, null, 400).isLocked)
    }

    @Test
    fun `negative readings lock on magnitude`() {
        val d = detector()
        d.update(-0.1, slow, 0)
        assertTrue(d.update(-0.1, slow, 200).isLocked)
    }
}
