class HarmonicPhaseTracker(
    override val sampleRate: Double = 48_000.0,
    val minHz: Double = 35.0,
    val maxHz: Double = 350.0,
    val initialHz: Double = 100.0,

    /** Number of harmonic demodulators. Channels above Nyquist automatically stop voting. */
    val harmonics: Int = 12,

    /**
     * Fine-track capture bandwidth measured in FUNDAMENTAL Hz.
     * Harmonic k receives a baseband LP cutoff of approximately k * this value.
     */
    val captureHz: Double = 4.0,

    /** Time constant with which the stable omega estimate absorbs the fused residual. */
    val frequencyTrackingTimeSeconds: Double = 0.030,

    /**
     * How much of the instantaneous fused residual appears in phaseAdvance.
     * 0 = only stable omega; 1 = strongly cycle-following. Values >1 are allowed experimentally.
     */
    var phaseFollow: Double = 1.0,

    /** Fundamental-Hz distance from the robust median after which a partial is rejected. */
    val residualOutlierHz: Double = 7.0,

    /** Mildly prefer low partials: weight is energy / k^harmonicWeightExponent. */
    val harmonicWeightExponent: Double = 0.5,

    /** Input RMS-ish level below which the tracker is considered gated. */
    val gateLevel: Double = 0.01,

    /** Baseband partial amplitude needed to cast a vote, relative to [gateLevel]. */
    val partialGateScale: Double = 0.35,

    // Broadband power follower for harmonicity / gate measurement.
    broadbandPowerTimeSeconds: Double = 0.020,

    // Confidence smoothing, mostly to prevent lock chatter.
    confidenceTimeSeconds: Double = 0.025,

    /** Confidence above which [TrackedPhaseState.locked] is true. */
    val lockThreshold: Double = 0.65,

    // ---- coarse periodicity reacquisition ----

    /** Raw samples averaged into one coarse sample. 8 -> 6 kHz at a 48 kHz audio rate. */
    val coarseDecimation: Int = 8,

    /** Number of downsampled samples in each normalized-correlation comparison. */
    val coarseWindow: Int = 384,

    /** Re-run coarse correlation this many downsampled samples. */
    val coarseHop: Int = 96,

    /** Absolute normalized-correlation floor for accepting a periodicity candidate. */
    val coarseMinCorrelation: Double = 0.60,

    /**
     * Accept peaks this close to the best correlation and choose the shortest-lag one.
     * This avoids selecting the equally-good 2T / 3T autocorrelation peaks.
     */
    val coarseRelativePeak: Double = 0.93,

    /** Ignore coarse/fine disagreement smaller than this. Fine tracking has better precision. */
    val coarseDeadbandHz: Double = 1.5,

    /** Coarse pull while fine confidence is weak. */
    val coarseUnlockedPull: Double = 0.65,

    /** Coarse pull when the fine bank claims to be confident (principally octave-trap correction). */
    val coarseLockedPull: Double = 0.12,
) : PhaseTracker {

    override fun process(input: Float): TrackedPhaseState {
        val x = if (input.isFinite()) input.toDouble() else 0.0

        // Broadband reference power. For a sum of sinusoids this lets us estimate what fraction
        // the harmonic channels explain.
        broadbandPower =
            broadbandPowerKeep * broadbandPower +
                (1.0 - broadbandPowerKeep) * (x * x)

        // The coarse path runs independently of the fine demodulators and may pull omega if it
        // discovers a convincing different periodicity.
        pushCoarse(x)
        advanceCoarse()

        // Generate sin(k phi), cos(k phi) recursively: only one sin/cos pair per audio sample
        // regardless of harmonic count.
        val c1 = cos(phase)
        val s1 = sin(phase)
        var ck = c1
        var sk = s1

        var voteCount = 0
        var explainedPower = 0.0
        var activePartials = 0

        // amplitude of a real sinus is ~2*|complex baseband| after demodulation.
        val partialAmplitudeFloor = gateLevel * partialGateScale
        val partialMagnitudeSquaredFloor =
            0.25 * partialAmplitudeFloor * partialAmplitudeFloor

        val nyquist = sampleRate * 0.5

        for (index in 0 until harmonics) {
            val k = index + 1
            val harmonicHz = omega * sampleRate / TWO_PI * k

            // Recurrence values ck/sk are still advanced even for channels above Nyquist so the
            // loop remains branch-free and simple.
            if (harmonicHz < nyquist * 0.98) {
                // x * exp(-j k phi)
                val mixRe = x * ck
                val mixIm = -x * sk

                val add = lpAdd[index]

                lp1Re[index] += add * (mixRe - lp1Re[index])
                lp1Im[index] += add * (mixIm - lp1Im[index])
                lp2Re[index] += add * (lp1Re[index] - lp2Re[index])
                lp2Im[index] += add * (lp1Im[index] - lp2Im[index])

                val re = lp2Re[index]
                val im = lp2Im[index]
                val magnitudeSquared = re * re + im * im

                // For x=A cos(...), |baseband|~=A/2 and sinusoidal average power=A²/2,
                // therefore explained power is approximately 2|z|².
                explainedPower += 2.0 * magnitudeSquared

                if (magnitudeSquared >= partialMagnitudeSquaredFloor) {
                    activePartials++

                    if (previousValid[index]) {
                        val pr = previousRe[index]
                        val pi = previousIm[index]

                        val cross = im * pr - re * pi
                        val dot = re * pr + im * pi
                        val harmonicResidual = atan2(cross, dot)

                        // Convert harmonic residual to FUNDAMENTAL radians/sample.
                        val residual = harmonicResidual / k

                        // Reject absurd jumps before they can affect the robust estimator.
                        val hardLimit =
                            TWO_PI * (captureHz * 2.5) / sampleRate

                        if (abs(residual) <= hardLimit) {
                            residualScratch[voteCount] = residual

                            val harmonicPreference = harmonicPreferences[index]

                            weightScratch[voteCount] =
                                magnitudeSquared * harmonicPreference

                            voteCount++
                        }
                    }

                    previousRe[index] = re
                    previousIm[index] = im
                    previousValid[index] = true
                } else {
                    // Do not measure phase advance across a long interval where this partial was
                    // below the usable floor.
                    previousValid[index] = false
                }
            } else {
                previousValid[index] = false
            }

            // harmonic-angle recurrence:
            // exp(j(k+1)phi) = exp(jkphi) exp(jphi)
            val nextC = ck * c1 - sk * s1
            val nextS = sk * c1 + ck * s1
            ck = nextC
            sk = nextS
        }

        val gateOpen =
            sqrt(broadbandPower.coerceAtLeast(0.0)) >= gateLevel

        val broadband = broadbandPower.coerceAtLeast(1e-20)
        val harmonicity =
            if (gateOpen)
                (explainedPower / broadband).coerceIn(0.0, 1.0)
            else
                0.0

        var fusedResidual = 0.0
        var agreement = 0.0

        if (gateOpen && voteCount > 0) {
            // Sort only the dense scratch prefix. N is normally 8..16, so insertion sort is both
            // allocation-free and cheaper than bringing a general collection sort into audio code.
            insertionSortPairs(residualScratch, weightScratch, voteCount)

            val median = weightedMedian(
                residualScratch,
                weightScratch,
                voteCount
            )

            val outlierRadius =
                TWO_PI * residualOutlierHz / sampleRate

            var weightedResidual = 0.0
            var acceptedWeight = 0.0
            var totalWeight = 0.0

            for (n in 0 until voteCount) {
                val r = residualScratch[n]
                val w = weightScratch[n]
                totalWeight += w

                if (abs(r - median) <= outlierRadius) {
                    weightedResidual += w * r
                    acceptedWeight += w
                }
            }

            if (acceptedWeight > 0.0) {
                fusedResidual = weightedResidual / acceptedWeight

                // Agreement is the fraction of spectral voting weight surviving robust rejection.
                agreement =
                    (acceptedWeight / totalWeight.coerceAtLeast(1e-30))
                        .coerceIn(0.0, 1.0)

                omega =
                    (omega + frequencyAlpha * fusedResidual)
                        .coerceIn(minOmega, maxOmega)
            }
        }

        val instantaneousConfidence =
            if (gateOpen && voteCount > 0)
                (harmonicity * agreement).coerceIn(0.0, 1.0)
            else
                0.0

        smoothedConfidence =
            confidenceKeep * smoothedConfidence +
                (1.0 - confidenceKeep) * instantaneousConfidence

        // The agile coordinate used by lock=1 bank voices.
        val actualAdvance =
            omega + (if (phaseFollow.isFinite()) phaseFollow else 0.0) * fusedResidual

        phase = wrapTrackedPhase(phase + actualAdvance)

        state.phase = phase
        state.phaseAdvance = actualAdvance
        state.omega = omega
        state.level = sqrt(broadbandPower.coerceAtLeast(0.0))
        state.confidence = if (gateOpen) smoothedConfidence else 0.0
        state.gateOpen = gateOpen
        state.locked = gateOpen && smoothedConfidence >= lockThreshold
        state.harmonicity = harmonicity
        state.activePartials = activePartials

        return state
    }

    private fun pushCoarse(x: Double) {
        coarseAccumulator += x
        coarseAccumulatorCount++

        if (coarseAccumulatorCount < coarseDecimation) return

        val coarseSample = coarseAccumulator / coarseDecimation
        coarseAccumulator = 0.0
        coarseAccumulatorCount = 0

        coarseRing[coarseWrite] = coarseSample
        coarseWrite = (coarseWrite + 1) and coarseMask
        coarseCount = (coarseCount + 1).coerceAtMost(coarseRingSize)

        coarseSinceAnalysis++

        if (
            coarseCount < coarseWindow + coarseMaxLag + 2 ||
            coarseSinceAnalysis < coarseHop
        ) {
            return
        }

        if (analysisLag != 0) return
        coarseSinceAnalysis = 0
        for (j in coarseSnapshot.indices)
            coarseSnapshot[j] = coarseRing[(coarseWrite - 1 - j) and coarseMask]
        analysisLag = coarseMinLag
    }

    private fun advanceCoarse() {
        val lag = analysisLag
        if (lag == 0) return
        var cross = 0.0
        var e0 = 0.0
        var e1 = 0.0
        var sum0 = 0.0
        var sum1 = 0.0
        for (j in 0 until coarseWindow) {
            val a = coarseSnapshot[j]
            val b = coarseSnapshot[j + lag]
            cross += a * b; e0 += a * a; e1 += b * b
            sum0 += a; sum1 += b
        }
        // Center both windows so DC cannot masquerade as perfect periodicity.
        cross -= sum0 * sum1 / coarseWindow
        val denominator = e0 + e1 - (sum0 * sum0 + sum1 * sum1) / coarseWindow
        coarseScores[lag] = if (denominator > 1e-20) (2.0 * cross / denominator).coerceIn(-1.0, 1.0) else 0.0
        if (lag < coarseMaxLag) { analysisLag++; return }
        analysisLag = 0
        val estimate = estimateCoarsePeriodicity()
        if (estimate <= 0.0 || broadbandPower < gateLevel * gateLevel) return

        val candidateHz = estimate
        val differenceHz =
            abs(candidateHz - omega * sampleRate / TWO_PI)

        if (differenceHz <= coarseDeadbandHz) return

        val targetOmega =
            (TWO_PI * candidateHz / sampleRate)
                .coerceIn(minOmega, maxOmega)

        val pull =
            if (differenceHz > maxOf(2.0 * captureHz, candidateHz * 0.20)) 0.85
            else if (smoothedConfidence < lockThreshold)
                coarseUnlockedPull
            else
                coarseLockedPull

        val before = omega
        omega += pull * (targetOmega - omega)
        omega = omega.coerceIn(minOmega, maxOmega)

        // A large retune invalidates the fine filters' old baseband phase history.
        if (abs(omega - before) > before * 0.10) {
            clearFineHistory()
        }
    }

    private fun estimateCoarsePeriodicity(): Double {
        var best = -1.0

        for (lag in coarseMinLag..coarseMaxLag) best = maxOf(best, coarseScores[lag])

        if (best < coarseMinCorrelation) {
            coarseHz = 0.0
            coarseCorrelation = best
            return 0.0
        }

        val threshold =
            maxOf(
                coarseMinCorrelation,
                best * coarseRelativePeak
            )

        var chosenLag = -1

        // Smallest lag == highest frequency. The first peak almost as good as the global peak is
        // preferred over equally strong 2T / 3T repetitions.
        for (lag in (coarseMinLag + 1) until coarseMaxLag) {
            val y = coarseScores[lag]
            if (
                y >= threshold &&
                y >= coarseScores[lag - 1] &&
                y > coarseScores[lag + 1]
            ) {
                chosenLag = lag
                break
            }
        }

        if (chosenLag < 0) {
            // Boundary / pathological fallback: choose the global maximum.
            var maxScore = Double.NEGATIVE_INFINITY
            for (lag in coarseMinLag..coarseMaxLag) {
                if (coarseScores[lag] > maxScore) {
                    maxScore = coarseScores[lag]
                    chosenLag = lag
                }
            }
        }

        var refinedLag = chosenLag.toDouble()

        if (chosenLag > coarseMinLag && chosenLag < coarseMaxLag) {
            val ym = coarseScores[chosenLag - 1]
            val y0 = coarseScores[chosenLag]
            val yp = coarseScores[chosenLag + 1]

            val curvature = ym - 2.0 * y0 + yp
            if (abs(curvature) > 1e-12) {
                refinedLag +=
                    (0.5 * (ym - yp) / curvature)
                        .coerceIn(-0.5, 0.5)
            }
        }

        val hz =
            (coarseRate / refinedLag)
                .coerceIn(minHz, maxHz)

        coarseHz = hz
        coarseCorrelation = coarseScores[chosenLag]

        return hz
    }

    private fun clearFineHistory() {
        lp1Re.fill(0.0)
        lp1Im.fill(0.0)
        lp2Re.fill(0.0)
        lp2Im.fill(0.0)
        previousRe.fill(0.0)
        previousIm.fill(0.0)
        previousValid.fill(false)
        smoothedConfidence *= 0.5
    }
}


private fun insertionSortPairs(
    values: DoubleArray,
    weights: DoubleArray,
    count: Int,
) {
    for (i in 1 until count) {
        val v = values[i]
        val w = weights[i]
        var j = i - 1

        while (j >= 0 && values[j] > v) {
            values[j + 1] = values[j]
            weights[j + 1] = weights[j]
            j--
        }

        values[j + 1] = v
        weights[j + 1] = w
    }
}


private fun weightedMedian(
    values: DoubleArray,
    weights: DoubleArray,
    count: Int,
): Double {
    var total = 0.0
    for (n in 0 until count) total += weights[n]

    val halfway = total * 0.5
    var accumulated = 0.0

    for (n in 0 until count) {
        accumulated += weights[n]
        if (accumulated >= halfway) return values[n]
    }

    return values[count - 1]
}


private fun nextPowerOfTwo(value: Int): Int {
    var v = 1
    while (v < value) v = v shl 1
    return v
}