package com.onthelevel.core.sensors

import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

/** Minimal 3-vector helpers for calibration rotations. Pure Kotlin, module-internal. */
internal object Vec3Math {

    data class Vec3(val x: Double, val y: Double, val z: Double)

    val WORLD_UP_FLAT = Vec3(0.0, 0.0, 1.0)

    fun norm(v: Vec3): Double = sqrt(v.x * v.x + v.y * v.y + v.z * v.z)

    fun normalize(v: Vec3): Vec3 {
        val n = norm(v)
        return if (n == 0.0) v else Vec3(v.x / n, v.y / n, v.z / n)
    }

    fun dot(a: Vec3, b: Vec3): Double = a.x * b.x + a.y * b.y + a.z * b.z

    fun cross(a: Vec3, b: Vec3): Vec3 = Vec3(
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x,
    )

    /** Rodrigues rotation of [v] by [angleRadians] about the UNIT axis [axis]. */
    fun rotate(v: Vec3, axis: Vec3, angleRadians: Double): Vec3 {
        val c = cos(angleRadians)
        val s = sin(angleRadians)
        val kxv = cross(axis, v)
        val kdv = dot(axis, v)
        return Vec3(
            v.x * c + kxv.x * s + axis.x * kdv * (1 - c),
            v.y * c + kxv.y * s + axis.y * kdv * (1 - c),
            v.z * c + kxv.z * s + axis.z * kdv * (1 - c),
        )
    }
}
