113 prepared_.store(
false, std::memory_order_relaxed);
118 fs2_ =
static_cast<double>(osFactor_) * sampleRate_;
124 oversampler_ = std::make_unique<Oversampling<T>>(
126 oversampler_->prepare(spec);
130 oversampler_.reset();
134 channels_.resize(
static_cast<size_t>(numChannels_));
135 for (
auto& ch : channels_)
136 ch = std::make_unique<ChannelState>(fs2_);
138 latency_ = oversampler_ ? oversampler_->getLatency() : 0;
140 while (drySize_ < latency_ + maxBlock_ + 1) drySize_ <<= 1;
141 dryRing_.assign(
static_cast<size_t>(numChannels_),
142 std::vector<T>(
static_cast<size_t>(drySize_), T(0)));
145 calibrateReference();
147 prepared_.store(
true, std::memory_order_relaxed);
148 dirty_.store(
true, std::memory_order_release);
155 if (!prepared_.load(std::memory_order_relaxed))
return;
156 const double sagR =
static_cast<double>(sag_.load(std::memory_order_relaxed)) * 40e3;
157 const int stages = stages_.load(std::memory_order_relaxed);
158 for (
auto& ch : channels_)
159 ch->reset(sagR, stages);
160 for (
auto& d : dryRing_)
161 std::fill(d.begin(), d.end(), T(0));
163 if (oversampler_) oversampler_->reset();
167 currentMix_ = mix_.load(std::memory_order_relaxed);
176 if (!std::isfinite(db))
return;
177 driveDb_.store(std::clamp(db, T(-12), T(36)), std::memory_order_relaxed);
178 dirty_.store(
true, std::memory_order_release);
184 if (!std::isfinite(treble))
return;
185 treble_.store(std::clamp(treble, T(0), T(1)), std::memory_order_relaxed);
186 dirty_.store(
true, std::memory_order_release);
193 if (!std::isfinite(bass))
return;
194 bass_.store(std::clamp(bass, T(0), T(1)), std::memory_order_relaxed);
195 dirty_.store(
true, std::memory_order_release);
201 if (!std::isfinite(middle))
return;
202 middle_.store(std::clamp(middle, T(0), T(1)), std::memory_order_relaxed);
203 dirty_.store(
true, std::memory_order_release);
209 if (!std::isfinite(sag))
return;
210 sag_.store(std::clamp(sag, T(0), T(1)), std::memory_order_relaxed);
211 dirty_.store(
true, std::memory_order_release);
217 stages_.store(std::clamp(stages, 1, 2), std::memory_order_relaxed);
218 dirty_.store(
true, std::memory_order_release);
236 if (factor < 1 || factor > 16 || (factor & (factor - 1)) != 0)
return;
237 if (factor == osFactor_)
return;
242 if (prepared_.load(std::memory_order_relaxed))
252 if (!std::isfinite(db))
return;
253 outputDb_.store(std::clamp(db, T(-24), T(12)), std::memory_order_relaxed);
260 if (!std::isfinite(mix))
return;
261 mix_.store(std::clamp(mix, T(0), T(1)), std::memory_order_relaxed);
264 [[nodiscard]] T
getDrive() const noexcept {
return driveDb_.load(std::memory_order_relaxed); }
265 [[nodiscard]] T
getTreble() const noexcept {
return treble_.load(std::memory_order_relaxed); }
266 [[nodiscard]] T
getBass() const noexcept {
return bass_.load(std::memory_order_relaxed); }
267 [[nodiscard]] T
getMiddle() const noexcept {
return middle_.load(std::memory_order_relaxed); }
268 [[nodiscard]] T
getSag() const noexcept {
return sag_.load(std::memory_order_relaxed); }
269 [[nodiscard]]
int getStages() const noexcept {
return stages_.load(std::memory_order_relaxed); }
270 [[nodiscard]] T
getOutput() const noexcept {
return outputDb_.load(std::memory_order_relaxed); }
271 [[nodiscard]] T
getMix() const noexcept {
return mix_.load(std::memory_order_relaxed); }
275 [[nodiscard]]
int getLatency() const noexcept {
return latency_; }
284 return supplyNow_.load(std::memory_order_relaxed);
288 [[nodiscard]] std::vector<uint8_t>
getState()
const
293 w.
write(
"drive",
static_cast<float>(driveDb_.load(std::memory_order_relaxed)));
294 w.
write(
"treble",
static_cast<float>(treble_.load(std::memory_order_relaxed)));
295 w.
write(
"bass",
static_cast<float>(bass_.load(std::memory_order_relaxed)));
296 w.
write(
"middle",
static_cast<float>(middle_.load(std::memory_order_relaxed)));
297 w.
write(
"sag",
static_cast<float>(sag_.load(std::memory_order_relaxed)));
298 w.
write(
"stages", stages_.load(std::memory_order_relaxed));
299 w.
write(
"output",
static_cast<float>(outputDb_.load(std::memory_order_relaxed)));
300 w.
write(
"mix",
static_cast<float>(mix_.load(std::memory_order_relaxed)));
301 w.
write(
"oversampling", osFactor_);
329 if (!prepared_.load(std::memory_order_relaxed))
return;
332 const int nCh = std::min(buffer.getNumChannels(), numChannels_);
333 const int nS = buffer.getNumSamples();
334 if (nCh == 0 || nS == 0)
return;
342 for (
int ch = 0; ch < nCh; ++ch)
344 T* d = buffer.getChannel(ch);
345 for (
int i = 0; i < nS; ++i)
346 if (!std::isfinite(d[i])) d[i] = T(0);
351 if (dirty_.load(std::memory_order_relaxed)
352 && dirty_.exchange(
false, std::memory_order_acquire))
357 const T mixTarget = mix_.load(std::memory_order_relaxed);
358 const T mixStart = currentMix_;
359 const T mixStep = (mixTarget - mixStart) /
static_cast<T
>(nS);
360 const double outGain = mScale_
361 * std::pow(10.0,
static_cast<double>(outputDb_.load(std::memory_order_relaxed)) / 20.0);
364 for (
int ch = 0; ch < nCh; ++ch)
366 const T* in = buffer.getChannel(ch);
367 auto& dry = dryRing_[
static_cast<size_t>(ch)];
369 for (
int i = 0; i < nS; ++i)
371 dry[
static_cast<size_t>(dp)] = in[i];
372 dp = (dp + 1) & (drySize_ - 1);
379 const bool osOn = (oversampler_ !=
nullptr);
380 auto osView = osOn ? oversampler_->upsample(buffer) : buffer;
381 const int osN = osView.getNumSamples();
390 if (outGainSm_ <= 0.0) outGainSm_ = outGain;
391 if (hScaleSm_ <= 0.0) hScaleSm_ = hScale_;
392 const double kSm = 1.0 - std::exp(-
static_cast<double>(osN) / (0.030 * fs2_));
393 const double outEnd = outGainSm_ * std::pow(outGain / outGainSm_, kSm);
394 const double hEnd = hScaleSm_ * std::pow(hScale_ / hScaleSm_, kSm);
395 const double outRat = std::pow(outEnd / outGainSm_, 1.0 /
static_cast<double>(osN));
396 const double hRat = std::pow(hEnd / hScaleSm_, 1.0 /
static_cast<double>(osN));
398 for (
int ch = 0; ch < nCh; ++ch)
400 T* d = osView.getChannel(ch);
401 auto& state = *channels_[
static_cast<size_t>(ch)];
402 double g = outGainSm_, h = hScaleSm_;
403 for (
int i = 0; i < osN; ++i)
407 d[i] =
static_cast<T
>(g
408 * state.processSample(h *
static_cast<double>(d[i]),
409 numStagesActive_, sagR_));
414 if (osOn) oversampler_->downsample(buffer);
415 supplyNow_.store(
static_cast<T
>(kBplus - sagR_ * channels_[0]->ipLP),
416 std::memory_order_relaxed);
421 for (
int ch = 0; ch < nCh; ++ch)
423 T* d = buffer.getChannel(ch);
424 const auto& dry = dryRing_[
static_cast<size_t>(ch)];
425 for (
int i = 0; i < nS; ++i)
427 const int idx = (dryPos_ + i - latency_) & (drySize_ - 1);
428 const T drySample = dry[
static_cast<size_t>(idx)];
429 const T mixVal = mixStart + mixStep *
static_cast<T
>(i);
430 d[i] = drySample + (d[i] - drySample) * mixVal;
433 currentMix_ = mixTarget;
434 dryPos_ = (dryPos_ + nS) & (drySize_ - 1);
439 static constexpr double kMu = 100.0, kEx = 1.4, kKg1 = 1060.0;
440 static constexpr double kKp = 600.0, kKvb = 300.0;
441 static constexpr double kBplus = 300.0;
442 static constexpr double kRL = 100e3;
443 static constexpr double kRk = 1.5e3;
444 static constexpr double kCk = 22e-6;
445 static constexpr double kInterstage = 0.12;
448 static void koren(
double vpk,
double vgk,
double& ip,
449 double& dIpdVpk,
double& dIpdVgk)
noexcept
451 vpk = std::max(vpk, 0.0);
452 const double s = std::sqrt(kKvb + vpk * vpk);
453 const double u = kKp * (1.0 / kMu + vgk / s);
455 double sp = 0.0, sig = 0.0;
456 if (u > 30.0) { sp = u; sig = 1.0; }
457 else if (u < -30.0) { sp = std::exp(u); sig = sp; }
460 const double eu = std::exp(u);
462 sig = eu / (1.0 + eu);
465 const double e1 = (vpk / kKp) * sp;
473 const double e1ex1 = std::pow(e1, kEx - 1.0);
474 ip = 2.0 * e1ex1 * e1 / kKg1;
475 const double dIpdE1 = 2.0 * kEx * e1ex1 / kKg1;
477 const double dUdVpk = -kKp * vgk * vpk / (s * s * s);
478 const double dE1dVpk = sp / kKp + (vpk / kKp) * sig * dUdVpk;
479 const double dE1dVgk = vpk * sig / s;
480 dIpdVpk = dIpdE1 * dE1dVpk;
481 dIpdVgk = dIpdE1 * dE1dVgk;
487 double fs2 = 96000.0;
493 void settleDC(
double bplus)
noexcept
497 for (
int it = 0; it < 60; ++it)
499 const double vkS = i * kRk;
500 const double vpk = bplus - i * kRL - vkS;
501 double ipK = 0.0, dVpk = 0.0, dVgk = 0.0;
502 koren(vpk, -vkS, ipK, dVpk, dVgk);
503 const double f = i - ipK;
504 const double fp = 1.0 - (dVpk * (-(kRL + kRk)) + dVgk * (-kRk));
505 const double di = f / fp;
507 i = std::clamp(i, 0.0, bplus / (kRL + kRk));
508 if (std::abs(di) < 1e-15)
break;
513 vpDC = bplus - i * kRL;
517 [[nodiscard]]
double processSample(
double vg,
double bplusEff)
noexcept
521 vg = 0.7 * std::tanh(vg / 0.7);
526 const double h2c = 0.5 / (fs2 * kCk);
527 const double denom = 1.0 + h2c / kRk;
528 const double kA = (vk + h2c * fPrev) / denom;
529 const double kB = h2c / denom;
531 const double iMax = bplusEff / kRL + 1e-3;
532 double i = std::clamp(ip, 0.0, iMax);
534 for (
int it = 0; it < 8; ++it)
536 const double vkN = kA + kB * i;
537 const double vpk = bplusEff - i * kRL - vkN;
538 const double vgk = vg - vkN;
539 double ipK = 0.0, dVpk = 0.0, dVgk = 0.0;
540 koren(vpk, vgk, ipK, dVpk, dVgk);
542 const double f = i - ipK;
543 const double fp = 1.0 - (dVpk * (-kRL - kB) + dVgk * (-kB));
544 const double di = f / std::max(fp, 1e-6);
546 i = std::clamp(i, 0.0, iMax);
547 if (std::abs(di) < 1e-12)
break;
550 const double vkN = kA + kB * i;
551 fPrev = i - vkN / kRk;
554 return (bplusEff - i * kRL) - vpDC;
561 explicit ChannelState(
double fs2In)
577 void reset(
double sagR,
int numStages)
noexcept
585 for (
int it = 0; it < 8; ++it)
589 iTotal = stage1.ip + (numStages > 1 ? stage2.ip : 0.0);
590 const double bpNew = kBplus - sagR * iTotal;
591 if (std::abs(bpNew - bp) < 1e-9)
break;
595 outHpX = outHpY = 0.0;
597 for (
auto& set : flatten)
603 void setFlattenCoeffs(
int stageCount,
604 const std::array<BiquadCoeffs, 3>& c)
noexcept
606 auto& set = flatten[
static_cast<size_t>(stageCount - 1)];
607 for (
int k = 0; k < 3; ++k)
608 set[
static_cast<size_t>(k)].setCoeffs(c[
static_cast<size_t>(k)]);
611 void setToneControls(
double t,
double b,
double m)
noexcept
613 fmv.setControls(t, b, m);
616 [[nodiscard]]
double processSample(
double vgIn,
int numStages,
double sagR)
noexcept
619 const double sagAlpha = 1.0 - std::exp(-1.0 / (0.07 * fs2));
620 const double iTotal = stage1.ip + (numStages > 1 ? stage2.ip : 0.0);
621 ipLP += sagAlpha * (iTotal - ipLP);
622 const double bplusEff = kBplus - sagR * ipLP;
625 double v = stage1.processSample(vgIn, bplusEff);
626 v = fmv.processSample(v);
628 v = stage2.processSample(v * kInterstage, bplusEff);
631 const double a = 1.0 - 2.0 * std::numbers::pi * 8.0 / fs2;
632 const double y = a * (outHpY + v - outHpX);
636 double out = (numStages > 1) ? y : -y;
643 auto& fl = flatten[numStages > 1 ? 1 : 0];
644 out = fl[0].processSample(out, 0);
645 out = fl[1].processSample(out, 0);
646 out = fl[2].processSample(out, 0);
650 double fs2 = 96000.0;
651 TriodeStage stage1, stage2;
652 double ipLP = 1.6e-3;
653 double outHpX = 0.0, outHpY = 0.0;
654 std::array<std::array<Biquad<double, 1>, 3>, 2> flatten;
656 wdf::ToneStackFMV<double> fmv;
660 void recompute() noexcept
662 const double drive = std::pow(10.0,
static_cast<double>(
663 driveDb_.load(std::memory_order_relaxed)) / 20.0);
664 const double t =
static_cast<double>(treble_.load(std::memory_order_relaxed));
665 const double b =
static_cast<double>(bass_.load(std::memory_order_relaxed));
666 const double m =
static_cast<double>(middle_.load(std::memory_order_relaxed));
667 const double sag =
static_cast<double>(sag_.load(std::memory_order_relaxed));
668 numStagesActive_ = stages_.load(std::memory_order_relaxed);
677 for (
auto& ch : channels_)
678 ch->setToneControls(t, b, m);
699 const double driveDbNow =
static_cast<double>(driveDb_.load(std::memory_order_relaxed));
700 mScale_ = std::pow(drive, 0.25)
701 / std::max(programGainAt(driveDbNow, numStagesActive_), 1e-9);
705 [[nodiscard]]
double programGainAt(
double driveDb,
int stages)
const noexcept
707 const auto& lut = gProgLut_[
static_cast<size_t>(stages - 1)];
711 const double pos = std::min(std::max(0.0, (driveDb - kDriveLutMinDb) / kDriveLutStepDb),
712 static_cast<double>(kDriveLutN - 1));
713 const int idx = std::min(
static_cast<int>(pos), kDriveLutN - 2);
714 const double frac = pos -
static_cast<double>(idx);
715 const double a = std::max(lut[
static_cast<size_t>(idx)], 1e-9);
716 const double b = std::max(lut[
static_cast<size_t>(idx) + 1], 1e-9);
717 return a * std::pow(b / a, frac);
735 void calibrateReference() noexcept
737 constexpr double kRefTones[] = { 100.0, 200.0, 400.0, 800.0, 1600.0, 3200.0, 6400.0 };
738 constexpr double kSagRef = 0.3 * 40e3;
739 const int settle =
static_cast<int>(0.060 * fs2_);
740 const int meas =
static_cast<int>(0.050 * fs2_);
741 constexpr double kAmpPerTone = 0.095 / 2.6457513;
743 for (
int st = 1; st <= 2; ++st)
745 ChannelState cal(fs2_);
746 cal.setToneControls(0.5, 0.5, 0.5);
747 cal.reset(kSagRef, st);
750 std::array<double, 7> gs1 {}, gs2 {}, gc {};
751 for (
int k = 0; k < 7; ++k)
752 gc[
static_cast<size_t>(k)] =
753 2.0 * std::cos(2.0 * std::numbers::pi * kRefTones[k] / fs2_);
754 for (
int i = 0; i < settle + meas; ++i)
757 for (
int k = 0; k < 7; ++k)
758 x += std::sin(2.0 * std::numbers::pi * kRefTones[k] * i / fs2_ + k * 1.7);
760 const double y = cal.processSample(x, st, kSagRef);
762 for (
int k = 0; k < 7; ++k)
764 auto& s1 = gs1[
static_cast<size_t>(k)];
765 auto& s2 = gs2[
static_cast<size_t>(k)];
766 const double s0 = y + gc[
static_cast<size_t>(k)] * s1 - s2;
770 std::array<double, 7> gainDb {};
771 for (
int k = 0; k < 7; ++k)
773 const double c = gc[
static_cast<size_t>(k)] * 0.5;
774 const double s1 = gs1[
static_cast<size_t>(k)], s2 = gs2[
static_cast<size_t>(k)];
775 const double mag = std::sqrt(std::max(s1 * s1 + s2 * s2 - 2.0 * c * s1 * s2, 0.0))
777 gainDb[
static_cast<size_t>(k)] =
778 20.0 * std::log10(std::max(mag / kAmpPerTone, 1e-9));
783 for (
double g : gainDb) mean += g / 7.0;
784 const double lo = 0.5 * (gainDb[0] + gainDb[1]) - mean;
785 const double mid = (gainDb[2] + gainDb[3] + gainDb[4]) / 3.0 - mean;
786 const double hi = 0.5 * (gainDb[5] + gainDb[6]) - mean;
788 std::array<BiquadCoeffs, 3> fc = {
793 for (
auto& ch : channels_)
794 ch->setFlattenCoeffs(st, fc);
802 ChannelState sweep(fs2_);
803 sweep.setToneControls(0.5, 0.5, 0.5);
804 sweep.setFlattenCoeffs(st, fc);
805 sweep.reset(kSagRef, st);
806 const int settle2 =
static_cast<int>(0.030 * fs2_);
807 const int meas2 =
static_cast<int>(0.030 * fs2_);
809 for (
int kd = 0; kd < kDriveLutN; ++kd)
811 const double d = std::pow(10.0,
812 (kDriveLutMinDb + kd * kDriveLutStepDb) / 20.0);
813 double inSq = 0.0, outSq = 0.0;
814 for (
int i = 0; i < settle2 + meas2; ++i, ++t)
817 for (
int k = 0; k < 7; ++k)
818 x += std::sin(2.0 * std::numbers::pi * kRefTones[k] * t / fs2_ + k * 1.7);
820 const double y = sweep.processSample(d * x, st, kSagRef);
827 gProgLut_[
static_cast<size_t>(st - 1)][
static_cast<size_t>(kd)] =
828 (outSq > 0.0 && inSq > 0.0) ? std::sqrt(outSq / inSq) : 1.0;
835 double sampleRate_ = 48000.0;
836 double fs2_ = 96000.0;
837 int numChannels_ = 0;
839 std::atomic<bool> prepared_ {
false };
844 std::unique_ptr<Oversampling<T>> oversampler_;
845 std::vector<std::unique_ptr<ChannelState>> channels_;
847 std::vector<std::vector<T>> dryRing_;
850 static constexpr int kDriveLutN = 13;
851 static constexpr double kDriveLutMinDb = -12.0;
852 static constexpr double kDriveLutStepDb = 4.0;
854 double hScale_ = 1.0;
855 double mScale_ = 1.0;
857 int numStagesActive_ = 1;
859 std::array<std::array<double, kDriveLutN>, 2> gProgLut_ {
860 { { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 },
861 { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 } } };
862 double hScaleSm_ = -1.0;
863 double outGainSm_ = -1.0;
864 T currentMix_ = T(1);
866 std::atomic<T> driveDb_ { T(0) };
867 std::atomic<T> treble_ { T(0.5) };
868 std::atomic<T> bass_ { T(0.5) };
869 std::atomic<T> middle_ { T(0.5) };
870 std::atomic<T> sag_ { T(0.3) };
871 std::atomic<int> stages_ { 1 };
872 std::atomic<T> outputDb_ { T(0) };
873 std::atomic<T> mix_ { T(1) };
874 std::atomic<T> supplyNow_ { T(300) };
875 std::atomic<bool> dirty_ {
true };