package com.onthelevel.core.sensors

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

/**
 * Pins the GravitySample contract: every sensor path yields device-frame WORLD-UP,
 * with flat-screen-up = (0, 0, +g).
 *
 * Per the Android sensor docs (TYPE_ACCELEROMETER), a device stationary flat on a
 * table reads +9.81 on Z: "the acceleration of the device (0 m/s²) minus the force
 * of gravity (−9.81 m/s²)". TYPE_GRAVITY shares that convention (it is the isolated
 * gravity component of the same signal), so both pass-through paths already match.
 * These tests prove the rotation-vector conversion produces the identical vector,
 * so NO sign adjustment is applied to any path.
 */
class SensorContractTest {

    private val g = RotationVectorMath.STANDARD_GRAVITY

    /** Device→world rotation matrix for a device pitched up by [degrees] about X. */
    private fun deviceToWorldPitch(degrees: Double): FloatArray {
        val r = Math.toRadians(degrees)
        return floatArrayOf(
            1f, 0f, 0f,
            0f, cos(r).toFloat(), (-sin(r)).toFloat(),
            0f, sin(r).toFloat(), cos(r).toFloat(),
        )
    }

    @Test
    fun `flat device - rotation path matches the documented gravity sensor output`() {
        val identity = floatArrayOf(1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 1f)
        val (x, y, z) = RotationVectorMath.worldUpDeviceFrame(identity)
        // Documented TYPE_GRAVITY / TYPE_ACCELEROMETER flat output: (0, 0, +9.81).
        assertEquals(0.0, x * g, 1e-6)
        assertEquals(0.0, y * g, 1e-6)
        assertEquals(g, z * g, 1e-6)
    }

    @Test
    fun `pitched device - rotation path matches the analytic gravity vector`() {
        val theta = 10.0
        val (x, y, z) = RotationVectorMath.worldUpDeviceFrame(deviceToWorldPitch(theta))
        val fromRotation = GravitySample(x * g, y * g, z * g, 0)

        // The gravity-sensor path for the same physical orientation:
        val r = Math.toRadians(theta)
        val fromGravitySensor = GravitySample(0.0, g * sin(r), g * cos(r), 0)

        assertEquals(fromGravitySensor.x, fromRotation.x, 1e-6)
        assertEquals(fromGravitySensor.y, fromRotation.y, 1e-6)
        assertEquals(fromGravitySensor.z, fromRotation.z, 1e-6)

        // And both derive the same pitch through the measurement math:
        assertEquals(
            OrientationMath.surfacePitchDegrees(fromGravitySensor),
            OrientationMath.surfacePitchDegrees(fromRotation),
            1e-6,
        )
    }
}
