package com.onthelevel.core.sensors

import kotlin.math.abs
import kotlin.math.acos
import kotlin.math.asin
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sqrt
import kotlin.math.tan

/**
 * Pure measurement math. No Android types — everything here is unit-testable
 * with synthetic and recorded gravity vectors (BRIEF.md §Testability).
 *
 * Angle sign conventions (documented so calibration and UI agree):
 *  - Surface pitch: positive when the TOP edge of the device is higher.
 *  - Surface roll:  positive when the RIGHT edge of the device is higher.
 *  - Edge level:    positive when the end the device Y axis points toward is higher.
 */
object OrientationMath {

    /** Angle between gravity and the screen normal — the single honest "how far off flat" number. */
    fun surfaceTiltMagnitudeDegrees(g: GravitySample): Double {
        val n = norm(g)
        if (n == 0.0) return 0.0
        return Math.toDegrees(acos((g.z / n).coerceIn(-1.0, 1.0)))
    }

    fun surfacePitchDegrees(g: GravitySample): Double =
        Math.toDegrees(atan2(g.y, g.z))

    fun surfaceRollDegrees(g: GravitySample): Double =
        Math.toDegrees(atan2(g.x, g.z))

    /**
     * Edge mode: device standing on a long edge (gravity mostly along ±x).
     * The reading is the deviation of the resting edge from horizontal —
     * the component of gravity along the device Y axis.
     */
    fun edgeLevelDegrees(g: GravitySample): Double {
        val n = norm(g)
        if (n == 0.0) return 0.0
        return Math.toDegrees(asin((g.y / n).coerceIn(-1.0, 1.0)))
    }

    /** Secondary edge reading: how far the screen leans from vertical (plumb). */
    fun edgePlumbLeanDegrees(g: GravitySample): Double {
        val n = norm(g)
        if (n == 0.0) return 0.0
        return Math.toDegrees(asin((g.z / n).coerceIn(-1.0, 1.0)))
    }

    /**
     * Percent grade = tan(angle) × 100. Returns signed infinity at/beyond
     * [VERTICAL_GRADE_CUTOFF_DEGREES]; UI renders that as "∞" (BRIEF.md §Angle).
     */
    fun percentGrade(angleDegrees: Double): Double {
        if (abs(angleDegrees) >= VERTICAL_GRADE_CUTOFF_DEGREES) {
            return if (angleDegrees > 0) Double.POSITIVE_INFINITY else Double.NEGATIVE_INFINITY
        }
        return tan(Math.toRadians(angleDegrees)) * 100.0
    }

    const val VERTICAL_GRADE_CUTOFF_DEGREES = 89.5
    /**
     * Unsigned angle between two gravity directions — the directional basis for
     * relative zero in the angle meter. Unlike subtracting tilt magnitudes, this
     * accounts for the axis of movement: zeroing at 10° pitch and moving to 10°
     * roll reports the true ~14° orientation change, not 0°.
     */
    fun angleBetweenDegrees(a: GravitySample, b: GravitySample): Double {
        val na = norm(a)
        val nb = norm(b)
        if (na == 0.0 || nb == 0.0) return 0.0
        val cosine = (a.x * b.x + a.y * b.y + a.z * b.z) / (na * nb)
        return Math.toDegrees(acos(cosine.coerceIn(-1.0, 1.0)))
    }

    /**
     * True when the device is physically in the selected mode's geometry (within
     * [PLACEMENT_TOLERANCE_DEGREES] of it). Guards against, e.g., Edge mode reading
     * "level" for a phone lying flat on a table — asin(gy) is near zero there too,
     * but the measurement is meaningless and must not lock.
     */
    fun isPlacementValid(g: GravitySample, mode: LevelMode): Boolean {
        val n = norm(g)
        if (n == 0.0) return false
        return when (mode) {
            LevelMode.SURFACE -> g.z / n >= PLACEMENT_MIN_COS
            LevelMode.EDGE -> abs(g.x) / n >= PLACEMENT_MIN_COS
        }
    }

    const val PLACEMENT_TOLERANCE_DEGREES = 45.0 // TODO(tune) against real handling
    private val PLACEMENT_MIN_COS = cos(Math.toRadians(PLACEMENT_TOLERANCE_DEGREES))

    private fun norm(g: GravitySample): Double = sqrt(g.x * g.x + g.y * g.y + g.z * g.z)
}
