package com.onthelevel.core.sensors

import org.junit.Assert.assertEquals
import org.junit.Assert.assertTrue
import org.junit.Test
import kotlin.math.cos
import kotlin.math.sin

class OrientationMathTest {

    private val g = 9.80665

    private fun flat() = GravitySample(0.0, 0.0, g, 0)

    /** Device tilted so the top edge is raised by [degrees] (rotation about the X axis). */
    private fun pitched(degrees: Double): GravitySample {
        val r = Math.toRadians(degrees)
        return GravitySample(0.0, g * sin(r), g * cos(r), 0)
    }

    /** Device tilted so the right edge is raised by [degrees] (rotation about the Y axis). */
    private fun rolled(degrees: Double): GravitySample {
        val r = Math.toRadians(degrees)
        return GravitySample(g * sin(r), 0.0, g * cos(r), 0)
    }

    /** Device standing on its long edge, tipped in the wall plane by [degrees]. */
    private fun onEdge(tipDegrees: Double): GravitySample {
        val r = Math.toRadians(tipDegrees)
        return GravitySample(g * cos(r), g * sin(r), 0.0, 0)
    }

    @Test
    fun `flat device reads zero everywhere in surface mode`() {
        assertEquals(0.0, OrientationMath.surfaceTiltMagnitudeDegrees(flat()), 1e-9)
        assertEquals(0.0, OrientationMath.surfacePitchDegrees(flat()), 1e-9)
        assertEquals(0.0, OrientationMath.surfaceRollDegrees(flat()), 1e-9)
    }

    @Test
    fun `pitch recovers the applied rotation with correct sign`() {
        assertEquals(1.0, OrientationMath.surfacePitchDegrees(pitched(1.0)), 1e-9)
        assertEquals(-2.5, OrientationMath.surfacePitchDegrees(pitched(-2.5)), 1e-9)
        assertEquals(0.0, OrientationMath.surfaceRollDegrees(pitched(1.0)), 1e-9)
    }

    @Test
    fun `roll recovers the applied rotation with correct sign`() {
        assertEquals(3.0, OrientationMath.surfaceRollDegrees(rolled(3.0)), 1e-9)
        assertEquals(0.0, OrientationMath.surfacePitchDegrees(rolled(3.0)), 1e-9)
    }

    @Test
    fun `tilt magnitude matches single-axis rotations`() {
        assertEquals(1.0, OrientationMath.surfaceTiltMagnitudeDegrees(pitched(1.0)), 1e-9)
        assertEquals(4.0, OrientationMath.surfaceTiltMagnitudeDegrees(rolled(4.0)), 1e-9)
    }

    @Test
    fun `surface magnitude remains authoritative through screen down`() {
        assertEquals(120.0, OrientationMath.surfaceTiltMagnitudeDegrees(pitched(120.0)), 1e-9)
    }

    @Test
    fun `edge mode reads zero when the long edge is horizontal`() {
        assertEquals(0.0, OrientationMath.edgeLevelDegrees(onEdge(0.0)), 1e-9)
        assertEquals(0.0, OrientationMath.edgePlumbLeanDegrees(onEdge(0.0)), 1e-9)
    }

    @Test
    fun `edge mode recovers in-plane tip with correct sign`() {
        assertEquals(1.5, OrientationMath.edgeLevelDegrees(onEdge(1.5)), 1e-9)
        assertEquals(-2.0, OrientationMath.edgeLevelDegrees(onEdge(-2.0)), 1e-9)
    }

    @Test
    fun `percent grade is tan-based and capped to infinity near vertical`() {
        assertEquals(0.0, OrientationMath.percentGrade(0.0), 1e-9)
        assertEquals(100.0, OrientationMath.percentGrade(45.0), 1e-6)
        assertTrue(OrientationMath.percentGrade(89.6).isInfinite())
        assertTrue(OrientationMath.percentGrade(-89.6) == Double.NEGATIVE_INFINITY)
    }

    @Test
    fun `angle between identical directions is zero`() {
        assertEquals(0.0, OrientationMath.angleBetweenDegrees(pitched(10.0), pitched(10.0)), 1e-9)
    }

    @Test
    fun `angle between same-axis tilts is their difference`() {
        assertEquals(15.0, OrientationMath.angleBetweenDegrees(pitched(10.0), pitched(25.0)), 1e-9)
    }

    @Test
    fun `angle between cross-axis tilts is directional, not a magnitude difference`() {
        // Zeroed at 10° pitch, moved to 10° roll: magnitudes are equal (difference 0),
        // but the device genuinely rotated acos(cos²10°) ≈ 14.1°.
        val expected = Math.toDegrees(
            kotlin.math.acos(cos(Math.toRadians(10.0)) * cos(Math.toRadians(10.0))),
        )
        assertEquals(expected, OrientationMath.angleBetweenDegrees(pitched(10.0), rolled(10.0)), 1e-9)
        assertTrue(expected > 14.0)
    }

    @Test
    fun `placement validity matches the selected mode's geometry`() {
        assertTrue(OrientationMath.isPlacementValid(flat(), LevelMode.SURFACE))
        assertTrue(!OrientationMath.isPlacementValid(flat(), LevelMode.EDGE))
        assertTrue(OrientationMath.isPlacementValid(onEdge(0.0), LevelMode.EDGE))
        assertTrue(!OrientationMath.isPlacementValid(onEdge(0.0), LevelMode.SURFACE))
    }

    @Test
    fun `zero vector does not produce NaN`() {
        val zero = GravitySample(0.0, 0.0, 0.0, 0)
        assertEquals(0.0, OrientationMath.surfaceTiltMagnitudeDegrees(zero), 1e-9)
        assertEquals(0.0, OrientationMath.edgeLevelDegrees(zero), 1e-9)
    }
}
