/* Last edited on 2008-02-08 13:40:17 by stolfi */

#include <math.h>

#include <affirm.h>

#include <pz_types.h>
#include <pz_proc.h>
#include <pz_fourier.h>

/* INTERNAL PROTOTYPES */

void pz_fix_fft(double_vec_t *v, double_vec_t *F);
int pz_bin_power_gtr(int n);

/* IMPLEMENTATIONS */

void pz_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};
        pz_fourier_transform(&F0, &V0);
        pz_fourier_transform(&F1, &V1); 
        pz_fix_fft(V, F);
      }
  }

void pz_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;
    }
  }

unsigned  pz_fourier_compute_order(double band, double step, double length) 
  { 
    int minN = (int)ceil(length/fmin(band/4.0, step));
    return pz_bin_power_gtr(minN);
  }

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

void pz_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: affirm(FALSE, "bug");
                  }
              }
            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: affirm(FALSE, "bug");
                  }
              }
          }
      }
  }