package com.medithings.vesiscan.managers

import com.medithings.vesiscan.ble.ImuSample
import kotlin.math.abs
import kotlin.math.atan2
import kotlin.math.sqrt

/**
 * IMU 샘플 묶음(한 mtb 응답 = 15샘플)에서 자세/안정성 지표 추출.
 *
 * 가정: VBT 센서의 Accel z축이 환자 몸 바깥 방향(센서 면 → 위).
 * 평평하게 누운 환자(Supine)에 부착 시 Az ≈ -1g (또는 +1g, 부착 면에 따라).
 *
 * 향후 임상 데이터로 calibration 필요.
 */
object ImuAnalysis {

    /** 한 응답 안의 모든 샘플 평균 Accel vector */
    fun meanAccel(samples: List<ImuSample>): Triple<Float, Float, Float>? {
        if (samples.isEmpty()) return null
        var sx = 0f; var sy = 0f; var sz = 0f
        for (s in samples) { sx += s.ax; sy += s.ay; sz += s.az }
        val n = samples.size
        return Triple(sx / n, sy / n, sz / n)
    }

    /** 한 응답 안의 모든 샘플 평균 Gyro vector */
    fun meanGyro(samples: List<ImuSample>): Triple<Float, Float, Float>? {
        if (samples.isEmpty()) return null
        var sx = 0f; var sy = 0f; var sz = 0f
        for (s in samples) { sx += s.gx; sy += s.gy; sz += s.gz }
        val n = samples.size
        return Triple(sx / n, sy / n, sz / n)
    }

    /**
     * 흔들림 강도 — Gyro 표준편차의 RMS. 정지 시 ≈ 0~5 dps, 흔드는 중 ≈ 50+ dps.
     */
    fun shakeRmsDps(samples: List<ImuSample>): Float {
        if (samples.size < 2) return 0f
        val mean = meanGyro(samples) ?: return 0f
        var sum = 0f
        for (s in samples) {
            val dx = s.gx - mean.first
            val dy = s.gy - mean.second
            val dz = s.gz - mean.third
            sum += dx * dx + dy * dy + dz * dz
        }
        return sqrt(sum / samples.size)
    }

    /**
     * 자세 추정 (간이 — Accel만 사용).
     * 평균 Accel vector의 방향 → 어느 축이 중력에 가까운지 판정.
     *
     * 반환: "Supine" / "Sitting" / "Standing" / "Tilted" / "Moving"
     *
     * 임시 기준 (임상 데이터로 보정 필요):
     *  - |Az| 가장 큼 + Az > 0.7g: 환자 몸 누움 (Supine, 센서면 위 향함)
     *  - |Az| 가장 큼 + Az < -0.7g: 센서 뒤집힘
     *  - |Ay| 가장 큼: Sitting (몸 세움)
     *  - |Ax| 가장 큼: Standing 또는 옆 기울기
     *  - 기타: Tilted
     *  - shakeRmsDps > 30: Moving (자세 판정 불가)
     */
    fun estimatePosture(samples: List<ImuSample>): String {
        if (samples.isEmpty()) return "—"
        if (shakeRmsDps(samples) > 30f) return "Moving"
        val (ax, ay, az) = meanAccel(samples) ?: return "—"
        val axAbs = abs(ax); val ayAbs = abs(ay); val azAbs = abs(az)
        return when {
            azAbs >= axAbs && azAbs >= ayAbs && azAbs > 0.7f ->
                if (az > 0) "Supine" else "Supine (flip)"
            ayAbs >= axAbs && ayAbs > 0.6f -> "Sitting"
            axAbs > 0.6f -> "Standing"
            else -> "Tilted"
        }
    }

    /**
     * Tilt 각도 (도) — 센서 z축이 중력에 대해 얼마나 기울었는지.
     * 0° = z축이 정확히 중력 방향, 90° = z축이 수평.
     */
    fun tiltDegrees(samples: List<ImuSample>): Float {
        val (ax, ay, az) = meanAccel(samples) ?: return 0f
        val mag = sqrt(ax * ax + ay * ay + az * az)
        if (mag < 0.1f) return 0f  // 자유낙하/이상치
        val tiltRad = atan2(sqrt(ax * ax + ay * ay), abs(az))
        return (tiltRad * 180.0 / Math.PI).toFloat()
    }
}