VesiscanClinicalAndroid/app/src/main/java/com/medithings/vesiscan/managers/EllipseFitSpecific.kt

/*
 * Halíř–Flusser direct ellipse-specific fit — Python 1:1 이식.
 *
 * 원본: piezophantomtest `vesiscan_test/library/bv_estimation.py`
 *       `_fit_ellipse_specific` (line 881-961).
 *
 * 일반 원뿔곡선  a·u² + b·u·v + c·v² + d·u + e·v + f = 0  에
 * 타원 제약  4ac − b² > 0  을 걸어 항상 타원 해 보장. tilt (교차항 b) 흡수.
 *
 * 알고리즘 흐름:
 *   1. 데이터 표준화 (mean subtract, std divide) → u, v
 *   2. D1 = [u², u·v, v²],  D2 = [u, v, 1]  → S1, S2, S3
 *   3. T = -S3⁻¹·S2ᵀ,  M = C1⁻¹·(S1 + S2·T)   ← C1⁻¹ 는 타원 제약 행렬 inverse
 *   4. M 의 3×3 non-symmetric eigenvalue → 3개 eigenvector
 *   5. 4·v[0]·v[2] − v[1]² > 0 인 eigenvector 선택 (타원 조건)
 *   6. 계수 unpack → 중심 (y0, z0), 반축 (b_si, a_ap), residual
 *
 * 3×3 non-symmetric eigenvalue 는 characteristic polynomial (Cardano cubic) +
 * null-space via cross product 방식으로 순수 Kotlin 구현.
 */
package com.medithings.vesiscan.managers

import kotlin.math.abs
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.max
import kotlin.math.sqrt

object EllipseFitSpecific {

    /** Halíř–Flusser fit 결과. Python 반환값과 동일 순서. */
    data class Result(
        val y0: Double,
        val z0: Double,
        val bSi: Double,        // SI 반extent
        val aAp: Double,        // AP 반extent
        val residuals: DoubleArray,
    )

    /** DEBUG: 테스트 케이스에서 Python 의 eigenvector 를 강제 주입 (원인 pinpoint 용). null 이면 정상 solver. */
    var debugForceEigenvector: DoubleArray? = null
    /** DEBUG: 마지막 fit 의 상류 intermediate. Python 대조용. */
    var debugYm: Double = 0.0; var debugYsS: Double = 0.0
    var debugZm: Double = 0.0; var debugZsS: Double = 0.0
    var debugT: Array<DoubleArray>? = null
    var debugA2: DoubleArray? = null

    /**
     * (yw_f, zw_f) 두 벡터 (>= 5 점) → 타원 fit.  실패 (타원해 없음/특이) 시 null.
     */
    fun fit(ywF: DoubleArray, zwF: DoubleArray): Result? {
        val n = ywF.size
        if (n < 5 || zwF.size != n) return null

        // 1) 표준화 — Python `ym, ys_ = mean, std + 1e-12`
        val ym = ywF.average()
        val ysS = std(ywF) + 1e-12
        val zm = zwF.average()
        val zsS = std(zwF) + 1e-12
        val u = DoubleArray(n) { (ywF[it] - ym) / ysS }
        val v = DoubleArray(n) { (zwF[it] - zm) / zsS }
        debugYm = ym; debugYsS = ysS; debugZm = zm; debugZsS = zsS

        // 2) D1 = [u², u·v, v²],  D2 = [u, v, 1]
        //    S1 = D1ᵀD1 (3×3),  S2 = D1ᵀD2 (3×3),  S3 = D2ᵀD2 (3×3)
        val s1 = Array(3) { DoubleArray(3) }
        val s2 = Array(3) { DoubleArray(3) }
        val s3 = Array(3) { DoubleArray(3) }
        for (i in 0 until n) {
            val u2 = u[i] * u[i]
            val uv = u[i] * v[i]
            val v2 = v[i] * v[i]
            val d1 = doubleArrayOf(u2, uv, v2)
            val d2 = doubleArrayOf(u[i], v[i], 1.0)
            for (a in 0..2) for (b in 0..2) {
                s1[a][b] += d1[a] * d1[b]
                s2[a][b] += d1[a] * d2[b]
                s3[a][b] += d2[a] * d2[b]
            }
        }

        // 3) T = -S3⁻¹ · S2ᵀ   (3×3)
        val s3inv = invert3x3(s3) ?: return null
        val s2t = transpose3x3(s2)
        val neg = Array(3) { DoubleArray(3) }
        matmul3x3(s3inv, s2t, neg)
        val t = Array(3) { row -> DoubleArray(3) { col -> -neg[row][col] } }
        debugT = t

        // M = C1⁻¹ · (S1 + S2·T)
        //   C1⁻¹ = [[0, 0, 0.5], [0, -1, 0], [0.5, 0, 0]]
        val s2t2 = Array(3) { DoubleArray(3) }
        matmul3x3(s2, t, s2t2)
        val sSum = Array(3) { row -> DoubleArray(3) { col -> s1[row][col] + s2t2[row][col] } }
        val c1inv = arrayOf(
            doubleArrayOf(0.0, 0.0, 0.5),
            doubleArrayOf(0.0, -1.0, 0.0),
            doubleArrayOf(0.5, 0.0, 0.0),
        )
        val m = Array(3) { DoubleArray(3) }
        matmul3x3(c1inv, sSum, m)

        // 4) M 의 3개 eigenvector 계산 (실수만 유지)
        //    DEBUG mode: Python 의 eigenvector 를 강제 주입해 downstream 만 검증.
        val eigenvectors = if (debugForceEigenvector != null) listOf(debugForceEigenvector!!)
                           else eig3Real(m) ?: return null

        // 5) 조건 4·a·c − b² > 0 인 eigenvector 선택
        var picked: DoubleArray? = null
        for (ev in eigenvectors) {
            val cond = 4.0 * ev[0] * ev[2] - ev[1] * ev[1]
            if (cond > 0) {
                picked = ev
                break
            }
        }
        picked ?: return null

        // 6) a1 = picked, a2 = T · a1
        var a1 = picked
        var a2 = matvec3(t, a1)
        debugA2 = a2
        var a = a1[0]; var b = a1[1]; var c = a1[2]
        var d = a2[0]; var e = a2[1]; var f = a2[2]

        // 부호 통일 — a<0 이면 전체 conic 부호 반전 (conic=0 은 부호 불변)
        if (a < 0) { a = -a; b = -b; c = -c; d = -d; e = -e; f = -f }

        val det2 = a * c - (b * b) / 4.0
        if (det2 <= 0) return null

        // A33 = [[a, b/2], [b/2, c]], center = solve(A33, [-d/2, -e/2])
        // A33⁻¹ = (1/det2) · [[c, -b/2], [-b/2, a]]
        val invA00 = c / det2
        val invA01 = -(b / 2.0) / det2
        val invA10 = -(b / 2.0) / det2
        val invA11 = a / det2
        val un0 = invA00 * (-d / 2.0) + invA01 * (-e / 2.0)
        val vn0 = invA10 * (-d / 2.0) + invA11 * (-e / 2.0)

        val fprime = a * un0 * un0 + b * un0 * vn0 + c * vn0 * vn0 + d * un0 + e * vn0 + f
        val k = -fprime
        if (k <= 0 || invA00 <= 0 || invA11 <= 0) return null
        val unHalf = sqrt(k * invA00)
        val vnHalf = sqrt(k * invA11)

        val residuals = DoubleArray(n) {
            val du = u[it] - un0
            val dv = v[it] - vn0
            val q = a * du * du + b * du * dv + c * dv * dv
            abs(q / k - 1.0)
        }

        val y0 = ym + ysS * un0
        val z0 = zm + zsS * vn0
        val bSi = ysS * unHalf
        val aAp = zsS * vnHalf
        return Result(y0, z0, bSi, aAp, residuals)
    }

    // ─── 3×3 helpers ─────────────────────────────────────────────

    private fun matmul3x3(a: Array<DoubleArray>, b: Array<DoubleArray>, out: Array<DoubleArray>) {
        for (i in 0..2) for (j in 0..2) {
            var s = 0.0
            for (k in 0..2) s += a[i][k] * b[k][j]
            out[i][j] = s
        }
    }

    private fun matvec3(a: Array<DoubleArray>, v: DoubleArray): DoubleArray {
        val out = DoubleArray(3)
        for (i in 0..2) {
            var s = 0.0
            for (k in 0..2) s += a[i][k] * v[k]
            out[i] = s
        }
        return out
    }

    private fun transpose3x3(a: Array<DoubleArray>): Array<DoubleArray> =
        Array(3) { i -> DoubleArray(3) { j -> a[j][i] } }

    private fun invert3x3(m: Array<DoubleArray>): Array<DoubleArray>? {
        val a = m[0][0]; val b = m[0][1]; val c = m[0][2]
        val d = m[1][0]; val e = m[1][1]; val f = m[1][2]
        val g = m[2][0]; val h = m[2][1]; val i = m[2][2]
        val det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
        if (abs(det) < 1e-30) return null
        val inv = det
        return arrayOf(
            doubleArrayOf((e * i - f * h) / inv, (c * h - b * i) / inv, (b * f - c * e) / inv),
            doubleArrayOf((f * g - d * i) / inv, (a * i - c * g) / inv, (c * d - a * f) / inv),
            doubleArrayOf((d * h - e * g) / inv, (b * g - a * h) / inv, (a * e - b * d) / inv),
        )
    }

    /**
     * 3×3 non-symmetric matrix 의 real eigenvector 목록.
     *
     * characteristic polynomial:  p(λ) = −λ³ + c2·λ² + c1·λ + c0  = 0
     *   c2 = tr(M)
     *   c1 = −(m00·m11 + m00·m22 + m11·m22 − m01·m10 − m02·m20 − m12·m21)
     *   c0 = det(M)
     *
     * 부호 정리 : λ³ − c2·λ² − c1·λ − c0 = 0
     */
    private fun eig3Real(m: Array<DoubleArray>): List<DoubleArray>? {
        val m00 = m[0][0]; val m01 = m[0][1]; val m02 = m[0][2]
        val m10 = m[1][0]; val m11 = m[1][1]; val m12 = m[1][2]
        val m20 = m[2][0]; val m21 = m[2][1]; val m22 = m[2][2]

        // λ³ + A·λ² + B·λ + C = 0
        val A = -(m00 + m11 + m22)
        val B = (m00 * m11 - m01 * m10) + (m00 * m22 - m02 * m20) + (m11 * m22 - m12 * m21)
        val C = -(m00 * (m11 * m22 - m12 * m21)
                  - m01 * (m10 * m22 - m12 * m20)
                  + m02 * (m10 * m21 - m11 * m20))

        val roots = cubicRootsReal(A, B, C) ?: return null

        // 각 real eigenvalue 마다 (M − λI) 의 null-space (rank-2 가정) 를 얻음
        val vecs = mutableListOf<DoubleArray>()
        for (lam in roots) {
            val b = arrayOf(
                doubleArrayOf(m00 - lam, m01, m02),
                doubleArrayOf(m10, m11 - lam, m12),
                doubleArrayOf(m20, m21, m22 - lam),
            )
            val v = nullVector3(b) ?: continue
            vecs.add(v)
        }
        return if (vecs.isEmpty()) null else vecs
    }

    /** λ³ + A·λ² + B·λ + C = 0 의 real root 들 (Cardano + 삼각치환). */
    private fun cubicRootsReal(A: Double, B: Double, C: Double): List<Double>? {
        // depressed cubic t³ + p·t + q = 0  (λ = t − A/3)
        val shift = A / 3.0
        val p = B - A * A / 3.0
        val q = 2.0 * A * A * A / 27.0 - A * B / 3.0 + C
        val disc = -4.0 * p * p * p - 27.0 * q * q

        val roots = mutableListOf<Double>()
        if (disc > 0) {
            // 3 개 real root (casus irreducibilis) — 삼각치환
            val m = 2.0 * sqrt(-p / 3.0)
            val arg = 3.0 * q / (p * m)
            val theta = acos(arg.coerceIn(-1.0, 1.0)) / 3.0
            for (k in 0..2) {
                val t = m * cos(theta - 2.0 * Math.PI * k / 3.0)
                roots.add(t - shift)
            }
        } else {
            // 1 개 real root (+ 2 complex conjugate)
            val D = q * q / 4.0 + p * p * p / 27.0
            val sqrtD = sqrt(max(D, 0.0))
            val u = cbrtSigned(-q / 2.0 + sqrtD)
            val vv = cbrtSigned(-q / 2.0 - sqrtD)
            roots.add(u + vv - shift)
        }
        return if (roots.isEmpty()) null else roots
    }

    private fun cbrtSigned(x: Double): Double =
        if (x >= 0) Math.cbrt(x) else -Math.cbrt(-x)

    /**
     * 3×3 rank-2 행렬의 null-space 벡터 (unit norm 아님, 그냥 non-zero).
     * 두 행의 cross product 로 계산. 세 pair 모두 시도해 largest norm 선택.
     */
    private fun nullVector3(m: Array<DoubleArray>): DoubleArray? {
        val pairs = listOf(0 to 1, 0 to 2, 1 to 2)
        var best: DoubleArray? = null
        var bestNorm = 0.0
        for ((i, j) in pairs) {
            val a = m[i]; val b = m[j]
            val c = doubleArrayOf(
                a[1] * b[2] - a[2] * b[1],
                a[2] * b[0] - a[0] * b[2],
                a[0] * b[1] - a[1] * b[0],
            )
            val nrm = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2])
            if (nrm > bestNorm) { bestNorm = nrm; best = c }
        }
        return if (bestNorm < 1e-12) null else best
    }

    private fun std(a: DoubleArray): Double {
        val mean = a.average()
        var s = 0.0
        for (v in a) { val d = v - mean; s += d * d }
        return sqrt(s / a.size)
    }
}