package com.onthelevel.core.sensors

/** Uncalibrated, smoothed Surface sample used by guided calibration. */
data class SurfaceCalibrationSample(
    val pitchDegrees: Double,
    val rollDegrees: Double,
    val tiltDegrees: Double,
    val timestampNanos: Long,
    val isSettling: Boolean,
)

/** Mean of one settled 1.5-second calibration capture. */
data class CapturedSurfaceSample(val pitchDegrees: Double, val rollDegrees: Double)

/** Collects a continuous settled capture on a roughly-level surface. */
class StableSurfaceCapture(
    private val durationMillis: Long = CAPTURE_DURATION_MILLIS,
    private val maximumTiltDegrees: Double = MAXIMUM_CALIBRATION_TILT_DEGREES,
) {
    private var startedAtNanos: Long? = null
    private var pitchTotal = 0.0
    private var rollTotal = 0.0
    private var count = 0

    fun add(sample: SurfaceCalibrationSample): CapturedSurfaceSample? {
        if (sample.isSettling || sample.tiltDegrees > maximumTiltDegrees) {
            reset()
            return null
        }
        val startedAt = startedAtNanos ?: sample.timestampNanos.also { startedAtNanos = it }
        pitchTotal += sample.pitchDegrees
        rollTotal += sample.rollDegrees
        count += 1
        if ((sample.timestampNanos - startedAt) / 1_000_000 < durationMillis || count < 2) return null
        return CapturedSurfaceSample(pitchTotal / count, rollTotal / count)
    }

    fun reset() {
        startedAtNanos = null
        pitchTotal = 0.0
        rollTotal = 0.0
        count = 0
    }

    companion object {
        const val CAPTURE_DURATION_MILLIS = 1_500L
        const val MAXIMUM_CALIBRATION_TILT_DEGREES = 5.0
    }
}
