package com.example.medilightv2android.managers

import com.example.medilightv2android.ble.PiezoChannelData
import kotlin.math.abs
import kotlin.math.max
import kotlin.math.min
import kotlin.math.sqrt

// ── Result Types ──

/**
 * Low-echo 기반 urine region 탐지 결과 (원은지 B방식 포팅)
 */
data class LowEchoResult(
    val ant: Int,               // 전벽 index
    val post: Int,              // 후벽 index
    val lowStart: Int,          // low-echo span 시작
    val lowEnd: Int,            // low-echo span 끝 (inclusive)
    val lowMean: Double,        // low-echo 구간 평균 진폭
    val urineLen: Int,          // post - ant - 1
    val score: Double,          // low_depth * urine_len
    val innerPeaks: List<Int>   // (ant, post) 내부 local peak indices
) {
    /** 방광 직경 (mm) = urine_len × distancePerSample */
    val diameterMm: Double
        get() = urineLen.toDouble() * PiezoConstants.distancePerSample

    /** 방광 용적 (ml) = K × (D_cm)³ (DrBench 공식, 팬텀 캘리브레이션) */
    val volumeMl: Double
        get() = PiezoConstants.volumeMl(diameterMm)

    /** 전벽 깊이 (mm) */
    val antDepthMm: Double
        get() = PiezoConstants.distanceMm(ant)

    /** 후벽 깊이 (mm) */
    val postDepthMm: Double
        get() = PiezoConstants.distanceMm(post)
}

/**
 * 채널별 분석 결과
 */
data class ChannelAnalysisResult(
    val channel: Int,
    val result: LowEchoResult?,
    val rawSignal: DoubleArray,
    val denoisedSignal: DoubleArray
) {
    val isValid: Boolean get() = result != null

    override fun equals(other: Any?): Boolean {
        if (this === other) return true
        if (other !is ChannelAnalysisResult) return false
        return channel == other.channel && result == other.result
    }

    override fun hashCode(): Int = 31 * channel + (result?.hashCode() ?: 0)
}

/**
 * 전체 측정 분석 결과
 */
data class PiezoAnalysisResult(
    val channels: List<ChannelAnalysisResult>,
    val volumeMl: Double,
    val confidence: Double,       // 0~100
    val validChannelCount: Int,
    val method: String            // "single", "multi_avg", "multi_weighted"
) {
    val bestChannel: ChannelAnalysisResult?
        get() = channels.filter { it.isValid }
            .maxByOrNull { it.result?.score ?: 0.0 }
}

// ── PiezoConstants (referenced by LowEchoResult) ──

/**
 * Physical constants for Piezo ultrasound echo measurement.
 * Mirrors iOS PiezoConstants, delegates distancePerSample/delayOffsetMm to PiezoHW.
 */
object PiezoConstants {
    const val adcMaxValue: Int = 4095
    const val adcVrefMv: Double = 3300.0

    const val sampleRateMhz: Double = 0.5375
    const val sampleIntervalUs: Double = 1.86
    const val soundSpeedMmUs: Double = 1.54

    val distancePerSample: Double get() = PiezoHW.distancePerSample
    val delayOffsetMm: Double get() = PiezoHW.delayOffsetMm

    const val volumeK: Double = 1.76

    fun distanceMm(index: Int): Double =
        index.toDouble() * distancePerSample + delayOffsetMm

    fun voltageMv(adcValue: Int): Double =
        adcValue.toDouble() / adcMaxValue.toDouble() * adcVrefMv

    fun volumeMl(diameterMm: Double): Double {
        val dCm = diameterMm / 10.0
        return volumeK * dCm * dCm * dCm
    }
}

// ── Analyzer ──

/**
 * Piezo 초음파 에코 분석기 — 원은지 연구원 B방식 알고리즘 포팅
 * Pipeline: Raw ADC → TVD Denoise → SG Smooth → Low-Echo Detection → Volume
 */
class PiezoEchoAnalyzer private constructor() {

    companion object {
        val shared = PiezoEchoAnalyzer()
    }

    // ── Parameters (노트북 검증값) ──

    // TVD
    val tvdWeight: Double = 0.06
    val tvdMaxIter: Int = 200
    val tvdTol: Double = 2e-4
    val tvdTau: Double = 0.25

    // Savitzky-Golay (window=5, poly=2 → 고정 계수)
    val sgCoeffs: DoubleArray = doubleArrayOf(-3.0/35.0, 12.0/35.0, 17.0/35.0, 12.0/35.0, -3.0/35.0)

    // Low Echo Detection
    val lowEchoAmpDefault: Double get() = GreenZoneConstants.lowEchoAmp.toDouble()
    val lowMinLen: Int = 3
    val mergeGapMax: Int = 3
    val peakSearchWin: Int = 20
    val postMaxIdx: Int = 80
    val minPeakMargin: Double = 30.0
    val minUrineLen: Int = 3
    val postRefineTolRatio: Double = 0.05
    val maxRealisticVolumeMl: Double = 1500.0
    val valleyStopRise: Double = 50.0
    val edgeDistDecay: Double = 0.12

    // ── Public API ──

    /** 단일 채널 raw ADC → 용적 분석 (crash-safe) */
    fun analyzeChannel(rawADC: List<UShort>, channel: Int = 0): ChannelAnalysisResult {
        val raw = DoubleArray(rawADC.size) { rawADC[it].toDouble() }
        if (raw.size < 10) {
            return ChannelAnalysisResult(channel = channel, result = null, rawSignal = raw, denoisedSignal = raw.copyOf())
        }
        return try {
            val denoised = denoise(raw)
            val result = detectLowEcho(raw = raw, denoised = denoised)
            ChannelAnalysisResult(channel = channel, result = result, rawSignal = raw, denoisedSignal = denoised)
        } catch (_: Exception) {
            ChannelAnalysisResult(channel = channel, result = null, rawSignal = raw, denoisedSignal = raw.copyOf())
        }
    }

    /** 다채널 분석 → 최종 용적 */
    fun analyzeMultiChannel(channelData: List<PiezoChannelData>): PiezoAnalysisResult {
        val results = channelData.map { ch ->
            analyzeChannel(ch.buffer, ch.channel)
        }

        // Step 1: Get valid results (algorithm found urine region)
        val validResults = results.filter { it.isValid }

        // Step 2: Filter out unrealistic volumes (container wall artifacts)
        val realisticResults = validResults.filter { (it.result?.volumeMl ?: 0.0) <= maxRealisticVolumeMl }

        // Step 3: Outlier removal — if 3+ results, remove > 2× median
        var filtered = realisticResults
        if (filtered.size >= 3) {
            val vols = filtered.mapNotNull { it.result?.volumeMl }.sorted()
            val median = vols[vols.size / 2]
            filtered = filtered.filter { cr ->
                val v = cr.result?.volumeMl ?: return@filter false
                v <= median * 2.5 && v >= median * 0.3
            }
        }

        val usedCount = filtered.size
        val volumeMl: Double
        val method: String
        val confidence: Double

        if (usedCount == 0) {
            volumeMl = 0.0
            method = "none"
            confidence = 0.0
        } else if (usedCount == 1) {
            volumeMl = filtered[0].result!!.volumeMl
            method = "single"
            confidence = min(100.0, filtered[0].result!!.score / 100.0)
        } else {
            // Median-based: use median volume (robust against remaining outliers)
            val vols = filtered.mapNotNull { it.result?.volumeMl }.sorted()
            val median = vols[vols.size / 2]
            volumeMl = median
            method = "median_${usedCount}ch"
            confidence = min(100.0, usedCount.toDouble() / results.size.toDouble() * 100.0)
        }

        return PiezoAnalysisResult(
            channels = results,
            volumeMl = volumeMl,
            confidence = confidence,
            validChannelCount = usedCount,
            method = method
        )
    }

    // ── Denoising Pipeline ──

    /** SG 디노이징 (6ch 알고리즘: TVD 제거, SG만 사용) */
    fun denoise(signal: DoubleArray): DoubleArray {
        return sgSmooth(signal)
    }

    /** 1D Total Variation Denoising — Chambolle (2004) dual projected gradient */
    fun tvDenoiseChambolle1D(signal: DoubleArray): DoubleArray {
        val n = signal.size
        if (n < 2 || tvdWeight <= 0) return signal.copyOf()
        // Skip if signal is flat (all same values)
        val mn = signal.min()
        val mx = signal.max()
        if (mx - mn < 1.0) return signal.copyOf()

        val p = DoubleArray(n - 1)
        var uPrev = signal.copyOf()
        val tauW = tvdTau / tvdWeight

        for (iter in 0 until tvdMaxIter) {
            // u = x + weight * div(p)
            val divP = DoubleArray(n)
            divP[0] = p[0]
            for (i in 1 until (n - 1)) {
                divP[i] = p[i] - p[i - 1]
            }
            divP[n - 1] = -p[n - 2]

            val u = DoubleArray(n) { signal[it] + tvdWeight * divP[it] }

            // grad(u) = diff(u) and dual update
            var maxDiff = 0.0
            for (i in 0 until (n - 1)) {
                val gradU = u[i + 1] - u[i]
                p[i] = (p[i] + tauW * gradU) / (1.0 + tauW * abs(gradU))
                maxDiff = max(maxDiff, abs(u[i] - uPrev[i]))
            }
            maxDiff = max(maxDiff, abs(u[n - 1] - uPrev[n - 1]))

            if (maxDiff < tvdTol) break
            uPrev = u
        }

        // Final reconstruction
        val divP = DoubleArray(n)
        divP[0] = p[0]
        for (i in 1 until (n - 1)) {
            divP[i] = p[i] - p[i - 1]
        }
        divP[n - 1] = -p[n - 2]

        return DoubleArray(n) { signal[it] + tvdWeight * divP[it] }
    }

    /** Savitzky-Golay 5-tap FIR smoothing (window=5, poly=2) */
    fun sgSmooth(signal: DoubleArray): DoubleArray {
        val n = signal.size
        if (n < 5) return signal.copyOf()

        val out = DoubleArray(n)

        // Inner: convolution with fixed coefficients [-3, 12, 17, 12, -3]/35
        for (i in 2 until (n - 2)) {
            out[i] = sgCoeffs[0] * signal[i - 2] +
                     sgCoeffs[1] * signal[i - 1] +
                     sgCoeffs[2] * signal[i] +
                     sgCoeffs[3] * signal[i + 1] +
                     sgCoeffs[4] * signal[i + 2]
        }

        // Edge: polynomial fit for first/last 2 samples
        // Left edge: fit poly2 to signal[0..4], evaluate at 0,1
        val leftCoeffs = polyFit2(signal.sliceArray(0 until 5))
        out[0] = evalPoly2(leftCoeffs, -2.0)
        out[1] = evalPoly2(leftCoeffs, -1.0)

        // Right edge: fit poly2 to signal[n-5..n-1], evaluate at n-2,n-1
        val rightCoeffs = polyFit2(signal.sliceArray((n - 5) until n))
        out[n - 2] = evalPoly2(rightCoeffs, 1.0)
        out[n - 1] = evalPoly2(rightCoeffs, 2.0)

        return out
    }

    // Quadratic LS fit to 5 points centered at 0: x = [-2,-1,0,1,2]
    // Returns Triple(c0, c1, c2) where f(t) = c0 + c1*t + c2*t²
    private fun polyFit2(y: DoubleArray): Triple<Double, Double, Double> {
        val c0 = (-3*y[0] + 12*y[1] + 17*y[2] + 12*y[3] - 3*y[4]) / 35.0
        val c1 = (-2*y[0] - y[1] + y[3] + 2*y[4]) / 10.0
        val c2 = (2*y[0] - y[1] - 2*y[2] - y[3] + 2*y[4]) / 14.0
        return Triple(c0, c1, c2)
    }

    private fun evalPoly2(c: Triple<Double, Double, Double>, t: Double): Double =
        c.first + c.second * t + c.third * t * t

    // ── Low Echo Detection ──

    /** 적응형 low-echo 임계값: 신호 통계 기반 */
    fun adaptiveThreshold(signal: DoubleArray): Double {
        if (signal.size < 10) return lowEchoAmpDefault
        val sorted = signal.sorted().toDoubleArray()
        val q25 = sorted[sorted.size / 4]
        val median = sorted[sorted.size / 2]
        val q75 = sorted[3 * sorted.size / 4]
        val iqr = q75 - q25
        return max(q25, median - 0.5 * iqr)
    }

    var useAdaptiveThreshold: Boolean = true

    /** 단일 1D 채널 → urine region 탐지 */
    fun detectLowEcho(raw: DoubleArray, denoised: DoubleArray): LowEchoResult? {
        val sg = denoised
        if (sg.size < 10) return null

        if (!useAdaptiveThreshold) {
            return detectLowEchoCore(sg = sg, threshold = lowEchoAmpDefault)
        }

        // Adaptive threshold: percentile + depth 보상 조합
        val thr = computeAdaptiveThreshold(sg)
        return detectLowEchoCore(sg = sg, threshold = thr)
    }

    /**
     * Adaptive threshold: 신호 통계 + depth attenuation 보상
     *
     * 1) Percentile 기반 베이스라인: 전체 신호의 q25~median 사이에서 결정
     * 2) 초반 peak(피부 반사) 제외: ringSkip(3) 이후 사용
     * 3) 고정 threshold와의 가중 평균으로 급격한 변동 방지
     */
    fun computeAdaptiveThreshold(sg: DoubleArray): Double {
        val skip = GreenZoneConstants.ringSkip
        val usable = if (sg.size > skip + 10) sg.sliceArray(skip until sg.size) else sg

        val sorted = usable.sorted()
        val n = sorted.size
        val q25 = sorted[n / 4]
        val q50 = sorted[n / 2]
        val q75 = sorted[3 * n / 4]
        val iqr = q75 - q25

        // Percentile 기반: 소변 영역은 보통 하위 25~50%
        val percThr = max(q25 + iqr * 0.3, q50 - iqr * 0.3)

        // 고정값과 adaptive의 가중 평균 (급격한 변동 방지)
        val adaptive = percThr.coerceIn(lowEchoAmpDefault * 0.7, lowEchoAmpDefault * 1.5)
        val blended = lowEchoAmpDefault * 0.4 + adaptive * 0.6

        return blended
    }

    /**
     * Low-echo 탐지 핵심 — prominence 기반 wall peak 선택
     * 1:1 port of low_echo_detection_method_b.py (2026-04-20 update)
     */
    private fun detectLowEchoCore(sg: DoubleArray, threshold: Double): LowEchoResult? {
        if (sg.size < 10) return null

        // 1) low-echo span 추출
        val lowMask = BooleanArray(sg.size) { sg[it] <= threshold }
        val rawSpans = contiguousTrueSpans(lowMask).filter { it.second - it.first + 1 >= lowMinLen }

        // 2) 병합 — gap 내부에 벽 후보(threshold + 5 초과 peak)가 있으면 병합하지 않음
        val gapPeakThr = threshold + 5.0
        val spans = mergeCloseSpansWithWallCheck(rawSpans, maxGap = mergeGapMax, signal = sg, gapPeakThr = gapPeakThr)
        val firstSpan = spans.firstOrNull() ?: return null

        val s = max(0, firstSpan.first)
        val e = min(sg.size - 1, firstSpan.second)
        if (s > e) return null

        val lowSlice = safeSlice(sg, from = s, to = e) ?: return null
        if (lowSlice.isEmpty()) return null
        val lowMean = lowSlice.sum() / lowSlice.size.toDouble()
        val peakMin = lowMean + minPeakMargin

        // 3) Prominence 기반 wall peak 선택
        val ant = selectWallByProminence(sg = sg, edge = s, searchWin = peakSearchWin,
            peakMin = peakMin, side = WallSide.ANT, otherEdge = e) ?: return null
        var post = selectWallByProminence(sg = sg, edge = e, searchWin = peakSearchWin,
            peakMin = peakMin, side = WallSide.POST, otherEdge = s) ?: return null

        // 4) post > POST_MAX_IDX: prominence 기반 재탐색
        if (post > postMaxIdx) {
            val backHalfEdge = (s + e) / 2
            post = selectWallByProminence(
                sg = sg, edge = backHalfEdge,
                searchWin = postMaxIdx - backHalfEdge,
                peakMin = peakMin, side = WallSide.POST, otherEdge = null
            ) ?: return null
        }

        val antH = sg[ant]
        val postH = sg[post]
        val lowDepth = ((antH + postH) / 2.0) - lowMean
        val urineLen = post - ant - 1

        if (urineLen < minUrineLen) return null

        return LowEchoResult(
            ant = ant,
            post = post,
            lowStart = s,
            lowEnd = e,
            lowMean = lowMean,
            urineLen = urineLen,
            score = lowDepth * urineLen.toDouble(),
            innerPeaks = findInnerPeaks(sg, left = ant, right = post)
        )
    }

    // ── Prominence-based Wall Selection ──

    private val maxPeakCandidates = 3

    private enum class WallSide { ANT, POST }

    /** peak 오른쪽에서 가장 가까운 valley의 sg 값 */
    private fun findRightValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
        val n = sg.size
        var v = sg[peakIdx]
        for (i in (peakIdx + 1) until min(n, peakIdx + maxDist)) {
            if (sg[i] < v) {
                v = sg[i]
            } else if (sg[i] > v + valleyStopRise) {
                break
            }
        }
        return v
    }

    private fun findLeftValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
        var v = sg[peakIdx]
        for (i in (peakIdx - 1) downTo max(0, peakIdx - maxDist)) {
            if (sg[i] < v) {
                v = sg[i]
            } else if (sg[i] > v + valleyStopRise) {
                break
            }
        }
        return v
    }

    /**
     * edge 양쪽에서 prominence 최대인 wall peak 선택
     * 1:1 port of _select_wall_by_prominence()
     */
    private fun selectWallByProminence(
        sg: DoubleArray, edge: Int, searchWin: Int,
        peakMin: Double, side: WallSide, otherEdge: Int?
    ): Int? {
        val n = sg.size
        if (n <= 0 || edge < 0 || edge >= n) return null

        // 검색 범위 결정
        val leftLo: Int
        val rightHi: Int

        if (side == WallSide.ANT) {
            leftLo = max(0, edge - searchWin)          // 바깥 (자유)
            var rh = min(n - 1, edge + searchWin)      // 안쪽
            if (otherEdge != null) rh = min(rh, otherEdge) // e를 넘지 않음
            rightHi = rh
        } else {
            var ll = max(0, edge - searchWin)           // 안쪽
            if (otherEdge != null) ll = max(ll, otherEdge) // s를 넘지 않음
            leftLo = ll
            rightHi = min(n - 1, edge + searchWin)     // 바깥 (자유)
        }

        // edge 왼쪽 peak 후보 — edge+1 포함 (inclusive boundary)
        var leftCandidates = listOf<Int>()
        val leftEnd = min(edge + 1, n)
        if (leftEnd > leftLo) {
            val seg = safeSlice(sg, from = leftLo, to = leftEnd - 1)
            if (seg != null) {
                val pks = findPeaks1D(seg)
                val global = pks.map { it + leftLo }
                    .filter { it < edge && sg[it] >= peakMin }
                leftCandidates = global.sortedBy { abs(it - edge) }.take(maxPeakCandidates)
            }
        }

        // edge 오른쪽 peak 후보 — edge-1부터 시작 (inclusive boundary)
        var rightCandidates = listOf<Int>()
        val rightStart = max(edge - 1, 0)
        if (rightHi >= rightStart) {
            val seg = safeSlice(sg, from = rightStart, to = rightHi)
            if (seg != null) {
                val pks = findPeaks1D(seg)
                val global = pks.map { it + rightStart }
                    .filter { it >= edge && sg[it] >= peakMin }
                rightCandidates = global.sortedBy { abs(it - edge) }.take(maxPeakCandidates)
            }
        }

        val candidates = leftCandidates + rightCandidates
        if (candidates.isEmpty()) return null

        // prominence + edge distance penalty (EDGE_DIST_DECAY=0.12)
        var bestPeak: Int? = null
        var bestScore = -1.0

        for (p in candidates) {
            val valley: Double = if (side == WallSide.ANT) {
                findRightValley(sg, peakIdx = p)
            } else {
                findLeftValley(sg, peakIdx = p)
            }
            val prom = sg[p] - valley
            val dist = abs(p - edge)
            val score = prom / (1.0 + edgeDistDecay * dist)
            if (score > bestScore) {
                bestScore = score
                bestPeak = p
            }
        }

        return bestPeak
    }

    // ── Span Utilities ──

    fun findPeaks1D(x: DoubleArray, height: Double? = null): List<Int> {
        val n = x.size
        if (n < 3) return emptyList()

        val peaks = mutableListOf<Int>()
        var i = 1
        while (i < n - 1) {
            if (x[i - 1] < x[i]) {
                var j = i
                while (j < n - 1 && x[j + 1] == x[j]) j++
                if (j < n - 1 && x[j + 1] < x[j]) {
                    peaks.add((i + j) / 2)
                }
                i = j + 1
            } else {
                i++
            }
        }

        return if (height != null) {
            peaks.filter { x[it] >= height }
        } else {
            peaks
        }
    }

    fun contiguousTrueSpans(mask: BooleanArray): List<Pair<Int, Int>> {
        val spans = mutableListOf<Pair<Int, Int>>()
        var start: Int? = null
        for ((i, m) in mask.withIndex()) {
            if (m && start == null) {
                start = i
            } else if (!m && start != null) {
                spans.add(Pair(start, i - 1))
                start = null
            }
        }
        if (start != null) {
            spans.add(Pair(start, mask.size - 1))
        }
        return spans
    }

    fun mergeCloseSpans(spans: List<Pair<Int, Int>>, maxGap: Int): List<Pair<Int, Int>> {
        return mergeCloseSpansWithWallCheck(spans, maxGap = maxGap, signal = null, gapPeakThr = 0.0)
    }

    /** 병합 시 gap 내부에 벽 후보(gapPeakThr 초과 peak)가 있으면 병합하지 않음 */
    fun mergeCloseSpansWithWallCheck(
        spans: List<Pair<Int, Int>>, maxGap: Int,
        signal: DoubleArray?, gapPeakThr: Double
    ): List<Pair<Int, Int>> {
        if (spans.isEmpty()) return emptyList()
        val ordered = spans.sortedBy { it.first }
        val merged = mutableListOf(ordered[0])
        for (i in 1 until ordered.size) {
            val (s, e) = ordered[i]
            val prevEnd = merged[merged.size - 1].second
            val gap = s - prevEnd - 1
            if (gap <= maxGap) {
                // Check: gap 내부에 벽 후보 peak이 있는지
                var hasWallPeak = false
                if (signal != null && gapPeakThr > 0) {
                    val gapStart = prevEnd + 1
                    val gapEnd = s - 1
                    if (gapStart <= gapEnd) {
                        val gapSlice = safeSlice(signal, from = gapStart, to = gapEnd)
                        if (gapSlice != null) {
                            val peaks = findPeaks1D(gapSlice, height = gapPeakThr)
                            if (peaks.isNotEmpty()) hasWallPeak = true
                            // Also check endpoints
                            if (!hasWallPeak) {
                                if ((gapSlice.firstOrNull() ?: 0.0) > gapPeakThr) hasWallPeak = true
                                if ((gapSlice.lastOrNull() ?: 0.0) > gapPeakThr) hasWallPeak = true
                            }
                        }
                    }
                }
                if (hasWallPeak) {
                    // Wall candidate in gap — don't merge
                    merged.add(Pair(s, e))
                } else {
                    merged[merged.size - 1] = Pair(merged[merged.size - 1].first, max(prevEnd, e))
                }
            } else {
                merged.add(Pair(s, e))
            }
        }
        return merged
    }

    fun nearestPeak(signal: DoubleArray, center: Int, left: Int, right: Int, minHeight: Double): Int? {
        if (signal.isEmpty()) return null
        val l = max(0, left)
        val r = min(signal.size - 1, right)
        val local = safeSlice(signal, from = l, to = r) ?: return null
        val pkLocal = findPeaks1D(local, height = minHeight)
        val candidates = (pkLocal.map { it + l }).toMutableSet()

        // Endpoint correction
        if (l == r) {
            if (signal[l] >= minHeight) candidates.add(l)
        } else {
            if (signal[l] >= minHeight && signal[l] >= signal[l + 1]) candidates.add(l)
            if (signal[r] >= minHeight && signal[r] >= signal[r - 1]) candidates.add(r)
        }

        if (candidates.isEmpty()) return null

        val sorted = candidates.sorted()
        // Find nearest to center, tie-break by amplitude
        var bestIdx = sorted[0]
        var bestDist = abs(sorted[0] - center)
        for (c in sorted) {
            val dist = abs(c - center)
            if (dist < bestDist || (dist == bestDist && signal[c] > signal[bestIdx])) {
                bestIdx = c
                bestDist = dist
            }
        }
        return bestIdx
    }

    fun findInnerPeaks(signal: DoubleArray, left: Int, right: Int): List<Int> {
        val l = left + 1
        val r = right - 1
        val local = safeSlice(signal, from = l, to = r) ?: return emptyList()
        return findPeaks1D(local).map { it + l }
    }

    // ── Safe Array Slicing ──

    /** Safe inclusive range slice — returns null if bounds are invalid */
    private fun safeSlice(arr: DoubleArray, from: Int, to: Int): DoubleArray? {
        val l = max(0, from)
        val r = min(arr.size - 1, to)
        if (l > r || arr.isEmpty()) return null
        return arr.sliceArray(l..r)
    }
}