// Independent true-peak polyphase 4x, windowed-sinc. Not ITU-certified.
// g++ -O2 -std=c++17 -o /tmp/tp_poly /tmp/tp_poly.cpp && /tmp/tp_poly
#include <cmath>
#include <cstdio>
#include <vector>
#include <algorithm>
static constexpr double PI = 3.14159265358979323846;
static double sinc(double x) {
  if (std::fabs(x) < 1e-12) return 1.0;
  return std::sin(PI * x) / (PI * x);
}
static double blackman(int n, int N) {
  if (N <= 1) return 1.0;
  double i = (double)n / (double)(N - 1);
  return 0.42 - 0.5 * std::cos(2 * PI * i) + 0.08 * std::cos(4 * PI * i);
}
// L-phase polyphase, taps_per_phase, cutoff as fraction of Nyquist (0.5 fs)
static std::vector<std::vector<double>> design(int L, int taps, double cutoff) {
  int N = L * taps;
  std::vector<double> h(N);
  double sum = 0;
  double fc = cutoff; // cycles per input sample; 1.0 = Nyquist of input
  int mid = (N - 1) / 2;
  for (int n = 0; n < N; n++) {
    double x = (n - mid) / (double)L;
    h[n] = sinc(x * fc) * blackman(n, N) * fc;
    sum += h[n];
  }
  for (double &v : h) v *= (double)L / sum; // preserve gain at DC after L upsample
  std::vector<std::vector<double>> ph(L, std::vector<double>(taps, 0.0));
  for (int n = 0; n < N; n++) ph[n % L][n / L] = h[n];
  return ph;
}
static double peak_db(const std::vector<double> &x) {
  double m = 0;
  for (double v : x) m = std::max(m, std::fabs(v));
  return 20.0 * std::log10(std::max(m, 1e-12));
}
static std::vector<double> upsample4(const std::vector<double> &x,
                                    const std::vector<std::vector<double>> &ph) {
  int L = (int)ph.size();
  int taps = (int)ph[0].size();
  std::vector<double> y(x.size() * L, 0.0);
  for (size_t i = 0; i < x.size(); i++) {
    for (int p = 0; p < L; p++) {
      double acc = 0;
      for (int k = 0; k < taps; k++) {
        int j = (int)i - k;
        if (j >= 0) acc += ph[p][k] * x[j];
      }
      y[i * L + p] = acc;
    }
  }
  return y;
}
static std::vector<double> sine_fs4(int n, double A) {
  std::vector<double> x(n);
  for (int i = 0; i < n; i++)
    x[i] = A * std::sin(2 * PI * 0.25 * i + PI / 4);
  return x;
}
static std::vector<double> tanh_clip_hf(int n, double drive) {
  // generate at 4x, tanh, decimate by taking every 4th AFTER generating mix
  int n4 = n * 4;
  std::vector<double> y(n);
  for (int i = 0; i < n; i++) {
    // mix fs/4 + 0.47*fs (near nyquist) then clip in 4x domain, take sample
    double acc = 0;
    for (int p = 0; p < 4; p++) {
      double t = (i * 4 + p) / 4.0;
      double s = 0.7 * std::sin(2 * PI * 0.25 * t) +
                 0.7 * std::sin(2 * PI * 0.47 * t);
      acc = std::tanh(drive * s); // last phase used as "held" — better: store all then pick
    }
    double t0 = i;
    double s = 0.7 * std::sin(2 * PI * 0.25 * t0) +
               0.7 * std::sin(2 * PI * 0.47 * t0);
    // actually clip at input rate is wrong; do 4x:
    (void)s;
  }
  // proper 4x clip then ::4
  std::vector<double> x4(n4);
  for (int i = 0; i < n4; i++) {
    double t = i / 4.0;
    double s = 0.85 * std::sin(2 * PI * 0.25 * t) +
               0.85 * std::sin(2 * PI * 0.47 * t);
    x4[i] = std::tanh(drive * s);
  }
  for (int i = 0; i < n; i++) y[i] = x4[i * 4];
  return y;
}
static std::vector<double> trim(const std::vector<double> &x, int e) {
  if ((int)x.size() <= 2 * e) return x;
  return std::vector<double>(x.begin() + e, x.end() - e);
}
int main() {
  const int L = 4;
  auto p12 = design(L, 12, 0.92);
  auto p48 = design(L, 48, 1.00);
  int n = 48000; // 1s @ 48k
  auto oracle = sine_fs4(n, 0.5);
  auto otrim = trim(oracle, 512);
  printf("oracle fs/4 phase pi/4 A=0.5  analytic=-6.0206 dBTP\n");
  printf("sample-peak dBFS          %.4f\n", peak_db(otrim));
  auto u12 = trim(upsample4(oracle, p12), 512 * L);
  auto u48 = trim(upsample4(oracle, p48), 512 * L);
  printf("12@0.92 4x peak dBTP      %.4f  err_vs_analytic %.4f\n",
         peak_db(u12), peak_db(u12) - (-6.020599913279624));
  printf("48@1.00 4x peak dBTP      %.4f  err_vs_analytic %.4f\n",
         peak_db(u48), peak_db(u48) - (-6.020599913279624));
  auto clip = tanh_clip_hf(n, 6.0);
  auto ctrim = trim(clip, 512);
  auto c12 = trim(upsample4(clip, p12), 512 * L);
  auto c48 = trim(upsample4(clip, p48), 512 * L);
  printf("tanh-clip drive6 sample   %.4f\n", peak_db(ctrim));
  printf("tanh-clip 12@0.92 dBTP    %.4f  vs48 %+0.4f\n",
         peak_db(c12), peak_db(c12) - peak_db(c48));
  printf("tanh-clip 48@1.00 dBTP    %.4f\n", peak_db(c48));
  printf("note: windowed-sinc polyphase, NOT ITU BS.1770 coefficients\n");
  return 0;
}