saturator.hpp

Go to the documentation of this file.

#pragma once

 
#include <pulp/signal/biquad.hpp>
#include <pulp/signal/denormal.hpp>
#include <pulp/signal/oversampling.hpp>
#include <pulp/signal/units.hpp>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <array>
 
namespace pulp::signal {

enum class SaturatorShape : std::uint8_t {
    tanh_soft,   
    atan_soft,   
    cubic_soft,  
    sinh_arc,    
};

enum class SaturatorAliasPolicy : std::uint8_t {
    adaa,          
    oversample_2x, 
    off,           
    adaa2,         
};
 
namespace detail {

struct SaturatorShapeMath {
    static double f(SaturatorShape shape, double u) {
        switch (shape) {
            case SaturatorShape::tanh_soft: return std::tanh(u);
            case SaturatorShape::atan_soft: return std::atan(u);
            case SaturatorShape::cubic_soft:
                if (u > 1.0) return 2.0 / 3.0;
                if (u < -1.0) return -2.0 / 3.0;
                return u - u * u * u / 3.0;
            case SaturatorShape::sinh_arc: return std::asinh(u);
        }
        return u;
    }

    static double df(SaturatorShape shape, double u) {
        switch (shape) {
            case SaturatorShape::tanh_soft: {
                const double c = std::cosh(u);
                return 1.0 / (c * c);
            }
            case SaturatorShape::atan_soft: return 1.0 / (1.0 + u * u);
            case SaturatorShape::cubic_soft: return std::abs(u) < 1.0 ? 1.0 - u * u : 0.0;
            case SaturatorShape::sinh_arc: return 1.0 / std::sqrt(1.0 + u * u);
        }
        return 1.0;
    }

    static double antiderivative(SaturatorShape shape, double u) {
        switch (shape) {
            case SaturatorShape::tanh_soft:
                // ln(cosh u), computed so large |u| does not overflow cosh:
                // ln(cosh u) = |u| + ln((1 + e^(−2|u|)) / 2).
                {
                    const double a = std::abs(u);
                    return a + std::log1p(std::exp(-2.0 * a)) - 0.6931471805599453;
                }
            case SaturatorShape::atan_soft:
                return u * std::atan(u) - 0.5 * std::log1p(u * u);
            case SaturatorShape::cubic_soft: {
                const double a = std::abs(u);
                if (a < 1.0) return u * u * 0.5 - u * u * u * u / 12.0;
                // Even, not odd: integrating the constant ±2/3 outward from
                // |u| = 1 in both directions gives the same magnitude. The
                // 5/12 matches the interior branch at |u| = 1 (1/2 − 1/12), so
                // F1 is continuous there.
                return (2.0 / 3.0) * (a - 1.0) + 5.0 / 12.0;
            }
            case SaturatorShape::sinh_arc:
                return u * std::asinh(u) - std::sqrt(1.0 + u * u);
        }
        return 0.5 * u * u;
    }

    static double second_antiderivative(SaturatorShape shape, double u) {
        switch (shape) {
        case SaturatorShape::tanh_soft:
            return tanh_second_antiderivative(u);
        case SaturatorShape::atan_soft:
            return 0.5 * (u * u - 1.0) * std::atan(u) - 0.5 * u * std::log1p(u * u) + 0.5 * u;
        case SaturatorShape::cubic_soft: {
            const double a = std::abs(u);
            if (a < 1.0)
                return u * u * u / 6.0 - u * u * u * u * u / 60.0;
            // Odd continuation of the clipped branch: integrating
            // F1 = (2/3)(a − 1) + 5/12 outward from F2(1) = 3/20.
            const double r = a - 1.0;
            return std::copysign(r * r / 3.0 + (5.0 / 12.0) * r + 3.0 / 20.0, u);
        }
        case SaturatorShape::sinh_arc: {
            const double root = std::sqrt(1.0 + u * u);
            return 0.25 * (2.0 * u * u - 1.0) * std::asinh(u) - 0.75 * u * root;
        }
        }
        return u * u * u / 6.0;
    }

    static double tanh_second_antiderivative(double u) {
        const double a = std::abs(u);
        const double s = std::log1p(std::exp(-2.0 * a));
        const double s2 = s * s;
        // Li₂(w) = s − s²/4 + s³·P(s²), P from B₂ … B₁₆.
        const double odd =
            1.0 / 36.0 +
            s2 * (-1.0 / 3600.0 +
                  s2 * (1.0 / 211680.0 + s2 * (-1.0 / 10886400.0 +
                                               s2 * (1.0 / 526901760.0 +
                                                     s2 * (-4.0647616451442256e-11 +
                                                           s2 * (8.921691020456452e-13 +
                                                                 s2 * -1.9939295860721074e-14))))));
        const double li2_w = s - 0.25 * s2 + s * s2 * odd;
        constexpr double kLn2 = 0.69314718055994530942;
        constexpr double kPiSquaredOver24 = 0.41123351671205660911;
        const double magnitude =
            0.5 * a * a - kLn2 * a - 0.5 * li2_w - 0.25 * s2 + kPiSquaredOver24;
        return std::copysign(magnitude, u);
    }
};
 
}  // namespace detail

inline double adaa_linear_magnitude(int order, double cycles_per_sample) noexcept {
    constexpr double kPi = 3.14159265358979323846;
    if (order <= 0)
        return 1.0;
    if (order == 1)
        return std::abs(std::cos(kPi * cycles_per_sample));
    return std::abs(1.0 + 2.0 * std::cos(2.0 * kPi * cycles_per_sample)) / 3.0;
}

class SaturatorAdaa2 {
  public:
    static constexpr double kTolerance = 1e-4;

    void reset(SaturatorShape shape, double previous, double before) {
        u_[0] = previous;
        u_[1] = before;
        f2_[0] = detail::SaturatorShapeMath::second_antiderivative(shape, previous);
        f2_[1] = detail::SaturatorShapeMath::second_antiderivative(shape, before);
    }

    double step(SaturatorShape shape, double u) {
        const double f2 = detail::SaturatorShapeMath::second_antiderivative(shape, u);
        // Sort (u, u_[0], u_[1]) with their F2 values.
        double p[3] = {u, u_[0], u_[1]};
        double q[3] = {f2, f2_[0], f2_[1]};
        u_[1] = u_[0];
        f2_[1] = f2_[0];
        u_[0] = u;
        f2_[0] = f2;
        if (p[0] > p[1])
            swap_pair(p, q, 0, 1);
        if (p[1] > p[2])
            swap_pair(p, q, 1, 2);
        if (p[0] > p[1])
            swap_pair(p, q, 0, 1);
 
        const double spread = p[2] - p[0];
        const double scale = std::max(1.0, std::max(std::abs(p[0]), std::abs(p[2])));
        const double tolerance = kTolerance * scale;
        if (spread <= tolerance)
            return detail::SaturatorShapeMath::f(shape, (p[0] + p[1] + p[2]) / 3.0);
        const double upper = first_difference(shape, p[2], p[1], q[2], q[1], tolerance);
        const double lower = first_difference(shape, p[1], p[0], q[1], q[0], tolerance);
        return 2.0 * (upper - lower) / spread;
    }
 
  private:
    static void swap_pair(double* p, double* q, int i, int j) {
        std::swap(p[i], p[j]);
        std::swap(q[i], q[j]);
    }

    static double first_difference(SaturatorShape shape, double hi, double lo, double f2_hi,
                                   double f2_lo, double tolerance) {
        const double delta = hi - lo;
        if (delta > tolerance)
            return (f2_hi - f2_lo) / delta;
        const double mid = 0.5 * (hi + lo);
        return detail::SaturatorShapeMath::antiderivative(shape, mid) +
               delta * delta * detail::SaturatorShapeMath::df(shape, mid) / 24.0;
    }
 
    double u_[2] = {0.0, 0.0};
    double f2_[2] = {0.0, 0.0};
};

template <typename SampleType = float>
class SaturatorT {
public:
    using Shape = SaturatorShape;
    using AliasPolicy = SaturatorAliasPolicy;
 
    // ── Design parameters (the complete roster; see the file doc block) ────

    static constexpr double kDriveDbMin = -12.0;
    static constexpr double kDriveDbMax = 36.0;
    static constexpr double kDriveDbDefault = 0.0;

    static constexpr double kBiasMax = 1.0;

    static constexpr double kCubicBiasClamp = 0.98;

    static constexpr double kTonePreHzDefault = 3000.0;
    static constexpr double kTonePreHzMax = 8000.0;

    static constexpr double kTonePreHzFloor = 20.0;

    static constexpr double kPreBoostDbDefault = 9.0;
    static constexpr double kPreBoostDbMax = 18.0;

    static constexpr double kShelfQ = 0.70710678118654752;

    static constexpr double kAdaaEpsilon = 1e-6;

    static constexpr double kOutputTrimDbMax = 24.0;
 
    // ── Lifecycle ─────────────────────────────────────────────────────────
 
    SaturatorT() { update_shaper(); }

    void prepare(double sample_rate) {
        sample_rate_ = sample_rate > 0.0 ? sample_rate : sample_rate_;
 
        // The house oversampler, at 2x with the linear-phase FIR kind. Composed
        // rather than reimplemented: its x2 standard-quality prototype is 129
        // taps, whose polyphase halves give exactly 64 samples of group delay at
        // the input rate — the value `latency_samples()` reports.
        using Os = OversamplerT<SampleType>;
        oversampler_.set_kind(Os::Kind::linear_phase_fir);
        oversampler_.set_quality(Os::Quality::standard);
        oversampler_.set_factor(Os::Factor::x2);
        oversampler_.set_sample_rate(static_cast<SampleType>(sample_rate_));
        oversample_latency_ = oversampler_.latency_samples();
 
        // The dry path is delayed to match the wet path (or the dry/wet mix
        // becomes a comb filter) by a fixed-capacity ring, so an unprepared
        // instance aligns adaa2's one sample too and switching policy never
        // allocates.
        oversample_latency_ =
            std::min(oversample_latency_, static_cast<int>(kDryDelayCapacity) - 1);
 
        update_tone();
        reset();
    }
 
    void set_shape(Shape shape) {
        shape_ = shape;
        update_shaper();
    }
 
    Shape shape() const { return shape_; }
 
    void set_drive_db(double db) {
        if (!std::isfinite(db)) return;
        drive_db_ = std::clamp(db, kDriveDbMin, kDriveDbMax);
        update_shaper();
    }
 
    double drive_db() const { return drive_db_; }

    void set_bias(double normalized) {
        if (!std::isfinite(normalized)) return;
        bias_request_ = std::clamp(normalized, -kBiasMax, kBiasMax);
        update_shaper();
    }

    double bias() const { return bias_; }

    void set_tone_pre_hz(double hz) {
        if (!std::isfinite(hz)) return;
        tone_pre_hz_ = std::clamp(hz, 0.0, kTonePreHzMax);
        update_tone();
    }

    void set_tone_tracking(bool tracking) {
        tone_tracking_ = tracking;
        update_tone();
    }

    void set_tone_de_hz(double hz) {
        if (!std::isfinite(hz)) return;
        tone_de_hz_ = std::clamp(hz, 0.0, kTonePreHzMax);
        update_tone();
    }
 
    void set_pre_boost_db(double db) {
        if (!std::isfinite(db)) return;
        pre_boost_db_ = std::clamp(db, 0.0, kPreBoostDbMax);
        update_tone();
    }

    void set_alias_policy(AliasPolicy policy) {
        if (policy == alias_policy_)
            return;
        alias_policy_ = policy;
        adaa_previous_ = input_history_[0];
        adaa2_history_[0] = input_history_[0];
        adaa2_history_[1] = input_history_[1];
        reseed_adaa();
    }
    AliasPolicy alias_policy() const { return alias_policy_; }
 
    void set_mix(double wet01) {
        if (!std::isfinite(wet01)) return;
        mix_ = std::clamp(wet01, 0.0, 1.0);
    }
 
    void set_output_trim_db(double db) {
        if (!std::isfinite(db)) return;
        output_trim_ = units::db_to_linear(std::clamp(db, -kOutputTrimDbMax, kOutputTrimDbMax));
    }

    int latency_samples() const noexcept {
        switch (alias_policy_) {
        case AliasPolicy::oversample_2x:
            return oversample_latency_;
        case AliasPolicy::adaa2:
            return 1;
        case AliasPolicy::adaa:
        case AliasPolicy::off:
            return 0;
        }
        return 0;
    }

    double group_delay_samples() const noexcept {
        switch (alias_policy_) {
        case AliasPolicy::oversample_2x:
            return oversampler_.latency().exact_input_samples;
        case AliasPolicy::adaa2:
            return 1.0;
        case AliasPolicy::adaa:
            return 0.5;
        case AliasPolicy::off:
            return 0.0;
        }
        return 0.0;
    }

    void reset() {
        adaa_previous_ = 0.0;
        adaa2_history_[0] = adaa2_history_[1] = 0.0;
        input_history_[0] = input_history_[1] = 0.0;
        reseed_adaa();
        pre_filter_.reset();
        de_filter_.reset();
        oversampler_.reset();
        dry_delay_.fill(SampleType{0});
        dry_write_ = 0;
        dry_valid_ = 0;
    }

    SampleType process(SampleType input) {
        if (!std::isfinite(static_cast<double>(input))) {
            recover_audio_fault();
            return SampleType{0};
        }
        const double dry = delayed_dry(static_cast<double>(input));
 
        double x = static_cast<double>(input);
        if (tone_active_) x = pre_filter_.process(static_cast<SampleType>(x));
        input_history_[1] = input_history_[0];
        input_history_[0] = x;
 
        double wet = 0.0;
        switch (alias_policy_) {
            case AliasPolicy::off:
                wet = shaped(x);
                break;
            case AliasPolicy::adaa:
                wet = shaped_adaa(x);
                break;
            case AliasPolicy::adaa2:
                wet = shaped_adaa2(x);
                break;
            case AliasPolicy::oversample_2x:
                wet = oversampler_.process(static_cast<SampleType>(x), [this](SampleType s) {
                    return static_cast<SampleType>(shaped(static_cast<double>(s)));
                });
                break;
        }
 
        if (tone_active_) wet = de_filter_.process(static_cast<SampleType>(wet));
 
        const double mixed = dry + mix_ * (wet - dry);
        const double output = snap_to_zero(mixed * output_trim_);
        if (!std::isfinite(output)) {
            recover_audio_fault();
            return SampleType{0};
        }
        return static_cast<SampleType>(output);
    }

    double worst_case_gain() const { return max_gain_; }

    double shaped(double x) const {
        const double u = drive_ * x + bias_;
        return (detail::SaturatorShapeMath::f(shape_, u) - bias_offset_) * inv_drive_slope_;
    }
 
private:
    void recover_audio_fault() noexcept {
        adaa_previous_ = 0.0;
        adaa2_history_[0] = adaa2_history_[1] = 0.0;
        input_history_[0] = input_history_[1] = 0.0;
        reseed_adaa();
        pre_filter_.reset();
        de_filter_.reset();
        oversampler_.reset();
        dry_write_ = 0;
        dry_valid_ = 0;
    }

    double antiderivative(double x) const {
        const double u = drive_ * x + bias_;
        return (detail::SaturatorShapeMath::antiderivative(shape_, u) -
                drive_ * bias_offset_ * x) *
               inv_drive_sq_slope_;
    }

    double shaped_adaa(double x) {
        const double previous = adaa_previous_;
        const double previous_antiderivative = adaa_previous_antiderivative_;
        const double current_antiderivative = antiderivative(x);
        adaa_previous_ = x;
        adaa_previous_antiderivative_ = current_antiderivative;
        const double delta = x - previous;
        if (std::abs(delta) > kAdaaEpsilon)
            return (current_antiderivative - previous_antiderivative) / delta;
        // The difference quotient is numerically meaningless here; the limit it
        // is approximating is the curve evaluated at the midpoint.
        return shaped(0.5 * (x + previous));
    }

    double shaped_adaa2(double x) {
        adaa2_history_[1] = adaa2_history_[0];
        adaa2_history_[0] = x;
        const double v = adaa2_.step(shape_, drive_ * x + bias_);
        return (v - bias_offset_) * inv_drive_slope_;
    }

    void reseed_adaa() {
        adaa_previous_antiderivative_ = antiderivative(adaa_previous_);
        adaa2_.reset(shape_, drive_ * adaa2_history_[0] + bias_,
                     drive_ * adaa2_history_[1] + bias_);
    }

    double delayed_dry(double input) {
        const int latency = latency_samples();
        if (latency <= 0)
            return input;
        const std::size_t size = dry_delay_.size();
        const std::size_t read = (dry_write_ + size - static_cast<std::size_t>(latency)) % size;
        const double out = dry_valid_ >= static_cast<std::size_t>(latency)
                               ? static_cast<double>(dry_delay_[read])
                               : 0.0;
        dry_delay_[dry_write_] = static_cast<SampleType>(input);
        dry_write_ = (dry_write_ + 1) % size;
        if (dry_valid_ < size) ++dry_valid_;
        return out;
    }
 
    void update_shaper() {
        drive_ = units::db_to_linear(drive_db_);
 
        // The cubic's slope reaches zero at |b| = 1, and the normaliser divides
        // by it. Clamp per shape rather than globally so the other three keep
        // their full range.
        const double limit = shape_ == Shape::cubic_soft ? kCubicBiasClamp : kBiasMax;
        bias_ = std::clamp(bias_request_, -limit, limit);
 
        bias_offset_ = detail::SaturatorShapeMath::f(shape_, bias_);
        const double slope = detail::SaturatorShapeMath::df(shape_, bias_);
        // Guarded, though the per-shape clamp above should make it unreachable:
        // a zero here would be a silent inf rather than a loud failure.
        const double safe_slope = slope > 1e-12 ? slope : 1e-12;
        inv_drive_slope_ = 1.0 / (drive_ * safe_slope);
        inv_drive_sq_slope_ = 1.0 / (drive_ * drive_ * safe_slope);
        // Every shape's steepest slope is at the origin and equals 1, so the
        // largest gain the biased, slope-normalised curve can present is
        // f'(0)/f'(b) = 1/f'(b). Drive cancels out of the ratio.
        max_gain_ = 1.0 / safe_slope;
        reseed_adaa();
    }
 
    void update_tone() {
        tone_active_ = tone_pre_hz_ >= kTonePreHzFloor && pre_boost_db_ > 0.0;
        if (!tone_active_) return;
 
        const double de_hz = tone_tracking_ ? tone_pre_hz_
                                            : std::max(tone_de_hz_, kTonePreHzFloor);
        // An RBJ high shelf at −G is the exact algebraic reciprocal of the same
        // shelf at +G, so a tracked pair cancels to numerical precision rather
        // than to within a fitted tolerance.
        pre_filter_.set_coefficients(BiquadT<SampleType>::Type::high_shelf,
                                     static_cast<SampleType>(tone_pre_hz_),
                                     static_cast<SampleType>(kShelfQ),
                                     static_cast<SampleType>(sample_rate_),
                                     static_cast<SampleType>(pre_boost_db_));
        de_filter_.set_coefficients(BiquadT<SampleType>::Type::high_shelf,
                                    static_cast<SampleType>(de_hz),
                                    static_cast<SampleType>(kShelfQ),
                                    static_cast<SampleType>(sample_rate_),
                                    static_cast<SampleType>(-pre_boost_db_));
    }
 
    double sample_rate_ = 44100.0;
 
    Shape shape_ = Shape::tanh_soft;
    AliasPolicy alias_policy_ = AliasPolicy::adaa;
 
    double drive_db_ = kDriveDbDefault;
    double bias_request_ = 0.0;
    double tone_pre_hz_ = 0.0;  // off by default; the node's table supplies 3000
    double tone_de_hz_ = kTonePreHzDefault;
    double pre_boost_db_ = kPreBoostDbDefault;
    bool tone_tracking_ = true;
    bool tone_active_ = false;
    double mix_ = 1.0;
    double output_trim_ = 1.0;
 
    // Derived, recomputed only on a parameter change.
    double drive_ = 1.0;
    double bias_ = 0.0;
    double bias_offset_ = 0.0;
    double inv_drive_slope_ = 1.0;
    double inv_drive_sq_slope_ = 1.0;
    double max_gain_ = 1.0;
 
    double adaa_previous_ = 0.0;
    double adaa_previous_antiderivative_ = 0.0;
    double adaa2_history_[2] = {0.0, 0.0};
    // The two most recent shaper inputs, whatever the policy; a policy switch
    // seeds the ADAA paths from them.
    double input_history_[2] = {0.0, 0.0};
    SaturatorAdaa2 adaa2_{};
    BiquadT<SampleType> pre_filter_{};
    BiquadT<SampleType> de_filter_{};
    OversamplerT<SampleType> oversampler_{};
    int oversample_latency_ = 0;
    // Holds the largest dry alignment any policy needs: the composed x2
    // standard-quality oversampler's 64 samples.
    static constexpr std::size_t kDryDelayCapacity = 128;
    std::array<SampleType, kDryDelayCapacity> dry_delay_{};
    std::size_t dry_write_ = 0;
    std::size_t dry_valid_ = 0;
};
 
using Saturator = SaturatorT<float>;
using Saturator64 = SaturatorT<double>;
 
}  // namespace pulp::signal