WDF.h

// DSPark -- Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi -- MIT License
 
#pragma once
 
#include "DspMath.h"
 
#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>
#include <tuple>
#include <utility>
 
namespace dspark {
namespace wdf {
 
// ============================================================================
// One-port leaf elements
// ============================================================================
 
template <FloatType T>
class Resistor
{
public:
    explicit Resistor(T resistanceOhms) noexcept : value_(resistanceOhms) {}
 
    void setResistance(T ohms) noexcept { value_ = std::max(ohms, T(1e-9)); }
 
    [[nodiscard]] T getResistance() const noexcept { return value_; }
 
    void prepare(double) noexcept {}
    void updatePorts() noexcept { R_ = static_cast<double>(value_); }
    void reset() noexcept { a_ = 0; }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
    [[nodiscard]] T reflected() noexcept { return T(0); }
    void incident(T a) noexcept { a_ = a; }
 
    [[nodiscard]] T getVoltage() const noexcept { return a_ * T(0.5); }
    [[nodiscard]] T getCurrent() const noexcept
    {
        return static_cast<T>(static_cast<double>(a_) * 0.5 / R_);
    }
 
private:
    T value_;
    double R_ = 1.0;
    T a_ = 0;
};
 
template <FloatType T>
class Capacitor
{
public:
    explicit Capacitor(T farads) noexcept : value_(farads) {}
 
    void setCapacitance(T farads) noexcept { value_ = std::max(farads, T(1e-18)); }
 
    [[nodiscard]] T getCapacitance() const noexcept { return value_; }
 
    void prepare(double sampleRate) noexcept { fs_ = sampleRate; }
    void updatePorts() noexcept { R_ = 1.0 / (2.0 * fs_ * static_cast<double>(value_)); }
    void reset() noexcept { state_ = 0; a_ = 0; b_ = 0; }
 
    void offsetVoltage(T offset) noexcept
    {
        state_ += offset;
        a_ += offset;
        b_ += offset;
    }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
    [[nodiscard]] T reflected() noexcept { b_ = state_; return b_; }
    void incident(T a) noexcept { a_ = a; state_ = a; }
 
    [[nodiscard]] T getVoltage() const noexcept { return (a_ + b_) * T(0.5); }
    [[nodiscard]] T getCurrent() const noexcept
    {
        return static_cast<T>(static_cast<double>(a_ - b_) * 0.5 / R_);
    }
 
private:
    T value_;
    double fs_ = 48000.0;
    double R_ = 1.0;
    T state_ = 0, a_ = 0, b_ = 0;
};
 
template <FloatType T>
class Inductor
{
public:
    explicit Inductor(T henries) noexcept : value_(henries) {}
 
    void setInductance(T henries) noexcept { value_ = std::max(henries, T(1e-12)); }
 
    void prepare(double sampleRate) noexcept { fs_ = sampleRate; }
    void updatePorts() noexcept { R_ = 2.0 * fs_ * static_cast<double>(value_); }
    void reset() noexcept { state_ = 0; a_ = 0; b_ = 0; }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
    [[nodiscard]] T reflected() noexcept { b_ = -state_; return b_; }
    void incident(T a) noexcept { a_ = a; state_ = a; }
 
    [[nodiscard]] T getVoltage() const noexcept { return (a_ + b_) * T(0.5); }
    [[nodiscard]] T getCurrent() const noexcept
    {
        return static_cast<T>(static_cast<double>(a_ - b_) * 0.5 / R_);
    }
 
private:
    T value_;
    double fs_ = 48000.0;
    double R_ = 1.0;
    T state_ = 0, a_ = 0, b_ = 0;
};
 
template <FloatType T>
class ResistiveVoltageSource
{
public:
    explicit ResistiveVoltageSource(T seriesResistanceOhms) noexcept
        : value_(seriesResistanceOhms) {}
 
    void setResistance(T ohms) noexcept { value_ = std::max(ohms, T(1e-9)); }
    void setVoltage(T volts) noexcept { vs_ = volts; }
 
    void prepare(double) noexcept {}
    void updatePorts() noexcept { R_ = static_cast<double>(value_); }
    void reset() noexcept { a_ = 0; }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
    [[nodiscard]] T reflected() noexcept { return vs_; }
    void incident(T a) noexcept { a_ = a; }
 
    [[nodiscard]] T getVoltage() const noexcept { return (a_ + vs_) * T(0.5); }
 
private:
    T value_;
    double R_ = 1.0;
    T vs_ = 0, a_ = 0;
};
 
// ============================================================================
// Connectors
// ============================================================================
 
template <FloatType T, typename Child1, typename Child2>
class Series
{
public:
    Series(Child1& c1, Child2& c2) noexcept : c1_(c1), c2_(c2) {}
 
    void prepare(double sampleRate) noexcept
    {
        c1_.prepare(sampleRate);
        c2_.prepare(sampleRate);
    }
    void updatePorts() noexcept
    {
        c1_.updatePorts();
        c2_.updatePorts();
        const double r1 = c1_.portResistance();
        const double r2 = c2_.portResistance();
        R_ = r1 + r2;
        gamma1_ = static_cast<T>(r1 / R_);
    }
    void reset() noexcept { c1_.reset(); c2_.reset(); a1_ = 0; a2_ = 0; }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
 
    [[nodiscard]] T reflected() noexcept
    {
        a1_ = c1_.reflected();
        a2_ = c2_.reflected();
        return a1_ + a2_;             // -(a1+a2) with the up-port inversion folded in
    }
 
    void incident(T a3) noexcept
    {
        // b_k = a_k - gamma_k * (a1 + a2 + a3'), a3' = -a3 (folded inversion),
        // with gamma1 + gamma2 = 1 on the adapted port.
        const T sum = a1_ + a2_ - a3;
        c1_.incident(a1_ - gamma1_ * sum);
        c2_.incident(a2_ - (sum - gamma1_ * sum));   // a2 - gamma2 * sum
    }
 
private:
    Child1& c1_;
    Child2& c2_;
    double R_ = 2.0;
    T gamma1_ = T(0.5);
    T a1_ = 0, a2_ = 0;
};
 
template <FloatType T, typename Child1, typename Child2>
class Parallel
{
public:
    Parallel(Child1& c1, Child2& c2) noexcept : c1_(c1), c2_(c2) {}
 
    void prepare(double sampleRate) noexcept
    {
        c1_.prepare(sampleRate);
        c2_.prepare(sampleRate);
    }
    void updatePorts() noexcept
    {
        c1_.updatePorts();
        c2_.updatePorts();
        const double g1 = 1.0 / c1_.portResistance();
        const double g2 = 1.0 / c2_.portResistance();
        R_ = 1.0 / (g1 + g2);
        d1_ = static_cast<T>(g1 / (g1 + g2));
    }
    void reset() noexcept { c1_.reset(); c2_.reset(); a1_ = 0; a2_ = 0; bUp_ = 0; }
 
    [[nodiscard]] double portResistance() const noexcept { return R_; }
 
    [[nodiscard]] T reflected() noexcept
    {
        a1_ = c1_.reflected();
        a2_ = c2_.reflected();
        bUp_ = d1_ * a1_ + (T(1) - d1_) * a2_;   // weighted node wave
        return bUp_;
    }
 
    void incident(T a3) noexcept
    {
        // Common node wave: bNode = bUp + a3; then b_k = bNode - a_k.
        const T bNode = bUp_ + a3;
        c1_.incident(bNode - a1_);
        c2_.incident(bNode - a2_);
    }
 
private:
    Child1& c1_;
    Child2& c2_;
    double R_ = 0.5;
    T d1_ = T(0.5);
    T a1_ = 0, a2_ = 0, bUp_ = 0;
};
 
template <FloatType T, typename Child>
class Inverter
{
public:
    explicit Inverter(Child& c) noexcept : c_(c) {}
 
    void prepare(double sampleRate) noexcept { c_.prepare(sampleRate); }
    void updatePorts() noexcept { c_.updatePorts(); }
    void reset() noexcept { c_.reset(); }
 
    [[nodiscard]] double portResistance() const noexcept { return c_.portResistance(); }
    [[nodiscard]] T reflected() noexcept { return -c_.reflected(); }
    void incident(T a) noexcept { c_.incident(-a); }
 
private:
    Child& c_;
};
 
// ============================================================================
// Roots
// ============================================================================
 
template <FloatType T, typename Tree>
class IdealVoltageSourceRoot
{
public:
    explicit IdealVoltageSourceRoot(Tree& tree) noexcept : tree_(tree) {}
 
    void setVoltage(T volts) noexcept { vs_ = volts; }
 
    void prepare(double sampleRate) noexcept
    {
        tree_.prepare(sampleRate);
        tree_.updatePorts();
        tree_.reset();
    }
    void reset() noexcept { tree_.reset(); }
 
    void process() noexcept
    {
        const T a = tree_.reflected();
        tree_.incident(T(2) * vs_ - a);
    }
 
private:
    Tree& tree_;
    T vs_ = 0;
};
 
namespace detail {
 
template <typename F>
[[nodiscard]] inline double solveMonotonic(double a, double seed, F&& evaluate) noexcept
{
    double lo = std::min(0.0, a);
    double hi = std::max(0.0, a);
    double v = std::clamp(seed, lo, hi);
    double prevAbsF = 1e300;
 
    for (int it = 0; it < 48; ++it)
    {
        double f = 0.0, fp = 1.0;
        evaluate(v, f, fp);
 
        const double absF = std::abs(f);
        if (absF < 1e-10)
            break;
        if (f > 0.0) hi = v;
        else         lo = v;
 
        // Accept the Newton step only while it makes real progress. Deep in
        // the exponential region Newton crawls at ~nVt per step (the classic
        // diode pathology), so a stalled |f| switches to bisection, which
        // halves the bracket unconditionally. Quadratic near the root,
        // bisection-guaranteed everywhere.
        const double vn = v - f / fp;
        if (vn > lo && vn < hi && absF < 0.7 * prevAbsF)
            v = vn;
        else
            v = 0.5 * (lo + hi);
        prevAbsF = absF;
    }
    return v;
}
 
} // namespace detail
 
template <FloatType T, typename Tree>
class DiodePairRoot
{
public:
    explicit DiodePairRoot(Tree& tree, T saturationCurrent = T(2.52e-9),
                           T idealityTimesVt = T(1.752 * 0.02585)) noexcept
        : tree_(tree), is_(saturationCurrent), nvt_(idealityTimesVt) {}
 
    void setSaturationCurrent(T amps) noexcept { is_ = std::max(amps, T(1e-15)); }
    void setIdealityTimesVt(T volts) noexcept { nvt_ = std::max(volts, T(1e-4)); }
 
    void prepare(double sampleRate) noexcept
    {
        tree_.prepare(sampleRate);
        tree_.updatePorts();
        tree_.reset();
        v_ = 0.0;
    }
    void reset() noexcept { tree_.reset(); v_ = 0.0; }
 
    void process() noexcept
    {
        const double a = static_cast<double>(tree_.reflected());
        const double k = 2.0 * tree_.portResistance() * static_cast<double>(is_);
        const double nvt = static_cast<double>(nvt_);
 
        // Far-field analytic seed: neglecting the linear term in
        // v + k sinh(v/nVt) = a gives v0 = nVt asinh(a/k), within ~nVt of the
        // root at any drive level (deep in the exponential, plain Newton
        // would crawl at ~nVt per iteration). Newton then converges in 1-3.
        const double seed = nvt * std::asinh(a / k);
 
        v_ = detail::solveMonotonic(a, seed, [k, nvt, a](double v, double& f, double& fp)
        {
            // Guard the argument: cosh overflows around |x| > 710 in double,
            // far outside any solution the bracket allows anyway.
            const double x = std::clamp(v / nvt, -700.0, 700.0);
            f  = v + k * std::sinh(x) - a;
            fp = 1.0 + (k / nvt) * std::cosh(x);
        });
 
        tree_.incident(static_cast<T>(2.0 * v_ - a));
    }
 
    [[nodiscard]] T getVoltage() const noexcept { return static_cast<T>(v_); }
 
private:
    Tree& tree_;
    T is_, nvt_;
    double v_ = 0.0;
};
 
template <FloatType T, typename Tree>
class DiodeRoot
{
public:
    explicit DiodeRoot(Tree& tree, T saturationCurrent = T(2.52e-9),
                       T idealityTimesVt = T(1.752 * 0.02585)) noexcept
        : tree_(tree), is_(saturationCurrent), nvt_(idealityTimesVt) {}
 
    void setSaturationCurrent(T amps) noexcept { is_ = std::max(amps, T(1e-15)); }
    void setIdealityTimesVt(T volts) noexcept { nvt_ = std::max(volts, T(1e-4)); }
 
    void prepare(double sampleRate) noexcept
    {
        tree_.prepare(sampleRate);
        tree_.updatePorts();
        tree_.reset();
        v_ = 0.0;
    }
    void reset() noexcept { tree_.reset(); v_ = 0.0; }
 
    void process() noexcept
    {
        const double a = static_cast<double>(tree_.reflected());
        const double k = tree_.portResistance() * static_cast<double>(is_);
        const double nvt = static_cast<double>(nvt_);
 
        // Far-field analytic seed: forward drive lands near
        // nVt ln(1 + a/k); reverse drive blocks, so the root sits near a.
        const double seed = (a > 0.0) ? nvt * std::log1p(a / k) : a;
 
        v_ = detail::solveMonotonic(a, seed, [k, nvt, a](double v, double& f, double& fp)
        {
            const double x = std::clamp(v / nvt, -700.0, 700.0);
            f  = v + k * std::expm1(x) - a;
            fp = 1.0 + (k / nvt) * std::exp(x);
        });
 
        tree_.incident(static_cast<T>(2.0 * v_ - a));
    }
 
    [[nodiscard]] T getVoltage() const noexcept { return static_cast<T>(v_); }
 
private:
    Tree& tree_;
    T is_, nvt_;
    double v_ = 0.0;
};
 
// ============================================================================
// R-type adaptor (arbitrary topologies)
// ============================================================================
 
template <FloatType T, typename... Children>
class RType
{
    template <FloatType> friend class ToneStackFMV;
public:
    static constexpr int kNumPorts = static_cast<int>(sizeof...(Children)) + 1;
    static constexpr int kMaxNodes = 12;
    static_assert(sizeof...(Children) >= 1, "RType needs at least one child");
 
    RType(const std::array<std::pair<int, int>, static_cast<size_t>(kNumPorts)>& portNodes,
          int numNodes, Children&... children) noexcept
        : children_(children...), portNodes_(portNodes),
          numNodes_(std::clamp(numNodes, 1, kMaxNodes))
    {
        assert(numNodes >= 1 && numNodes <= kMaxNodes);
        // Release-safe node sanitising: an out-of-range node index would
        // stamp the conductance/RHS arrays out of bounds. Clamp into
        // [-1, numNodes_-1] (-1 = ground); a clamped topology is wrong but
        // cannot corrupt memory.
        for (auto& [p, m] : portNodes_)
        {
            assert(p >= -1 && p < numNodes_ && m >= -1 && m < numNodes_);
            p = std::clamp(p, -1, numNodes_ - 1);
            m = std::clamp(m, -1, numNodes_ - 1);
        }
    }
 
    void prepare(double sampleRate) noexcept
    {
        std::apply([&](auto&... ch) { (ch.prepare(sampleRate), ...); }, children_);
    }
 
    void updatePorts() noexcept
    {
        std::apply([&](auto&... ch) { (ch.updatePorts(), ...); }, children_);
 
        // Gather child port resistances (port 0 resolved by adaptation).
        {
            int i = 1;
            std::apply([&](auto&... ch)
                       { ((portR_[static_cast<size_t>(i++)] = ch.portResistance()), ...); },
                       children_);
        }
 
        // 1) Thevenin resistance looking into the network from port 0:
        //    assemble MNA without port 0, inject 1 A across its nodes.
        assembleConductance(false);
        factor();
        double rhs[kMaxNodes] = {};
        stampCurrent(rhs, 0, 1.0);
        solve(rhs);
        double rth = portVoltage(rhs, 0);
        portR_[0] = std::clamp(rth, 1e-6, 1e12);
 
        // 2) Full MNA with every port loaded; S = 2M - I where column j of M
        //    is the port-voltage response to a_j = 1 (Norton: 1/R_j).
        assembleConductance(true);
        factor();
        for (int j = 0; j < kNumPorts; ++j)
        {
            double v[kMaxNodes] = {};
            stampCurrent(v, j, 1.0 / portR_[static_cast<size_t>(j)]);
            solve(v);
            for (int i = 0; i < kNumPorts; ++i)
            {
                const double m = portVoltage(v, i);
                s_[static_cast<size_t>(i)][static_cast<size_t>(j)] =
                    2.0 * m - (i == j ? 1.0 : 0.0);
            }
        }
        // Adaptation leaves no instantaneous reflection at the up port.
        s_[0][0] = 0.0;
    }
 
    void reset() noexcept
    {
        std::apply([&](auto&... ch) { (ch.reset(), ...); }, children_);
        a_.fill(0.0);
    }
 
    [[nodiscard]] double portResistance() const noexcept { return portR_[0]; }
 
    [[nodiscard]] T reflected() noexcept
    {
        int i = 1;
        std::apply([&](auto&... ch)
                   { ((a_[static_cast<size_t>(i++)] = static_cast<double>(ch.reflected())), ...); },
                   children_);
        double b0 = 0.0;
        for (int j = 1; j < kNumPorts; ++j)
            b0 += s_[0][static_cast<size_t>(j)] * a_[static_cast<size_t>(j)];
        return static_cast<T>(b0);
    }
 
    void incident(T aUp) noexcept
    {
        a_[0] = static_cast<double>(aUp);
        int i = 1;
        std::apply([&](auto&... ch)
                   {
                       ((ch.incident(static_cast<T>(rowDot(i))), ++i), ...);
                   },
                   children_);
    }
 
private:
    // Preserve a topology-proven zero-current voltage mode after the nodal
    // solve: S*v = v. For the FMV network the source and three coupling
    // capacitors share a common DC voltage; every resistor has zero voltage.
    // Correcting only the final nonzero column removes factorization roundoff
    // in this identity without adding arithmetic to sample processing.
    void preserveDcMode(const std::array<double, static_cast<size_t>(kNumPorts)>& mode) noexcept
    {
        int pivot = kNumPorts - 1;
        while (pivot > 0 && mode[static_cast<size_t>(pivot)] == 0.0) --pivot;
        if (pivot == 0) return; // Keep the adapted up-port reflection at zero.
        for (int i = 0; i < kNumPorts; ++i)
        {
            double remainder = 0.0;
            for (int j = 0; j < kNumPorts; ++j)
                if (j != pivot)
                    remainder += s_[static_cast<size_t>(i)][static_cast<size_t>(j)]
                               * mode[static_cast<size_t>(j)];
            s_[static_cast<size_t>(i)][static_cast<size_t>(pivot)] =
                (mode[static_cast<size_t>(i)] - remainder) / mode[static_cast<size_t>(pivot)];
        }
    }
 
    [[nodiscard]] double rowDot(int row) const noexcept
    {
        double acc = 0.0;
        for (int j = 0; j < kNumPorts; ++j)
            acc += s_[static_cast<size_t>(row)][static_cast<size_t>(j)] * a_[static_cast<size_t>(j)];
        return acc;
    }
 
    void assembleConductance(bool includePort0) noexcept
    {
        for (auto& row : g_) row.fill(0.0);
        for (int j = includePort0 ? 0 : 1; j < kNumPorts; ++j)
        {
            const double g = 1.0 / portR_[static_cast<size_t>(j)];
            const int p = portNodes_[static_cast<size_t>(j)].first;
            const int m = portNodes_[static_cast<size_t>(j)].second;
            if (p >= 0) g_[static_cast<size_t>(p)][static_cast<size_t>(p)] += g;
            if (m >= 0) g_[static_cast<size_t>(m)][static_cast<size_t>(m)] += g;
            if (p >= 0 && m >= 0)
            {
                g_[static_cast<size_t>(p)][static_cast<size_t>(m)] -= g;
                g_[static_cast<size_t>(m)][static_cast<size_t>(p)] -= g;
            }
        }
    }
 
    void stampCurrent(double* rhs, int port, double amps) const noexcept
    {
        const int p = portNodes_[static_cast<size_t>(port)].first;
        const int m = portNodes_[static_cast<size_t>(port)].second;
        if (p >= 0) rhs[p] += amps;
        if (m >= 0) rhs[m] -= amps;
    }
 
    [[nodiscard]] double portVoltage(const double* v, int port) const noexcept
    {
        const int p = portNodes_[static_cast<size_t>(port)].first;
        const int m = portNodes_[static_cast<size_t>(port)].second;
        return (p >= 0 ? v[p] : 0.0) - (m >= 0 ? v[m] : 0.0);
    }
 
    void factor() noexcept
    {
        const int n = numNodes_;
        for (int i = 0; i < n; ++i)
        {
            for (int j = 0; j < n; ++j)
                lu_[static_cast<size_t>(i)][static_cast<size_t>(j)] =
                    g_[static_cast<size_t>(i)][static_cast<size_t>(j)];
            piv_[static_cast<size_t>(i)] = i;
        }
        for (int k = 0; k < n; ++k)
        {
            int p = k;
            for (int i = k + 1; i < n; ++i)
                if (std::abs(lu_[static_cast<size_t>(i)][static_cast<size_t>(k)])
                    > std::abs(lu_[static_cast<size_t>(p)][static_cast<size_t>(k)]))
                    p = i;
            if (p != k)
            {
                std::swap(lu_[static_cast<size_t>(p)], lu_[static_cast<size_t>(k)]);
                std::swap(piv_[static_cast<size_t>(p)], piv_[static_cast<size_t>(k)]);
            }
            double d = lu_[static_cast<size_t>(k)][static_cast<size_t>(k)];
            if (std::abs(d) < 1e-300)
                d = (d >= 0.0 ? 1e-300 : -1e-300);
            const double invD = 1.0 / d;
            for (int i = k + 1; i < n; ++i)
            {
                const double f = lu_[static_cast<size_t>(i)][static_cast<size_t>(k)] * invD;
                lu_[static_cast<size_t>(i)][static_cast<size_t>(k)] = f;
                for (int j = k + 1; j < n; ++j)
                    lu_[static_cast<size_t>(i)][static_cast<size_t>(j)]
                        -= f * lu_[static_cast<size_t>(k)][static_cast<size_t>(j)];
            }
        }
    }
 
    void solve(double* rhs) const noexcept
    {
        const int n = numNodes_;
        double y[kMaxNodes];
        for (int i = 0; i < n; ++i)
            y[i] = rhs[piv_[static_cast<size_t>(i)]];
        for (int i = 0; i < n; ++i)
            for (int j = 0; j < i; ++j)
                y[i] -= lu_[static_cast<size_t>(i)][static_cast<size_t>(j)] * y[j];
        for (int i = n - 1; i >= 0; --i)
        {
            for (int j = i + 1; j < n; ++j)
                y[i] -= lu_[static_cast<size_t>(i)][static_cast<size_t>(j)] * y[j];
            double d = lu_[static_cast<size_t>(i)][static_cast<size_t>(i)];
            if (std::abs(d) < 1e-300)
                d = (d >= 0.0 ? 1e-300 : -1e-300);
            y[i] /= d;
        }
        for (int i = 0; i < n; ++i)
            rhs[i] = y[i];
    }
 
    std::tuple<Children&...> children_;
    std::array<std::pair<int, int>, static_cast<size_t>(kNumPorts)> portNodes_;
    int numNodes_;
 
    std::array<double, static_cast<size_t>(kNumPorts)> portR_ {};
    std::array<double, static_cast<size_t>(kNumPorts)> a_ {};
    std::array<std::array<double, static_cast<size_t>(kNumPorts)>,
               static_cast<size_t>(kNumPorts)> s_ {};
 
    std::array<std::array<double, kMaxNodes>, kMaxNodes> g_ {};
    std::array<std::array<double, kMaxNodes>, kMaxNodes> lu_ {};
    std::array<int, kMaxNodes> piv_ {};
};
 
// ============================================================================
// Fender '59 Bassman FMV tone stack (reference R-type circuit)
// ============================================================================
 
template <FloatType T>
class ToneStackFMV
{
public:
    explicit ToneStackFMV(double sourceResistance = 1e3, double loadResistance = 1e6)
        : rOut_(static_cast<T>(sourceResistance)), c1_(T(0.25e-9)),
          r1Top_(T(125e3)), r1Bot_(T(125e3)), r4_(T(56e3)), c2_(T(20e-9)),
          r2_(T(500e3)), c3_(T(20e-9)), r3Top_(T(12.5e3)), r3Bot_(T(12.5e3)),
          rLoad_(static_cast<T>(loadResistance)),
          rtype_({ { { kSrc, -1 },          // port 0: ideal source root
                    { kSrc, kVi },          // rOut
                    { kVi, kA },            // C1
                    { kA, kVo },            // (1-t) R1
                    { kVo, kB },            // t R1
                    { kVi, kS },            // R4
                    { kS, kB },             // C2
                    { kB, kC },             // l R2 (rheostat)
                    { kS, kW },             // C3 to the middle wiper
                    { kC, kW },             // (1-m) R3
                    { kW, -1 },             // m R3 to ground
                    { kVo, -1 } } },        // grid load
                 kNumNodes,
                 rOut_, c1_, r1Top_, r1Bot_, r4_, c2_, r2_, c3_, r3Top_, r3Bot_, rLoad_),
          root_(rtype_)
    {
    }
 
    void prepare(double sampleRate) noexcept
    {
        root_.prepare(sampleRate);
        setControls(treble_, bass_, middle_);
    }
 
    void reset() noexcept { root_.reset(); }
 
    void copyStateFrom(const ToneStackFMV& source, T inputOffset = T(0)) noexcept
    {
        c1_ = source.c1_;
        c2_ = source.c2_;
        c3_ = source.c3_;
        if (inputOffset != T(0))
        {
            c1_.offsetVoltage(inputOffset);
            c2_.offsetVoltage(inputOffset);
            c3_.offsetVoltage(inputOffset);
        }
    }
 
    void setControls(T treble, T bass, T middle) noexcept
    {
        treble_ = std::clamp(treble, T(0), T(1));
        bass_ = std::clamp(bass, T(0), T(1));
        middle_ = std::clamp(middle, T(0), T(1));
 
        const double t = static_cast<double>(treble_);
        const double l = static_cast<double>(bass_) * static_cast<double>(bass_);
        const double m = static_cast<double>(middle_);
        constexpr double kRmin = 0.5;   // keeps the node matrix well-posed
 
        r1Top_.setResistance(static_cast<T>((1.0 - t) * 250e3 + kRmin));
        r1Bot_.setResistance(static_cast<T>(t * 250e3 + kRmin));
        r2_.setResistance(static_cast<T>(l * 1e6 + kRmin));
        r3Top_.setResistance(static_cast<T>((1.0 - m) * 25e3 + kRmin));
        r3Bot_.setResistance(static_cast<T>(m * 25e3 + kRmin));
        rtype_.updatePorts();           // rebuild scattering, keep states
        rtype_.preserveDcMode({1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0});
    }
 
    [[nodiscard]] T processSample(T input) noexcept
    {
        root_.setVoltage(input);
        root_.process();
        return rLoad_.getVoltage();
    }
 
    struct AnalogStateSpace
    {
        double a[3][3];
        double b[3];
        double c[3];
        double d;
        double capacitance[3];
    };
 
    [[nodiscard]] AnalogStateSpace analogStateSpace() const noexcept
    {
        // Unknowns: node voltages kVi..kW (7) then the three capacitor currents.
        constexpr int kN = 10;
        struct Element { int p, m; double value; bool capacitor; };
        const Element elements[] = {
            { kSrc, kVi, static_cast<double>(rOut_.getResistance()), false },
            { kVi, kA, static_cast<double>(c1_.getCapacitance()), true },
            { kA, kVo, static_cast<double>(r1Top_.getResistance()), false },
            { kVo, kB, static_cast<double>(r1Bot_.getResistance()), false },
            { kVi, kS, static_cast<double>(r4_.getResistance()), false },
            { kS, kB, static_cast<double>(c2_.getCapacitance()), true },
            { kB, kC, static_cast<double>(r2_.getResistance()), false },
            { kS, kW, static_cast<double>(c3_.getCapacitance()), true },
            { kC, kW, static_cast<double>(r3Top_.getResistance()), false },
            { kW, -1, static_cast<double>(r3Bot_.getResistance()), false },
            { kVo, -1, static_cast<double>(rLoad_.getResistance()), false } };
        double m[kN][kN] {};
        double rhs[kN][4] {};   // columns: unit C1, C2, C3 voltages, unit source
        int capIndex = 0;
        AnalogStateSpace out {};
        for (const auto& e : elements)
        {
            const int p = e.p - 1, q = e.m < 0 ? -1 : e.m - 1;   // kSrc maps to -1
            if (e.capacitor)
            {
                const int row = 7 + capIndex;
                if (p >= 0) { m[p][row] += 1.0; m[row][p] += 1.0; }
                if (q >= 0) { m[q][row] -= 1.0; m[row][q] -= 1.0; }
                rhs[row][capIndex] = 1.0;
                out.capacitance[capIndex] = e.value;
                ++capIndex;
                continue;
            }
            const double g = 1.0 / e.value;
            if (e.p == kSrc)
            {
                // Source node is driven: its conductance stamps the RHS.
                m[q][q] += g;
                rhs[q][3] += g;
                continue;
            }
            if (p >= 0) m[p][p] += g;
            if (q >= 0) m[q][q] += g;
            if (p >= 0 && q >= 0) { m[p][q] -= g; m[q][p] -= g; }
        }
        // Gaussian elimination with partial pivoting, four right-hand sides.
        for (int k = 0; k < kN; ++k)
        {
            int piv = k;
            for (int i = k + 1; i < kN; ++i)
                if (std::abs(m[i][k]) > std::abs(m[piv][k])) piv = i;
            if (piv != k)
                for (int j = 0; j < kN; ++j) std::swap(m[piv][j], m[k][j]);
            if (piv != k)
                for (int j = 0; j < 4; ++j) std::swap(rhs[piv][j], rhs[k][j]);
            for (int i = k + 1; i < kN; ++i)
            {
                const double f = m[i][k] / m[k][k];
                if (f == 0.0) continue;
                for (int j = k; j < kN; ++j) m[i][j] -= f * m[k][j];
                for (int j = 0; j < 4; ++j) rhs[i][j] -= f * rhs[k][j];
            }
        }
        for (int i = kN - 1; i >= 0; --i)
            for (int j = 0; j < 4; ++j)
            {
                double v = rhs[i][j];
                for (int k = i + 1; k < kN; ++k) v -= m[i][k] * rhs[k][j];
                rhs[i][j] = v / m[i][i];
            }
        // Capacitor current flows from its first node to its second node.
        for (int j = 0; j < 4; ++j)
        {
            for (int k = 0; k < 3; ++k)
            {
                const double dv = rhs[7 + k][j] / out.capacitance[k];
                if (j < 3) out.a[k][j] = dv;
                else out.b[k] = dv;
            }
            if (j < 3) out.c[j] = rhs[kVo - 1][j];
            else out.d = rhs[kVo - 1][j];
        }
        return out;
    }
 
private:
    static constexpr int kSrc = 0, kVi = 1, kA = 2, kVo = 3,
                         kB = 4, kS = 5, kC = 6, kW = 7;
    static constexpr int kNumNodes = 8;
 
    Resistor<T> rOut_;
    Capacitor<T> c1_;
    Resistor<T> r1Top_, r1Bot_, r4_;
    Capacitor<T> c2_;
    Resistor<T> r2_;
    Capacitor<T> c3_;
    Resistor<T> r3Top_, r3Bot_, rLoad_;
 
    using StackRType = RType<T, Resistor<T>, Capacitor<T>, Resistor<T>, Resistor<T>,
                             Resistor<T>, Capacitor<T>, Resistor<T>, Capacitor<T>,
                             Resistor<T>, Resistor<T>, Resistor<T>>;
    StackRType rtype_;
    IdealVoltageSourceRoot<T, StackRType> root_;
 
    T treble_ = T(0.5), bass_ = T(0.5), middle_ = T(0.5);
};
 
} // namespace wdf
} // namespace dspark