RISC-V/V203F6P6/disco/fft.cpp

//   fft.cpp - impelementation of class
//   of fast Fourier transform - FFT

#include "fft.h"
#ifdef __linux__
#include <cmath>
#include <cstdio>
class ComplexDouble {
  private:
    double re;
    double im;
  public:
    explicit constexpr ComplexDouble () noexcept : re(0.0), im(0.0) {}                           //!< konstruktor
    explicit constexpr ComplexDouble (const double x, const double y) noexcept : re(x), im(y) {} //!< konstruktor
    void  setc       (double x, double y) {re = x; im = y;}; //!< nastavení hodnoty
    void  operator&= (const ComplexDouble & x) {             //!< přičti a normuj na jednotku
      re += x.re;
      im += x.im;
      double a (re*re + im*im);
      a = 1.0 / ::sqrt (a);
      re *= a;
      im *= a;
    };
    void operator*= (const ComplexDouble & x) {  //!< znásob komplexním číslem
      const double a(x.re*re - x.im*im);
      im =           x.re*im + x.im*re;
      re = a;
    };
    double gre  (void) const {return re;};  //!< návrat reálné části
    double gim  (void) const {return im;};  //!< návrat imaginární části
};


static unsigned short Reverse [1 << FFTORDER];
static complex        Ones   [(1 << FFTORDER) >> 1];
// s tímhle nebude potřeba se zabývat - prosté přehození pořadí bitů
// Zde do pole pouze pro zrychlení (není pro to instrukce procesoru)
void IFFT::PrecomputeOrder (void) {
  const unsigned int half = N / 2;
  const unsigned int last = N - 1;
  unsigned j = 0u;
  for (unsigned i = 0; i < last; i++) {
    Reverse [i] = j;
    // printf ("i = %d, j = %d\n", i, j);
    unsigned k = half;
    while (k <= j) {
      j -= k;
      k /= 2;
    }
    j += k;
  }
  Reverse [last] = last;
}
void IFFT::PrecomputeOnes (void) {
  static constexpr double coeff = double((1UL << (HSHIFT)) - 1);
  const unsigned int half = N >> 1;
  unsigned int i,j,k;
  real reo, imo;
  // Tady to spočteme trochu jinak, bez použití sin a cos (ale stejně je potřeba sqrt)
  ComplexDouble   reone (1.0, 0.0), xx;        // reálná jednotka, pomocná proměnná
  ComplexDouble * aa = new ComplexDouble [M];  // pole komplexních jednotek - základní rotace
  k = M-1;
  aa[0].setc (0.0, 1.0);    // nultá rotace je o 90st. - komplexní jednotka
  for (i=1; i<k; i++) {     // další budou vždy polovinou předchozího úhlu
    aa[i]  = aa[i-1];       // tedy nastavíme předchozí úhel
    aa[i] &= reone;         // sečteme ho s reálnou jednotkou a normujeme
  }
  for (i=0; i<half; i++) {
    xx.setc (1.0, 0.0);     // nastavit základ, kterýn budeme rotovat = reálná jednotka
    for (j=0; j<k; j++) {   // procházíme jednotlivé bity indexu i
      // zleva - pokud je nastaven násobíme příslušně otočenou jednotkou
      if (i & (1 << (k-j-1))) xx *= aa[j];
    }
    // a sinus i kosinus je hotov
    reo = (real) (coeff * xx.gre());  // znásobíme koeficientem
    imo = (real) (coeff * xx.gim());  // a je to
    
    Ones [i].setc (reo, imo);
  }
  delete [] aa;
}
void IFFT::Precompute() {
  PrecomputeOrder();
  PrecomputeOnes();
  FILE * out = fopen("fftpre.cxx","w");
  fprintf(out, "/* GENERATED FILE - DO NOT EDIT. */\n");
  fprintf(out, "static const unsigned short Reverse [1 << FFTORDER] = {");
  for (unsigned n=0u; n<N; n++) {
    if ((n%8) == 0) fprintf(out, "\n");
    fprintf(out, " 0x%04Xu,", Reverse[n]);
  }
  fprintf(out, "\n};\n");
  /* Tímto debilním způsobem to dostanu do flash, nenašel jsem jak to udělat s complex */
  fprintf(out, "struct point {short x,y;};\n");
  fprintf(out, "static const point Ones [(1 << FFTORDER) >> 1] = {");
  const unsigned H = N >> 1;
  for (unsigned n=0u; n<H; n++) {
    if ((n%4) == 0) fprintf(out, "\n");
    complex c = Ones[n];
    fprintf(out, " {%+6d,%+6d},", c.getr(), c.geti());
  }
  fprintf(out, "\n};\n");
  fprintf(out, "static const unsigned short CosWindow [1 << FFTORDER] = {");
  static constexpr double alpha = 2.0 * M_PI / double (1 << FFTORDER), ampl = double ((1l << 15) - 1);
  for (unsigned n=0u; n<N; n++) {
    if ((n%8) == 0) fprintf(out, "\n");
    const double w = ampl * (1.0 - ::cos (double(n) * alpha));
    fprintf(out, " 0x%04lXu,", ::lround(w));
  }  
  fprintf(out, "\n};\n");
  fclose(out);
}

int main () {
  IFFT ifft;
  ifft.Precompute();
  return 0;
}
#else
#include "fftpre.cxx"
static_assert (sizeof(complex) == sizeof(point), "size failed");
void IFFT::Precompute      () {}
void IFFT::PrecomputeOnes  () {}
void IFFT::PrecomputeOrder () {}

void CopyToComplex (complex * dst, const unsigned short * src, const unsigned len) {
  for (unsigned n=0u; n<len; n++) {
    int k = src [n] - 0x800, m = CosWindow[n];
    dst [n].setc ((k * m) >> 12, 0);  // rozšířit z 12 na 16 bitů pro FFT
  }  
}
#endif

IFFT::IFFT() noexcept : reverse(Reverse), ones(reinterpret_cast<const complex * const> (Ones)) {
}

//   FORWARD FOURIER TRANSFORM, INPLACE VERSION
//     Data - both input data and output
bool IFFT::Forward (complex * const Data) const {
  if (!Data) return false;
  Rearrange (Data);
  Perform   (Data, false);
  return true;
}
//   INVERSE FOURIER TRANSFORM, INPLACE VERSION
//     Data  - both input data and output
bool IFFT::Inverse (complex * const Data) const {
  if (!Data) return false;
  Rearrange (Data);
  Perform   (Data, true);
  return true;
}
// Změna pořadí ve vstupním poli
void IFFT::Rearrange (complex * const Data) const {
  unsigned int Target, Position;
  complex      Temp;
  for (Position = 0; Position < N; Position++) {
    Target = reverse [Position];
    if (Target > Position) {
      Temp            = Data [Target];
      Data [Target]   = Data [Position];
      Data [Position] = Temp;
    }
  }
}

//   FFT implementation
void IFFT::Perform (complex * const Data, const bool Inverse) const {
  unsigned int Dest, Step, Jump, Group, Pair, Incr, Roti;
  complex Factor, Produc;
  static constexpr real Shift = 1;     // Úprava velikosti při forwardu, jinak to přeteče

  for (Step = 1; Step < N; Step <<= 1) {   // 1,2,...N/2
    // printf ("1.cyklus Step=%4d\n", Step);
    Jump = Step << 1;   // 2,4,...N
    Incr = N / Jump;    // N/2....1
    Roti = 0;
    for (Group = 0; Group < Step; Group++) {
      // printf (" 2.cyklus Group=%4d, rotace=%4d\n", Group, Roti);
      for (Pair = Group; Pair < N; Pair += Jump) {
        Dest   = Pair + Step;
        Factor = ones [Roti];
        Produc = Data [Pair];
        if (!Inverse) {
          Factor.conj();
          Factor >>= Shift;          // shifty s integer doprava o 1 bit
          Produc >>= Shift;          // Zároveň normalizace (není potřeba při reverzi).
        }
        Factor *= Data [Dest];
        // printf ("  3.cyklus Pair=%4d, Dest =%4d\n", Pair, Dest);
        Data [Pair].add (Produc, Factor);
        Data [Dest].sub (Produc, Factor);
      }
      Roti += Incr;
    }
  }
}