package com.onthelevel.core.sensors

import com.onthelevel.core.sensors.Vec3Math.Vec3
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.tan

/**
 * Two-sample (180° flip) calibration, per mode (BRIEF.md §Measurement modes and calibration).
 *
 * DERIVATION — the surface's true tilt is fixed in the world frame; the device's own bias
 * is fixed in the device frame. After rotating the device 180° about the CONTACT-PLANE
 * NORMAL — on the same, unmoved surface — the true tilt appears negated in device
 * readings while the bias does not move:
 *
 *   reading₁ =  tilt + bias
 *   reading₂ = -tilt + bias
 *   ⇒ bias = (reading₁ + reading₂) / 2
 *
 * Derivation happens in angle space near level, where the flip identity is exact to
 * first order. Preconditions the calibration UI must enforce: (a) rotation about the
 * contact-plane normal, (b) surface unmoved between samples, (c) surface within a few
 * degrees of level.
 *
 * APPLICATION — the stored bias angles parameterize a per-mode REFERENCE ORIENTATION:
 * the device-frame direction gravity reads when the device is truly level in that mode.
 * Correction is applied in vector space — a fixed rotation of every gravity sample that
 * maps the reference onto the mode's ideal axis — BEFORE any display angle is derived.
 * This stays correct away from zero (see tests at 30°), unlike subtracting scalar
 * offsets from derived angles.
 *
 * Biases are stored per mode and applied only to that mode's readings; a Surface
 * calibration must never touch Edge readings (AUDIT.md finding 3).
 */
object TwoSampleCalibration {

    /** Derive one axis's fixed device bias from two readings taken 180° apart. */
    fun deriveBiasDegrees(reading1Degrees: Double, reading2Degrees: Double): Double =
        (reading1Degrees + reading2Degrees) / 2.0

    fun deriveSurface(
        pitch1: Double, roll1: Double,
        pitch2: Double, roll2: Double,
    ): SurfaceCalibration = SurfaceCalibration(
        pitchBiasDegrees = deriveBiasDegrees(pitch1, pitch2),
        rollBiasDegrees = deriveBiasDegrees(roll1, roll2),
    )

    /**
     * Generalized bias derivation for a SYMMETRIC set of rotations about the surface
     * normal — 2-point (0°/180°) or 4-point (0°/90°/180°/270°). The surface's true tilt
     * is a sinusoid in rotation angle, so it sums to zero over any such symmetric set;
     * the mean of the readings is therefore the fixed device bias. The 4-point set also
     * cancels each axis with both a 180° opposite and a 90° pair, so it tolerates an
     * imperfect turn better and averages out more sensor noise (thorough mode).
     */
    fun deriveSurfaceFromSamples(samples: List<CapturedSurfaceSample>): SurfaceCalibration {
        require(samples.isNotEmpty()) { "Calibration needs at least one captured sample" }
        return SurfaceCalibration(
            pitchBiasDegrees = samples.sumOf { it.pitchDegrees } / samples.size,
            rollBiasDegrees = samples.sumOf { it.rollDegrees } / samples.size,
        )
    }

    fun deriveEdge(
        level1: Double,
        level2: Double,
        calibratedOnPositiveXEdge: Boolean = true,
    ): EdgeCalibration = EdgeCalibration(
        levelBiasDegrees = deriveBiasDegrees(level1, level2),
        calibratedOnPositiveXEdge = calibratedOnPositiveXEdge,
    )
}

/**
 * Surface-mode reference orientation, parameterized by the bias angles measured when the
 * device is truly flat. [apply] rotates each gravity sample by the fixed rotation that
 * aligns the reference direction with the screen normal (+Z).
 */
data class SurfaceCalibration(val pitchBiasDegrees: Double, val rollBiasDegrees: Double) {

    fun apply(g: GravitySample): GravitySample {
        if (pitchBiasDegrees == 0.0 && rollBiasDegrees == 0.0) return g
        // Reference: the direction gravity reads on a truly level surface —
        // atan2(y, z) = pitchBias and atan2(x, z) = rollBias by construction.
        val reference = Vec3Math.normalize(
            Vec3(
                tan(Math.toRadians(rollBiasDegrees)),
                tan(Math.toRadians(pitchBiasDegrees)),
                1.0,
            ),
        )
        val axis = Vec3Math.cross(reference, Vec3Math.WORLD_UP_FLAT)
        val axisNorm = Vec3Math.norm(axis)
        if (axisNorm < 1e-12) return g
        val unitAxis = Vec3(axis.x / axisNorm, axis.y / axisNorm, axis.z / axisNorm)
        val angle = acos(Vec3Math.dot(reference, Vec3Math.WORLD_UP_FLAT).coerceIn(-1.0, 1.0))
        val corrected = Vec3Math.rotate(Vec3(g.x, g.y, g.z), unitAxis, angle)
        return g.copy(x = corrected.x, y = corrected.y, z = corrected.z)
    }

    companion object { val NONE = SurfaceCalibration(0.0, 0.0) }
}

/**
 * Edge-mode reference orientation: a fixed rotation about the device Z axis that zeroes
 * the level reading for the calibrated placement, leaving plumb lean untouched (lean is
 * not part of "edge level" and must not be silently "calibrated" from a leaned placement).
 *
 * Valid for the long edge the calibration was performed on; [calibratedOnPositiveXEdge]
 * records which (true = the device's right edge down, gravity along +X). The calibration
 * flow must capture this from the placement it instructed.
 */
data class EdgeCalibration(
    val levelBiasDegrees: Double,
    val calibratedOnPositiveXEdge: Boolean = true,
) {
    fun apply(g: GravitySample): GravitySample {
        if (levelBiasDegrees == 0.0) return g
        val gamma = Math.toRadians(
            if (calibratedOnPositiveXEdge) -levelBiasDegrees else levelBiasDegrees,
        )
        val c = cos(gamma)
        val s = sin(gamma)
        return g.copy(x = g.x * c - g.y * s, y = g.x * s + g.y * c)
    }

    companion object { val NONE = EdgeCalibration(0.0) }
}
