package com.onthelevel.core.sensors

import com.onthelevel.core.sensors.Vec3Math.Vec3
import org.junit.Assert.assertEquals
import org.junit.Test
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.tan

class TwoSampleCalibrationTest {

    private val g = 9.80665

    private fun sample(v: Vec3) = GravitySample(v.x, v.y, v.z, 0)

    private fun pitched(degrees: Double): GravitySample {
        val r = Math.toRadians(degrees)
        return GravitySample(0.0, g * sin(r), g * cos(r), 0)
    }

    @Test
    fun `flip cancels true tilt and isolates device bias`() {
        // Surface truly tilted 0.5°, device bias +0.3°:
        val reading1 = 0.5 + 0.3   // as placed
        val reading2 = -0.5 + 0.3  // after 180° rotation about the surface normal
        val bias = TwoSampleCalibration.deriveBiasDegrees(reading1, reading2)
        assertEquals(0.3, bias, 1e-9)
    }

    @Test
    fun `surface calibration derives both axes independently`() {
        val cal = TwoSampleCalibration.deriveSurface(
            pitch1 = 0.8, roll1 = -0.1,
            pitch2 = -0.2, roll2 = 0.5,
        )
        assertEquals(0.3, cal.pitchBiasDegrees, 1e-9)
        assertEquals(0.2, cal.rollBiasDegrees, 1e-9)
    }

    @Test
    fun `surface correction zeroes a biased level placement - vector space`() {
        val cal = SurfaceCalibration(pitchBiasDegrees = 0.4, rollBiasDegrees = -0.3)
        // The reference direction: what a biased device measures on a TRULY level surface.
        val measuredAtLevel = sample(
            Vec3Math.normalize(
                Vec3(
                    tan(Math.toRadians(cal.rollBiasDegrees)),
                    tan(Math.toRadians(cal.pitchBiasDegrees)),
                    1.0,
                ),
            ).let { Vec3(it.x * g, it.y * g, it.z * g) },
        )
        val corrected = cal.apply(measuredAtLevel)
        assertEquals(0.0, OrientationMath.surfacePitchDegrees(corrected), 1e-9)
        assertEquals(0.0, OrientationMath.surfaceRollDegrees(corrected), 1e-9)
        assertEquals(0.0, OrientationMath.surfaceTiltMagnitudeDegrees(corrected), 1e-6)
    }

    @Test
    fun `surface correction is exact away from zero - same axis`() {
        // Misalignment purely about X by 0.5°: a true pitch of 30° measures as 30.5°.
        val cal = SurfaceCalibration(pitchBiasDegrees = 0.5, rollBiasDegrees = 0.0)
        val measured = pitched(30.5)
        val corrected = cal.apply(measured)
        assertEquals(30.0, OrientationMath.surfacePitchDegrees(corrected), 1e-9)
        assertEquals(30.0, OrientationMath.surfaceTiltMagnitudeDegrees(corrected), 1e-9)
    }

    @Test
    fun `surface correction is exact away from zero - cross axis`() {
        // General misalignment: pitch bias 0.4°, roll bias 0.3°. Build the measured
        // sample by applying the INVERSE of the correction rotation to the true
        // 30°-pitched gravity, then verify the correction recovers it exactly.
        val cal = SurfaceCalibration(pitchBiasDegrees = 0.4, rollBiasDegrees = 0.3)
        val reference = Vec3Math.normalize(
            Vec3(
                tan(Math.toRadians(cal.rollBiasDegrees)),
                tan(Math.toRadians(cal.pitchBiasDegrees)),
                1.0,
            ),
        )
        val axis = Vec3Math.normalize(Vec3Math.cross(reference, Vec3Math.WORLD_UP_FLAT))
        val angle = acos(Vec3Math.dot(reference, Vec3Math.WORLD_UP_FLAT).coerceIn(-1.0, 1.0))
        val true30 = pitched(30.0)
        val measured = sample(Vec3Math.rotate(Vec3(true30.x, true30.y, true30.z), axis, -angle))

        val corrected = cal.apply(measured)
        assertEquals(30.0, OrientationMath.surfacePitchDegrees(corrected), 1e-9)
        assertEquals(0.0, OrientationMath.surfaceRollDegrees(corrected), 1e-9)
        assertEquals(30.0, OrientationMath.surfaceTiltMagnitudeDegrees(corrected), 1e-9)
    }

    @Test
    fun `edge correction is exact away from zero and leaves lean untouched`() {
        // Misalignment about Z by 0.3° on the positive-X edge; true in-plane tip 10°.
        val bias = 0.3
        val cal = EdgeCalibration(levelBiasDegrees = bias, calibratedOnPositiveXEdge = true)
        fun onEdgeMeasured(tipDegrees: Double): GravitySample {
            val r = Math.toRadians(tipDegrees + bias) // Rz misalignment adds directly in-plane
            return GravitySample(g * cos(r), g * sin(r), 0.0, 0)
        }

        assertEquals(
            0.0,
            OrientationMath.edgeLevelDegrees(cal.apply(onEdgeMeasured(0.0))),
            1e-9,
        )
        assertEquals(
            10.0,
            OrientationMath.edgeLevelDegrees(cal.apply(onEdgeMeasured(10.0))),
            1e-9,
        )
        // Lean (device Z component) is untouched by the Z-rotation correction.
        val leaned = GravitySample(g * 0.99, 0.05, g * 0.1, 0)
        assertEquals(
            OrientationMath.edgePlumbLeanDegrees(leaned),
            OrientationMath.edgePlumbLeanDegrees(cal.apply(leaned)),
            1e-9,
        )
    }

    @Test
    fun `edge polarity flips the correction direction`() {
        val positive = EdgeCalibration(0.3, calibratedOnPositiveXEdge = true)
        val negative = EdgeCalibration(0.3, calibratedOnPositiveXEdge = false)
        val s = GravitySample(g, 0.1, 0.0, 0)
        val correctedPositive = positive.apply(s)
        val correctedNegative = negative.apply(s)
        // Opposite rotation directions about Z:
        assertEquals(
            OrientationMath.edgeLevelDegrees(correctedPositive) - OrientationMath.edgeLevelDegrees(s),
            -(OrientationMath.edgeLevelDegrees(correctedNegative) - OrientationMath.edgeLevelDegrees(s)),
            1e-6,
        )
    }

    @Test
    fun `surface and edge calibrations are independent and applied per mode`() {
        val surface = TwoSampleCalibration.deriveSurface(1.0, 1.0, 0.0, 0.0)
        val edge = EdgeCalibration.NONE
        // An edge sample passed through the untouched edge calibration is unchanged,
        // regardless of surface calibration state (AUDIT.md finding 3).
        val edgeSample = GravitySample(g, 0.12, 0.0, 0)
        assertEquals(edgeSample, edge.apply(edgeSample))
        assertEquals(0.5, surface.pitchBiasDegrees, 1e-9)
    }

    @Test
    fun `two-sample averaging matches the pairwise derivation`() {
        val fromSamples = TwoSampleCalibration.deriveSurfaceFromSamples(
            listOf(CapturedSurfaceSample(0.8, -0.1), CapturedSurfaceSample(-0.2, 0.5)),
        )
        val pairwise = TwoSampleCalibration.deriveSurface(0.8, -0.1, -0.2, 0.5)
        assertEquals(pairwise.pitchBiasDegrees, fromSamples.pitchBiasDegrees, 1e-9)
        assertEquals(pairwise.rollBiasDegrees, fromSamples.rollBiasDegrees, 1e-9)
    }

    @Test
    fun `four-point averaging recovers bias from a symmetric rotation set`() {
        // Bias is fixed in the device frame; the true surface tilt appears as a
        // symmetric set summing to zero over 0/90/180/270 (pitch: +t,0,-t,0 ;
        // roll: 0,+t,0,-t). The mean must therefore be exactly the bias.
        val biasPitch = 0.4
        val biasRoll = -0.3
        val t = 1.7
        val samples = listOf(
            CapturedSurfaceSample(biasPitch + t, biasRoll),
            CapturedSurfaceSample(biasPitch, biasRoll + t),
            CapturedSurfaceSample(biasPitch - t, biasRoll),
            CapturedSurfaceSample(biasPitch, biasRoll - t),
        )
        val cal = TwoSampleCalibration.deriveSurfaceFromSamples(samples)
        assertEquals(biasPitch, cal.pitchBiasDegrees, 1e-9)
        assertEquals(biasRoll, cal.rollBiasDegrees, 1e-9)
    }
}
