package com.onthelevel.core.sensors

import org.junit.Assert.assertEquals
import org.junit.Assert.assertNull
import org.junit.Test

class StableSurfaceCaptureTest {

    private fun sample(
        pitch: Double,
        roll: Double,
        timestampMillis: Long,
        tilt: Double = 1.0,
        settling: Boolean = false,
    ) = SurfaceCalibrationSample(pitch, roll, tilt, timestampMillis * 1_000_000, settling)

    @Test
    fun `captures the mean of a continuous settled one point five second window`() {
        val capture = StableSurfaceCapture()

        assertNull(capture.add(sample(0.2, -0.1, 0)))
        assertNull(capture.add(sample(0.4, -0.3, 750)))
        val result = capture.add(sample(0.6, -0.5, 1_500))

        requireNotNull(result)
        assertEquals(0.4, result.pitchDegrees, 1e-9)
        assertEquals(-0.3, result.rollDegrees, 1e-9)
    }

    @Test
    fun `motion or excessive tilt resets the capture window`() {
        val capture = StableSurfaceCapture()

        assertNull(capture.add(sample(0.2, 0.0, 0)))
        assertNull(capture.add(sample(0.2, 0.0, 750, settling = true)))
        assertNull(capture.add(sample(0.2, 0.0, 1_000)))
        assertNull(capture.add(sample(0.2, 0.0, 1_500, tilt = 5.1)))
        assertNull(capture.add(sample(0.2, 0.0, 1_750)))
        assertNull(capture.add(sample(0.2, 0.0, 2_500)))
        requireNotNull(capture.add(sample(0.2, 0.0, 3_250)))
    }
}
