package com.onthelevel.core.sensors

/**
 * Pure conversion from a rotation matrix to the device-frame world-up direction,
 * split out of AndroidSensorSource so the sensor-contract tests can prove the
 * rotation-vector path matches the gravity/accelerometer convention.
 */
object RotationVectorMath {

    /**
     * [rotationMatrix] is the row-major 3×3 device→world matrix produced by
     * SensorManager.getRotationMatrixFromVector. World-up expressed in the device
     * frame is Rᵀ·(0,0,1) — the matrix's third row. Multiplied by standard gravity
     * this matches what TYPE_GRAVITY reports for the same orientation.
     */
    fun worldUpDeviceFrame(rotationMatrix: FloatArray): Triple<Double, Double, Double> {
        require(rotationMatrix.size >= 9) { "Expected a 3x3 rotation matrix" }
        return Triple(
            rotationMatrix[6].toDouble(),
            rotationMatrix[7].toDouble(),
            rotationMatrix[8].toDouble(),
        )
    }

    const val STANDARD_GRAVITY = 9.80665
}
