frequency_shifter_ssb.hpp

Go to the documentation of this file.

#pragma once

 
#include <pulp/signal/dc_blocker.hpp>
#include <pulp/signal/delay_line.hpp>
#include <pulp/signal/denormal.hpp>
#include <pulp/signal/osc/phase.hpp>
#include <pulp/signal/smoothed_value.hpp>
#include <pulp/signal/units.hpp>
 
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <cstdint>
 
namespace pulp::signal {

template <typename SampleType = double>
class HilbertQuadratureNetworkT {
public:
    static constexpr std::size_t kSections = 4;

    static constexpr std::array<double, kSections> kInPhasePoles = {
        0.4021921162426, 0.8561710882420, 0.9722909545651, 0.9952884791278};

    static constexpr std::array<double, kSections> kQuadraturePoles = {
        0.6923878, 0.9360654322959, 0.9882295226860, 0.9987488452737};

    static constexpr double kBandEdgeNormalized = 4.6e-4;

    static constexpr double kImageRejectDb = 40.0;
 
    struct Outputs {
        SampleType in_phase = 0;    
        SampleType quadrature = 0;  
    };

    void reset() {
        in_phase_state_ = {};
        quadrature_state_ = {};
        delayed_ = 0;
    }
 
    Outputs process(SampleType x) {
        const SampleType in_phase = cascade(x, kInPhasePoles, in_phase_state_);
        const SampleType quadrature = cascade(x, kQuadraturePoles, quadrature_state_);
        const Outputs out{in_phase, delayed_};
        delayed_ = quadrature;
        return out;
    }

    static constexpr double band_low_hz(double sample_rate) {
        return kBandEdgeNormalized * sample_rate;
    }

    static constexpr double band_high_hz(double sample_rate) {
        return (0.5 - kBandEdgeNormalized) * sample_rate;
    }
 
private:
    struct SectionState {
        SampleType s1 = 0;
        SampleType s2 = 0;
    };
 
    using BranchState = std::array<SectionState, kSections>;
 
    static SampleType cascade(SampleType x, const std::array<double, kSections>& poles,
                              BranchState& state) {
        SampleType v = x;
        for (std::size_t k = 0; k < kSections; ++k) {
            const auto c = static_cast<SampleType>(poles[k] * poles[k]);
            const SampleType y = c * v + state[k].s1;
            state[k].s1 = state[k].s2;
            // Snap the recursive state: the top sections' poles sit at
            // |z| = 0.9994, so a decaying tail stalls in denormals for a long
            // time with no flush-to-zero guard. No-op above 1e-15.
            state[k].s2 = snap_to_zero(c * y - v);
            v = y;
        }
        return v;
    }
 
    BranchState in_phase_state_{};
    BranchState quadrature_state_{};
    SampleType delayed_ = 0;
};

enum class FrequencyShiftMode : std::uint8_t {
    up,            
    down,          
    dual_mono,     
    stereo_split,  
};

template <typename SampleType = float>
class SsbFrequencyShifterT {
public:
    using Mode = FrequencyShiftMode;
    using Network = HilbertQuadratureNetworkT<double>;
 
    // ── Design parameters (the complete roster; see §11 of the spec) ───────

    static constexpr double kMaxShiftHz = 5000.0;

    static constexpr double kLinZoneHz = 20.0;

    static constexpr double kSplit = 0.5;

    static constexpr double kMaxFeedback = 0.90;

    static constexpr double kMinDelayMs = 0.1;

    static constexpr double kMaxLoopMs = 50.0;

    static constexpr double kGshiftBudget = 1.02;

    static constexpr double kDcCornerHz = 5.0;

    static constexpr double kShiftSmoothingMs = 20.0;

    static constexpr double kGainSmoothingMs = 20.0;

    static constexpr double kDelaySmoothingMs = 50.0;
 
    // ── Lifecycle ─────────────────────────────────────────────────────────

    void prepare(double sample_rate) {
        sample_rate_ = sample_rate > 0.0 ? sample_rate : sample_rate_;
 
        const int max_delay = std::max(
            2, static_cast<int>(std::ceil(units::ms_to_samples(kMaxLoopMs, sample_rate_))) + 2);
        for (auto& line : delay_) line.prepare(max_delay);
        max_delay_samples_ = static_cast<double>(max_delay);
 
        // H(z) = (1 − z⁻¹)/(1 − p·z⁻¹) has its corner where the pole sits;
        // `exp(−2π·fc/fs)` places it at `kDcCornerHz` at any sample rate,
        // which the fixed 0.995 default cannot.
        const auto pole = static_cast<double>(std::exp(-2.0 * kPi * kDcCornerHz / sample_rate_));
        for (auto& dc : dc_blocker_) dc.set_pole(pole);
 
        const auto sr = static_cast<double>(sample_rate_);
        shift_smoother_.set_ramp_time(kShiftSmoothingMs * 0.001, sr);
        feedback_smoother_.set_ramp_time(kGainSmoothingMs * 0.001, sr);
        mix_smoother_.set_ramp_time(kGainSmoothingMs * 0.001, sr);
        spread_smoother_.set_ramp_time(kGainSmoothingMs * 0.001, sr);
        delay_smoother_.set_ramp_time(kDelaySmoothingMs * 0.001, sr);
 
        update_delay_target();
        reset();
    }

    void reset() {
        for (auto& n : network_) n.reset();
        for (auto& dc : dc_blocker_) dc.reset();
        for (auto& line : delay_) line.reset();
        carrier_.reset(0.0);
        shift_smoother_.set_immediate(shift_hz_);
        feedback_smoother_.set_immediate(feedback_);
        mix_smoother_.set_immediate(mix_);
        spread_smoother_.set_immediate(spread_);
        delay_smoother_.set_immediate(delay_target_samples_);
    }

    void discard_history() noexcept {
        for (auto& n : network_) n.reset();
        for (auto& dc : dc_blocker_) dc.reset();
        for (auto& line : delay_) line.discard_history();
        carrier_.reset(0.0);
        shift_smoother_.set_immediate(shift_hz_);
        feedback_smoother_.set_immediate(feedback_);
        mix_smoother_.set_immediate(mix_);
        spread_smoother_.set_immediate(spread_);
        delay_smoother_.set_immediate(delay_target_samples_);
    }
 
    // ── Parameters (real units throughout, series law 3) ──────────────────

    void set_shift_hz(double hz) {
        if (!std::isfinite(hz)) return;
        shift_hz_ = std::clamp(hz, -kMaxShiftHz, kMaxShiftHz);
        shift_smoother_.set_target(shift_hz_);
    }
 
    double shift_hz() const { return shift_hz_; }

    void set_feedback(double g) {
        if (!std::isfinite(g)) return;
        feedback_ = std::clamp(g, 0.0, kMaxFeedback);
        feedback_smoother_.set_target(feedback_);
    }
 
    double feedback() const { return feedback_; }

    void set_feedback_delay_ms(double ms) {
        if (!std::isfinite(ms)) return;
        feedback_delay_ms_ = std::clamp(ms, kMinDelayMs, kMaxLoopMs);
        update_delay_target();
        delay_smoother_.set_target(delay_target_samples_);
    }
 
    double feedback_delay_ms() const { return feedback_delay_ms_; }
 
    void set_mode(Mode mode) { mode_ = mode; }
    Mode mode() const { return mode_; }

    void set_mix(double wet01) {
        if (!std::isfinite(wet01)) return;
        mix_ = std::clamp(wet01, 0.0, 1.0);
        mix_smoother_.set_target(mix_);
    }

    void set_stereo_spread(double spread01) {
        if (!std::isfinite(spread01)) return;
        spread_ = std::clamp(spread01, 0.0, 1.0);
        spread_smoother_.set_target(spread_);
    }
 
    // ── Processing ────────────────────────────────────────────────────────

    SampleType process(SampleType x) {
        if (!std::isfinite(static_cast<double>(x))) {
            discard_history();
            return SampleType{0};
        }
        const Controls c = advance_controls();
        const double dry = static_cast<double>(x);
        const double wet = shift_channel(0, dry, c, /*left=*/true);
        return static_cast<SampleType>(snap_to_zero(dry + c.mix * (wet - dry)));
    }

    void process_stereo(SampleType& left, SampleType& right) {
        if (!std::isfinite(static_cast<double>(left)) ||
            !std::isfinite(static_cast<double>(right))) {
            discard_history();
            left = right = SampleType{0};
            return;
        }
        const Controls c = advance_controls();
        const double dry_l = static_cast<double>(left);
        const double dry_r = static_cast<double>(right);
        const double wet_l = shift_channel(0, dry_l, c, /*left=*/true);
        const double wet_r = shift_channel(1, dry_r, c, /*left=*/false);
        left = static_cast<SampleType>(snap_to_zero(dry_l + c.mix * (wet_l - dry_l)));
        right = static_cast<SampleType>(snap_to_zero(dry_r + c.mix * (wet_r - dry_r)));
    }

    int latency_samples() const noexcept { return 0; }
 
    // ── Registry and taper helpers (pure functions of shipped constants) ───

    static constexpr double worst_case_gain() {
        return 1.0 / (1.0 - kMaxFeedback * kGshiftBudget);
    }

    static double shift_hz_from_knob(double knob) {
        const double clamped = std::clamp(knob, -1.0, 1.0);
        const double m = std::abs(clamped);
        const double magnitude =
            m < kSplit ? (m / kSplit) * kLinZoneHz
                       : units::taper_log((m - kSplit) / (1.0 - kSplit), kLinZoneHz, kMaxShiftHz);
        return clamped < 0.0 ? -magnitude : magnitude;
    }

    static double knob_from_shift_hz(double hz) {
        const double clamped = std::clamp(hz, -kMaxShiftHz, kMaxShiftHz);
        const double m = std::abs(clamped);
        const double position =
            m < kLinZoneHz
                ? (m / kLinZoneHz) * kSplit
                : kSplit + (1.0 - kSplit) * units::untaper_log(m, kLinZoneHz, kMaxShiftHz);
        return clamped < 0.0 ? -position : position;
    }

    static double feedback_delay_ms_from_knob(double u) {
        return units::taper_log(std::clamp(u, 0.0, 1.0), kMinDelayMs, kMaxLoopMs);
    }
 
private:
    static constexpr double kPi = 3.14159265358979323846;
    static constexpr double kTwoPi = 6.283185307179586476925286766559;

    struct Controls {
        double cosine = 1.0;
        double sine = 0.0;
        double feedback = 0.0;
        double delay_samples = 0.0;
        double mix = 1.0;
        double spread = 1.0;
    };
 
    Controls advance_controls() {
        Controls c;
        c.feedback = feedback_smoother_.next();
        c.delay_samples = delay_smoother_.next();
        c.mix = mix_smoother_.next();
        c.spread = spread_smoother_.next();
 
        // In `stereo_split` the spread scales the carrier itself, so the two
        // legs land at `f ± spread·Δf`. Both legs read the SAME accumulator:
        // the right channel's carrier is the left's conjugate, which is the
        // same cosine with the sine negated, so one phase serves both.
        const double shift = shift_smoother_.next() *
                             (mode_ == Mode::stereo_split ? c.spread : 1.0);
 
        // Advance FIRST, then evaluate both functions at the advanced phase —
        // `LfoT::next_quadrature`'s order, and the reason an impulse produces a
        // non-zero sample 0: at `n = 0` the carrier is already off zero, so the
        // quadrature term contributes immediately.
        carrier_.advance(shift / sample_rate_);
        const double radians = kTwoPi * carrier_.phase();
        c.cosine = std::cos(radians);
        c.sine = std::sin(radians);
        return c;
    }

    double shift_channel(std::size_t channel, double input, const Controls& c, bool left) {
        // Read BEFORE the write, so the shortest realisable round trip is one
        // sample and the loop is never algebraic. `read()` looks back
        // `delay + 1` samples from the write cursor, so asking for
        // `delay_samples − 1` makes the total round trip exactly
        // `delay_samples` — with a floor of one sample.
        const double read_position = std::clamp(c.delay_samples - 1.0, 0.0, max_delay_samples_);
        const double recirculated = delay_[channel].read(read_position);
 
        const double driven = dc_blocker_[channel].process(input + c.feedback * recirculated);
        const auto pair = network_[channel].process(driven);
 
        // `up` keeps `f + Δf`, `down` keeps `f − Δf`. In `stereo_split` the
        // left leg is the up combine and the right the down one — exactly the
        // up/down pair run in parallel, over a carrier the spread has already
        // scaled.
        double quadrature_gain = 0.0;
        switch (mode_) {
            case Mode::up:
            case Mode::dual_mono:
                quadrature_gain = -1.0;
                break;
            case Mode::down:
                quadrature_gain = 1.0;
                break;
            case Mode::stereo_split:
                quadrature_gain = left ? -1.0 : 1.0;
                break;
        }
 
        const double wet = snap_to_zero(pair.in_phase * c.cosine +
                                        quadrature_gain * pair.quadrature * c.sine);
        delay_[channel].push(wet);
        return wet;
    }
 
    void update_delay_target() {
        const double samples = units::ms_to_samples(feedback_delay_ms_, sample_rate_);
        delay_target_samples_ = std::clamp(samples, 1.0, max_delay_samples_);
    }
 
    double sample_rate_ = 44100.0;
    double max_delay_samples_ = 1.0;
 
    double shift_hz_ = 0.0;
    double feedback_ = 0.0;
    double feedback_delay_ms_ = 8.0;
    double mix_ = 1.0;
    double spread_ = 1.0;
    double delay_target_samples_ = 1.0;
    Mode mode_ = Mode::up;
 
    std::array<Network, 2> network_{};
    std::array<DcBlocker<double>, 2> dc_blocker_{};
    std::array<DelayLineT<double>, 2> delay_{};
 
    osc::PhaseAccumulator carrier_{};
 
    SmoothedValue<double> shift_smoother_{};
    SmoothedValue<double> feedback_smoother_{};
    SmoothedValue<double> mix_smoother_{1.0};
    SmoothedValue<double> spread_smoother_{1.0};
    SmoothedValue<double> delay_smoother_{1.0};
};
 
using HilbertQuadratureNetwork = HilbertQuadratureNetworkT<double>;
using SsbFrequencyShifter = SsbFrequencyShifterT<float>;
using SsbFrequencyShifter64 = SsbFrequencyShifterT<double>;
 
}  // namespace pulp::signal