/*
 * vim: sw=2 ts=2 expandtab
 *
 * THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"):
 * <cfr34k@tkolb.de> wrote this file. As long as you retain this notice you can
 * do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a pizza in return. - Thomas Kolb
 */

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdint.h>

#include "config.h"

#include "fft.h"

double hanning_buffer[BLOCK_LEN];
int lookup_table[BLOCK_LEN];


void init_fft(void) {
  int i = 0;
  int ri, b;

  for(i = 0; i < BLOCK_LEN; i++) {
    hanning_buffer[i] = 0.5 * (1 - cos(2 * M_PI * i / BLOCK_LEN));
  }

  for(i = 0; i < BLOCK_LEN; i++) {
    ri = 0;

    for(b = 0; b < FFT_EXPONENT; b++)
      ri |= ((i >> b) & 1) << (FFT_EXPONENT - b - 1);

    lookup_table[i] = ri;
  }
}



void complex_to_absolute(double *re, double *im, double *result) {
  int i;

  for(i = 0; i < DATALEN; i++)
  {
    result[i] = sqrt( re[i]*re[i] + im[i]*im[i] );
  }
}



void apply_hanning(sample *dftinput) {
  int i;

  for(i = 0; i < BLOCK_LEN; i++)
    dftinput[i] *= hanning_buffer[i];
}



void fft_transform(sample *samples, double *resultRe, double *resultIm) {
  int i;
  int layer, part, element;
  int num_parts, num_elements;

  int left, right;

  double x_left_re, x_left_im, x_right_re, x_right_im;
  double param;
  double sinval, cosval;

  // re-arrange the input array according to the lookup table
  // and store it into the real output array (as the input is obviously real).
  // zero the imaginary output
  for(i = 0; i < BLOCK_LEN; i++)
  {
    resultRe[lookup_table[i]] = samples[i];
    resultIm[i] = 0;
  }

  // walk layers
  for(layer = 0; layer < FFT_EXPONENT; layer++)
  {
    // number of parts in current layer
    num_parts = 1 << (FFT_EXPONENT - layer - 1);

    // walk parts of layer
    for(part = 0; part < num_parts; part++)
    {
      // number of elements in current part
      num_elements = (1 << layer);

      // walk elements in part
      for(element = 0; element < num_elements; element++)
      {
        // calculate index of element in left and right half of part
        left = (1 << (layer + 1)) * part + element;
        right = left + (1 << layer);

        // buffer the elements for the calculation
        x_left_re = resultRe[left];
        x_left_im = resultIm[left];
        x_right_re = resultRe[right];
        x_right_im = resultIm[right];

        // precalculate the parameter for sin and cos
        param = -M_PI * element / (1 << layer);

        // precalculate sinus and cosinus values for param
        sinval = sin(param);
        cosval = cos(param);

        // combine the values according to a butterfly diagram
        resultRe[left]  = x_right_re + x_left_re * cosval - x_left_im * sinval;
        resultIm[left]  = x_right_im + x_left_im * cosval + x_left_re * sinval;
        resultRe[right] = x_right_re - x_left_re * cosval + x_left_im * sinval;
        resultIm[right] = x_right_im - x_left_im * cosval - x_left_re * sinval;
      }
    }
  }
}

uint32_t find_loudest_frequency(double *absFFT) {
  int maxPos = 0;
  double maxVal = 0;
  int i;

  for(i = 0; i < BLOCK_LEN; i++) {
    if(absFFT[i] > maxVal) {
      maxPos = i;
      maxVal = absFFT[i];
    }
  }

  return (double)maxPos * SAMPLE_RATE / BLOCK_LEN;
}

double get_energy_in_band(double *fft, uint32_t minFreq, uint32_t maxFreq) {
  int firstBlock = minFreq * BLOCK_LEN / SAMPLE_RATE;
  int lastBlock = maxFreq * BLOCK_LEN / SAMPLE_RATE;
  int i;

  double energy = 0;
  for(i = firstBlock; i < lastBlock; i++) {
    energy += fft[i];
  }

  return energy;
}