/* See frb_fourier.h */
/* Last edited on 2008-07-14 20:37:41 by stolfi */

#include <frb_types.h>
#include <frb_proc.h>
#include <frb_fourier.h>

#include <affirm.h>

#include <math.h>
#include <assert.h>

/* INTERNAL PROTOTYPES */

void frb_fix_fft( double_vec_t *v, double_vec_t *F );
int frb_bin_power_gtr ( int n );

/* IMPLEMENTATIONS */

void frb_fourier_transform ( double_vec_t *V, double_vec_t *F )
  { 
    int N = V->nel;
    if (N == 1)
      { F->el[0] = V->el[0]; }
    else
      { int H = N / 2;
        affirm(N == H + H, "N not even");
        int i;
        for (i = 0; i < H; i++) { F->el[i] = V->el[i*2]; F->el[i+H] = V->el[i*2+1]; };
        double_vec_t F0 = {H, F->el}, F1 = {H, F->el + H};
        double_vec_t V0 = {H, V->el}, V1 = {H, V->el + H};
        frb_fourier_transform(&F0, &V0);
        frb_fourier_transform(&F1, &V1); 
        frb_fix_fft(V, F);
      }
  }

void frb_fix_fft( double_vec_t *v, double_vec_t *F )
  {
    double c2, s2, cj, sj; 
    int N = v->nel;
    int H = N / 2;
    double c1 = cos(2.0 * M_PI / (double)N);
    double s1 = sin(2.0 * M_PI / (double)N);
    double coef = 1.0 / sqrt(2.00);

    cj =  c1; sj = s1;

    F->el[0] = coef * (v->el[0] + v->el[H]);
    F->el[H] = coef * (v->el[0] - v->el[H]); 

    int j;
    for (j = 1; j <= H-1 ; j++)
      { F->el[j] = coef * (v->el[j] + cj * v->el[j+H] - sj * v->el[N-j]);
        c2 = cj * c1 - sj * s1;
        s2 = sj * c1 + cj * s1;
        cj = c2; sj = s2;
      }

    cj = c1; sj = s1;  

    for (j = H+1; j < N; j++)
    { 
      F->el[j] = coef * ( v->el[j-H] - cj * v->el[j] + sj * v->el[N+H-j]);
      c2 = cj * c1 - sj * s1;
      s2 = sj * c1 + cj * s1;
      cj = c2; sj = s2;
    }
  }

void frb_power_spectrum (double_vec_t *F, double_vec_t *P)
{
  int nf = F->nel;
  int np = nf/2 + 1;
  affirm(P->nel == np, "bad power spectrum size");
  int k;
  for (k = 0; k < np; k++)
    { int f0 = k;
      int f1 = (nf - k) % nf;
      assert(f1 >= 0);
      double Pk;
      { double C = F->el[f0]; Pk = C*C; }
      if (f1 > f0) { double C = F->el[f1]; Pk += C*C; }
      P->el[k] = Pk;
    }
}

unsigned frb_fourier_compute_order( double band, double step, double length ) 
  { 
    int minN = (int)ceil(length/frb_dmin(band/4.0, step));
    return frb_bin_power_gtr(minN);
  }

int frb_bin_power_gtr ( int n )
  { 
    int x = 1;
    while (x <= n){ x = x + x; }
    return x;
  }

void frb_fourier_diff ( double_vec_t *F, unsigned order )
  {
    int N = F->nel;
    int H = N / 2; /* Max freq.*/
    if (order > 0)
      { F->el[0] = 0.0;
        int i;
        for (i = 1 ; i <= H ; i++)
          { double p = pow(((double)i),((double)order));
            double u = p * F->el[i];
            double v = p * F->el[N-i];
            if (i < H)
              { switch (order % 4)
                  { case 0: F->el[i] = +u; F->el[N-i] = +v; break;
                    case 1: F->el[i] = -v; F->el[N-i] = +u; break;
                    case 2: F->el[i] = -u; F->el[N-i] = -v; break;
                    case 3: F->el[i] = +v; F->el[N-i] = -u; break;
                    default: assert(FALSE);
                  }
              }
            else
              { switch (order % 4)
                  { case 0: F->el[i] = +u;  break;
                    case 1: F->el[i] = 0.0; break;
                    case 2: F->el[i] = -u;  break;
                    case 3: F->el[i] = 0.0; break;
                    default: assert(FALSE);
                  }
              }
          }
      }
  }