third_party/rnnoise/src/kiss_fft.cc

/*Copyright (c) 2003-2004, Mark Borgerding
  Lots of modifications by Jean-Marc Valin
  Copyright (c) 2005-2007, Xiph.Org Foundation
  Copyright (c) 2008,      Xiph.Org Foundation, CSIRO
  Copyright (c) 2018,      The WebRTC project authors

  All rights reserved.

  Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
       this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.*/

#include "third_party/rnnoise/src/kiss_fft.h"

#include <cassert>
#include <cmath>
#include <utility>

namespace rnnoise {
namespace {

void kf_bfly2(std::complex<float>* Fout, int m, int N) {
  if (m == 1) {
    for (int i = 0; i < N; i++) {
      const std::complex<float> t = Fout[1];
      Fout[1] = Fout[0] - t;
      Fout[0] += t;
      Fout += 2;
    }
  } else {
    constexpr float tw = 0.7071067812f;
    // We know that m==4 here because the radix-2 is just after a radix-4.
    assert(m == 4);
    for (int i = 0; i < N; i++) {
      std::complex<float>* Fout2 = Fout + 4;
      std::complex<float> t = Fout2[0];

      *Fout2 = Fout[0] - t;
      Fout[0] += t;

      t.real((Fout2[1].real() + Fout2[1].imag()) * tw);
      t.imag((Fout2[1].imag() - Fout2[1].real()) * tw);
      Fout2[1] = Fout[1] - t;
      Fout[1] += t;

      t.real(Fout2[2].imag());
      t.imag(-Fout2[2].real());
      Fout2[2] = Fout[2] - t;
      Fout[2] += t;

      t.real((Fout2[3].imag() - Fout2[3].real()) * tw);
      t.imag(-(Fout2[3].imag() + Fout2[3].real()) * tw);
      Fout2[3] = Fout[3] - t;
      Fout[3] += t;
      Fout += 8;
    }
  }
}

void kf_bfly4(std::complex<float>* Fout,
              const size_t fstride,
              const KissFft::KissFftState* st,
              int m,
              int N,
              int mm) {
  assert(Fout);
  assert(st);
  if (m == 1) {
    // Degenerate case where all the twiddles are 1.
    for (int i = 0; i < N; i++) {
      std::complex<float> scratch0 = Fout[0] - Fout[2];
      Fout[0] += Fout[2];
      std::complex<float> scratch1 = Fout[1] + Fout[3];
      Fout[2] = Fout[0] - scratch1;
      Fout[0] += scratch1;
      scratch1 = Fout[1] - Fout[3];

      Fout[1].real(scratch0.real() + scratch1.imag());
      Fout[1].imag(scratch0.imag() - scratch1.real());
      Fout[3].real(scratch0.real() - scratch1.imag());
      Fout[3].imag(scratch0.imag() + scratch1.real());
      Fout += 4;
    }
  } else {
    std::complex<float> scratch[6];
    const int m2 = 2 * m;
    const int m3 = 3 * m;
    std::complex<float>* Fout_beg = Fout;
    for (int i = 0; i < N; i++) {
      Fout = Fout_beg + i * mm;
      const std::complex<float>* tw1;
      const std::complex<float>* tw2;
      const std::complex<float>* tw3;
      tw3 = tw2 = tw1 = &st->twiddles[0];
      assert(m % 4 == 0);  // |m| is guaranteed to be a multiple of 4.
      for (int j = 0; j < m; j++) {
        scratch[0] = Fout[m] * *tw1;
        scratch[1] = Fout[m2] * *tw2;
        scratch[2] = Fout[m3] * *tw3;

        scratch[5] = Fout[0] - scratch[1];
        Fout[0] += scratch[1];
        scratch[3] = scratch[0] + scratch[2];
        scratch[4] = scratch[0] - scratch[2];
        Fout[m2] = Fout[0] - scratch[3];

        tw1 += fstride;
        tw2 += fstride * 2;
        tw3 += fstride * 3;
        Fout[0] += scratch[3];

        Fout[m].real(scratch[5].real() + scratch[4].imag());
        Fout[m].imag(scratch[5].imag() - scratch[4].real());
        Fout[m3].real(scratch[5].real() - scratch[4].imag());
        Fout[m3].imag(scratch[5].imag() + scratch[4].real());
        ++Fout;
      }
    }
  }
}

void kf_bfly3(std::complex<float>* Fout,
              const size_t fstride,
              const KissFft::KissFftState* st,
              int m,
              int N,
              int mm) {
  assert(Fout);
  assert(st);
  const size_t m2 = 2 * m;
  const std::complex<float>*tw1, *tw2;
  std::complex<float> scratch[5];
  std::complex<float> epi3;

  std::complex<float>* Fout_beg = Fout;
  epi3 = st->twiddles[fstride * m];
  for (int i = 0; i < N; i++) {
    Fout = Fout_beg + i * mm;
    tw1 = tw2 = &st->twiddles[0];
    size_t k = m;
    do {
      scratch[1] = Fout[m] * *tw1;
      scratch[2] = Fout[m2] * *tw2;

      scratch[3] = scratch[1] + scratch[2];
      scratch[0] = scratch[1] - scratch[2];
      tw1 += fstride;
      tw2 += fstride * 2;

      Fout[m] = Fout[0] - 0.5f * scratch[3];

      scratch[0] *= epi3.imag();

      Fout[0] += scratch[3];

      Fout[m2].real(Fout[m].real() + scratch[0].imag());
      Fout[m2].imag(Fout[m].imag() - scratch[0].real());

      Fout[m].real(Fout[m].real() - scratch[0].imag());
      Fout[m].imag(Fout[m].imag() + scratch[0].real());

      ++Fout;
    } while (--k);
  }
}

void kf_bfly5(std::complex<float>* Fout,
              const size_t fstride,
              const KissFft::KissFftState* st,
              int m,
              int N,
              int mm) {
  assert(Fout);
  assert(st);
  std::complex<float> scratch[13];
  const std::complex<float>* tw;
  std::complex<float> ya, yb;
  std::complex<float>* const Fout_beg = Fout;

  ya = st->twiddles[fstride * m];
  yb = st->twiddles[fstride * 2 * m];
  tw = &st->twiddles[0];

  for (int i = 0; i < N; i++) {
    Fout = Fout_beg + i * mm;
    std::complex<float>* Fout0 = Fout;
    std::complex<float>* Fout1 = Fout0 + m;
    std::complex<float>* Fout2 = Fout0 + 2 * m;
    std::complex<float>* Fout3 = Fout0 + 3 * m;
    std::complex<float>* Fout4 = Fout0 + 4 * m;

    // For non-custom modes, m is guaranteed to be a multiple of 4.
    for (int u = 0; u < m; ++u) {
      scratch[0] = *Fout0;

      scratch[1] = *Fout1 * tw[u * fstride];
      scratch[2] = *Fout2 * tw[2 * u * fstride];
      scratch[3] = *Fout3 * tw[3 * u * fstride];
      scratch[4] = *Fout4 * tw[4 * u * fstride];

      scratch[7] = scratch[1] + scratch[4];
      scratch[10] = scratch[1] - scratch[4];
      scratch[8] = scratch[2] + scratch[3];
      scratch[9] = scratch[2] - scratch[3];

      Fout0->real(Fout0->real() + scratch[7].real() + scratch[8].real());
      Fout0->imag(Fout0->imag() + scratch[7].imag() + scratch[8].imag());

      scratch[5].real(scratch[0].real() + scratch[7].real() * ya.real() +
                      scratch[8].real() * yb.real());
      scratch[5].imag(scratch[0].imag() + scratch[7].imag() * ya.real() +
                      scratch[8].imag() * yb.real());

      scratch[6].real(scratch[10].imag() * ya.imag() +
                      scratch[9].imag() * yb.imag());
      scratch[6].imag(
          -(scratch[10].real() * ya.imag() + scratch[9].real() * yb.imag()));

      *Fout1 = scratch[5] - scratch[6];
      *Fout4 = scratch[5] + scratch[6];

      scratch[11].real(scratch[0].real() + scratch[7].real() * yb.real() +
                       scratch[8].real() * ya.real());
      scratch[11].imag(scratch[0].imag() + scratch[7].imag() * yb.real() +
                       scratch[8].imag() * ya.real());
      scratch[12].real(scratch[9].imag() * ya.imag() -
                       scratch[10].imag() * yb.imag());
      scratch[12].imag(scratch[10].real() * yb.imag() -
                       scratch[9].real() * ya.imag());

      *Fout2 = scratch[11] + scratch[12];
      *Fout3 = scratch[11] - scratch[12];

      ++Fout0;
      ++Fout1;
      ++Fout2;
      ++Fout3;
      ++Fout4;
    }
  }
}

void compute_bitrev_table(int base_index,
                          const size_t stride,
                          const int16_t* factors,
                          const KissFft::KissFftState* st,
                          const int16_t* bitrev_table_last,
                          int16_t* bitrev_table) {
  const int p = *factors++;  // The radix.
  const int m = *factors++;  // Stage's fft length/p.
  if (m == 1) {
    for (int j = 0; j < p; j++) {
      assert(bitrev_table <= bitrev_table_last);
      *bitrev_table = base_index + j;
      bitrev_table += stride;
    }
  } else {
    for (int j = 0; j < p; j++) {
      compute_bitrev_table(base_index, stride * p, factors, st,
                           bitrev_table_last, bitrev_table);
      bitrev_table += stride;
      base_index += m;
    }
  }
}

// Populates |facbuf| with p1, m1, p2, m2, ... where p[i] * m[i] = m[i-1] and
// m0 = n.
bool kf_factor(int n, int16_t* facbuf) {
  assert(facbuf);
  int p = 4;
  int stages = 0;
  int nbak = n;

  // Factor out powers of 4, powers of 2, then any remaining primes.
  do {
    while (n % p) {
      switch (p) {
        case 4:
          p = 2;
          break;
        case 2:
          p = 3;
          break;
        default:
          p += 2;
          break;
      }
      if (p > 32000 || p * p > n)
        p = n;  // No more factors, skip to end.
    }
    n /= p;
    if (p > 5)
      return false;
    facbuf[2 * stages] = p;
    if (p == 2 && stages > 1) {
      facbuf[2 * stages] = 4;
      facbuf[2] = 2;
    }
    stages++;
  } while (n > 1);
  n = nbak;
  // Reverse the order to get the radix 4 at the end, so we can use the
  // fast degenerate case. It turns out that reversing the order also
  // improves the noise behavior.
  for (int i = 0; i < stages / 2; i++)
    std::swap(facbuf[2 * i], facbuf[2 * (stages - i - 1)]);
  for (int i = 0; i < stages; i++) {
    n /= facbuf[2 * i];
    facbuf[2 * i + 1] = n;
  }
  return true;
}

void compute_twiddles(const int nfft, std::complex<float>* twiddles) {
  constexpr double pi = 3.14159265358979323846264338327;
  assert(twiddles);
  for (int i = 0; i < nfft; ++i) {
    const double phase = (-2 * pi / nfft) * i;
    twiddles[i].real(std::cos(phase));
    twiddles[i].imag(std::sin(phase));
  }
}

void fft_impl(const KissFft::KissFftState* st, std::complex<float>* fout) {
  assert(st);
  assert(fout);
  int m2, m;
  int p;
  int L;
  int fstride[KissFft::kMaxFactors];

  fstride[0] = 1;
  L = 0;
  do {
    p = st->factors[2 * L];
    m = st->factors[2 * L + 1];
    assert(static_cast<size_t>(L + 1) < KissFft::kMaxFactors);
    fstride[L + 1] = fstride[L] * p;
    L++;
  } while (m != 1);
  m = st->factors[2 * L - 1];
  for (int i = L - 1; i >= 0; i--) {
    if (i != 0)
      m2 = st->factors[2 * i - 1];
    else
      m2 = 1;
    switch (st->factors[2 * i]) {
      case 2:
        kf_bfly2(fout, m, fstride[i]);
        break;
      case 4:
        kf_bfly4(fout, fstride[i], st, m, fstride[i], m2);
        break;
      case 3:
        kf_bfly3(fout, fstride[i], st, m, fstride[i], m2);
        break;
      case 5:
        kf_bfly5(fout, fstride[i], st, m, fstride[i], m2);
        break;
      default:
        assert(0);
        break;
    }
    m = m2;
  }
}

}  // namespace

KissFft::KissFftState::KissFftState(int num_fft_points)
    : nfft(num_fft_points), scale(1.f / nfft) {
  // Factorize |nfft|.
  // TODO(alessiob): Handle kf_factor fails (invalid nfft).
  if (!kf_factor(nfft, factors.data()))
    assert(0);
  // Twiddles.
  twiddles.resize(nfft);
  compute_twiddles(nfft, twiddles.data());
  // Bit-reverse table.
  bitrev.resize(nfft);
  compute_bitrev_table(0, 1, factors.data(), this, &bitrev.back(),
                       bitrev.data());
}

KissFft::KissFftState::~KissFftState() = default;

KissFft::KissFft(const int nfft) : state_(nfft) {}

KissFft::~KissFft() = default;

void KissFft::ForwardFft(const size_t in_size,
                         const std::complex<float>* in,
                         const size_t out_size,
                         std::complex<float>* out) {
  assert(in);
  assert(out);
  assert(in != out);  // In-place FFT not supported.
  assert(state_.nfft == static_cast<int>(in_size));
  assert(state_.nfft == static_cast<int>(out_size));
  // Bit-reverse the input.
  for (int i = 0; i < state_.nfft; i++)
    out[state_.bitrev[i]] = state_.scale * in[i];
  fft_impl(&state_, out);
}

void KissFft::ReverseFft(const size_t in_size,
                         const std::complex<float>* in,
                         const size_t out_size,
                         std::complex<float>* out) {
  assert(in);
  assert(out);
  assert(in != out);  // In-place IFFT not supported.
  assert(state_.nfft == static_cast<int>(in_size));
  assert(state_.nfft == static_cast<int>(out_size));
  // Bit-reverse the input.
  for (int i = 0; i < state_.nfft; i++)
    out[state_.bitrev[i]] = in[i];
  for (int i = 0; i < state_.nfft; i++)
    out[i].imag(-out[i].imag());
  fft_impl(&state_, out);
  for (int i = 0; i < state_.nfft; i++)
    out[i].imag(-out[i].imag());
}

}  // namespace rnnoise