package com.onthelevel.core.sensors

import kotlin.math.hypot

/**
 * Guards calibration persistence against a bad calibration.
 */
object SurfaceCalibrationValidation {
    const val MAXIMUM_BIAS_MAGNITUDE_DEGREES = 3.0
    const val PAIR_AGREEMENT_TOLERANCE_DEGREES = 0.4

    /** The combined two-axis correction must be small; a large mean means something moved. */
    fun isAcceptable(calibration: SurfaceCalibration): Boolean =
        hypot(calibration.pitchBiasDegrees, calibration.rollBiasDegrees) <= MAXIMUM_BIAS_MAGNITUDE_DEGREES

    /**
     * A 4-point set (0°/90°/180°/270°) contains TWO independent 180°-flip bias
     * estimates: the (0,180) pair and the (90,270) pair. A clean turn makes them
     * agree; a malformed rotation makes them diverge even when the overall mean stays
     * plausibly small — which the magnitude guard alone cannot detect. Reject when the
     * two estimates disagree beyond tolerance.
     *
     * The disagreement scales with the surface's true tilt, so this is naturally
     * lenient near level (where a turn error barely matters) and strict on a steeper
     * permitted surface (where it matters most). Sets that aren't the 4-point shape
     * carry no cross-check and pass here; the magnitude guard still applies.
     */
    fun isRotationConsistent(samples: List<CapturedSurfaceSample>): Boolean {
        if (samples.size != 4) return true
        val pitchA = (samples[0].pitchDegrees + samples[2].pitchDegrees) / 2.0
        val rollA = (samples[0].rollDegrees + samples[2].rollDegrees) / 2.0
        val pitchB = (samples[1].pitchDegrees + samples[3].pitchDegrees) / 2.0
        val rollB = (samples[1].rollDegrees + samples[3].rollDegrees) / 2.0
        return hypot(pitchA - pitchB, rollA - rollB) <= PAIR_AGREEMENT_TOLERANCE_DEGREES
    }
}
