#include "rpi_spectrum.h"

#include <fftw3.h>
#include <math.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "rpi_microphone.h"

typedef struct {
  int framerate;
  int number_of_bars;
  int lower_cut_off, upper_cut_off;
} param_t;

typedef struct {
  double sens;
  double gravity;
  double integral;
  double userEQ[128];
} config_t;

param_t param = {
    .framerate = 60, .number_of_bars = 128, .lower_cut_off = 50, .upper_cut_off = 10000};
config_t conf = {.sens = 1, .gravity = 0, .integral = 0, .userEQ = {1}};

pthread_t microphoneListener;

audio_data_t audio;

fftw_complex *out_bass_l;
fftw_plan p_bass_l;
fftw_complex *out_mid_l;
fftw_plan p_mid_l;
fftw_complex *out_treble_l;
fftw_plan p_treble_l;

// input: init
int *bars_left;
double *temp_l;

int bass_cut_off = 150;
int treble_cut_off = 2500;

void init_plan(int bufferSize, double **in, double **in_raw, fftw_complex **out, fftw_plan *p);

void spectrum_start_bg_worker();

void spectrum_stop_bg_worker();

void spectrum_setup(char *audio_source) {
  for (size_t i = 0; i < 128; i++) {
    conf.userEQ[i] = 1;
  }

  memset(&audio, 0, sizeof(audio));

  audio.source = malloc(1 + strlen(audio_source));
  strcpy(audio.source, audio_source);

  audio.format = -1;
  audio.rate = 0;
  audio.FFTbassbufferSize = 4096;
  audio.FFTmidbufferSize = 2048;
  audio.FFTtreblebufferSize = 1024;
  audio.terminate = 0;
  audio.channels = 1;
  audio.average = false;
  audio.left = true;
  audio.right = false;
  audio.bass_index = 0;
  audio.mid_index = 0;
  audio.treble_index = 0;
  audio.bass_multiplier = (double *)malloc(audio.FFTbassbufferSize * sizeof(double));
  audio.mid_multiplier = (double *)malloc(audio.FFTmidbufferSize * sizeof(double));
  audio.treble_multiplier = (double *)malloc(audio.FFTtreblebufferSize * sizeof(double));

  temp_l = (double *)malloc((audio.FFTbassbufferSize / 2 + 1) * sizeof(double));

  bars_left = (int *)malloc(256 * sizeof(int));

  for (int i = 0; i < audio.FFTbassbufferSize; i++) {
    audio.bass_multiplier[i] = 0.5 * (1 - cos(2 * M_PI * i / (audio.FFTbassbufferSize - 1)));
  }
  for (int i = 0; i < audio.FFTmidbufferSize; i++) {
    audio.mid_multiplier[i] = 0.5 * (1 - cos(2 * M_PI * i / (audio.FFTmidbufferSize - 1)));
  }
  for (int i = 0; i < audio.FFTtreblebufferSize; i++) {
    audio.treble_multiplier[i] = 0.5 * (1 - cos(2 * M_PI * i / (audio.FFTtreblebufferSize - 1)));
  }

  // BASS
  // audio.FFTbassbufferSize =  audio.rate / 20; // audio.FFTbassbufferSize;
  // audio.in_bass_l = fftw_alloc_real(audio.FFTbassbufferSize);
  // audio.in_bass_l_raw = fftw_alloc_real(audio.FFTbassbufferSize);

  // out_bass_l = fftw_alloc_complex(audio.FFTbassbufferSize / 2 + 1);
  // memset(out_bass_l, 0, (audio.FFTbassbufferSize / 2 + 1) * sizeof(fftw_complex));

  // p_bass_l =
  //     fftw_plan_dft_r2c_1d(audio.FFTbassbufferSize, audio.in_bass_l, out_bass_l, FFTW_MEASURE);

  init_plan(audio.FFTbassbufferSize, &audio.in_bass_l, &audio.in_bass_l_raw, &out_bass_l,
            &p_bass_l);

  // MID
  // audio.FFTmidbufferSize =  audio.rate / bass_cut_off; // audio.FFTbassbufferSize;
  // audio.in_mid_l = fftw_alloc_real(audio.FFTmidbufferSize);
  // audio.in_mid_l_raw = fftw_alloc_real(audio.FFTmidbufferSize);

  // out_mid_l = fftw_alloc_complex(audio.FFTmidbufferSize / 2 + 1);
  // memset(out_mid_l, 0, (audio.FFTmidbufferSize / 2 + 1) * sizeof(fftw_complex));

  // p_mid_l = fftw_plan_dft_r2c_1d(audio.FFTmidbufferSize, audio.in_mid_l, out_mid_l,
  // FFTW_MEASURE);

  init_plan(audio.FFTmidbufferSize, &audio.in_mid_l, &audio.in_mid_l_raw, &out_mid_l, &p_mid_l);

  // TRIEBLE
  // audio.FFTtreblebufferSize =  audio.rate / treble_cut_off; // audio.FFTbassbufferSize;
  // audio.in_treble_l = fftw_alloc_real(audio.FFTtreblebufferSize);
  // audio.in_treble_l_raw = fftw_alloc_real(audio.FFTtreblebufferSize);

  // out_treble_l = fftw_alloc_complex(audio.FFTtreblebufferSize / 2 + 1);
  // memset(out_treble_l, 0, (audio.FFTtreblebufferSize / 2 + 1) * sizeof(fftw_complex));

  // p_treble_l = fftw_plan_dft_r2c_1d(audio.FFTtreblebufferSize, audio.in_treble_l,
  // out_treble_l,
  //                                   FFTW_MEASURE);

  init_plan(audio.FFTtreblebufferSize, &audio.in_treble_l, &audio.in_treble_l_raw, &out_treble_l,
            &p_treble_l);

  reset_output_buffers(&audio);

  spectrum_start_bg_worker();
}

void spectrum_start_bg_worker() {
  pthread_mutex_init(&audio.lock, NULL);
  if (pthread_create(&microphoneListener, NULL, microphone_listen, (void *)&audio) < 0) {
    perror("pthread_create");
  }

  int timeout_counter = 0;
  while (audio.rate == 0) {
    usleep(2000);
    ++timeout_counter;
    if (timeout_counter > 2000) {
      fprintf(stderr, "could not get rate and/or format, problems with audio thread? "
                      "quiting...\n");
      exit(EXIT_FAILURE);
    }
  }
}

void spectrum_stop_bg_worker() {
  if (pthread_cancel(microphoneListener) != 0) {
    perror("pthread_cancel");
  }
  if (pthread_join(microphoneListener, NULL) != 0) {
    perror("pthread_join");
  }
}

void spectrum_execute() {
  int bars[256];
  int bars_mem[256];
  int bars_last[256];
  int previous_frame[256];
  int fall[256];
  float bars_peak[256];

  int height, lines, width, remainder, fp;

  bool reloadConf = false;

  for (int n = 0; n < 256; n++) {
    bars_last[n] = 0;
    previous_frame[n] = 0;
    fall[n] = 0;
    bars_peak[n] = 0;
    bars_mem[n] = 0;
    bars[n] = 0;
  }

  width = 256;
  height = UINT16_MAX;

  // getting numbers of bars
  int number_of_bars = param.number_of_bars;

  if (number_of_bars > 256)
    number_of_bars = 256; // cant have more than 256 bars

  // process [smoothing]: calculate gravity
  float g = conf.gravity * ((float)height / 2160) * pow((60 / (float)param.framerate), 2.5);

  // calculate integral value, must be reduced with height
  double integral = conf.integral;
  if (height > 320)
    integral = conf.integral * 1 / sqrt((log10((float)height / 10)));

  // process: calculate cutoff frequencies and eq

  double userEQ_keys_to_bars_ratio;

  if (number_of_bars > 0) {
    userEQ_keys_to_bars_ratio = (double)(((double)(number_of_bars < 128 ? number_of_bars : 128)) /
                                         ((double)number_of_bars));
  }

  // calculate frequency constant (used to distribute bars across the frequency band)
  double frequency_constant = log10((float)param.lower_cut_off / (float)param.upper_cut_off) /
                              (1 / ((float)number_of_bars + 1) - 1);

  float cut_off_frequency[256];
  float upper_cut_off_frequency[256];
  float relative_cut_off[256];
  double center_frequencies[256];
  int FFTbuffer_lower_cut_off[256];
  int FFTbuffer_upper_cut_off[256];
  double eq[256];

  int bass_cut_off_bar = -1;
  int treble_cut_off_bar = -1;
  bool first_bar = true;
  int first_treble_bar = 0;
  int bar_buffer[number_of_bars + 1];

  for (int n = 0; n < number_of_bars + 1; n++) {
    double bar_distribution_coefficient = frequency_constant * (-1);
    bar_distribution_coefficient +=
        ((float)n + 1) / ((float)number_of_bars + 1) * frequency_constant;
    cut_off_frequency[n] = param.upper_cut_off * pow(10, bar_distribution_coefficient);

    if (n > 0) {
      if (cut_off_frequency[n - 1] >= cut_off_frequency[n] &&
          cut_off_frequency[n - 1] > bass_cut_off)
        cut_off_frequency[n] =
            cut_off_frequency[n - 1] + (cut_off_frequency[n - 1] - cut_off_frequency[n - 2]);
    }

    relative_cut_off[n] = cut_off_frequency[n] / (audio.rate / 2);
    // remember nyquist!, per my calculations this should be rate/2
    // and nyquist freq in M/2 but testing shows it is not...
    // or maybe the nq freq is in M/4

    eq[n] = pow(cut_off_frequency[n], 1);

    // the numbers that come out of the FFT are very high
    // the EQ is used to "normalize" them by dividing with this very huge number
    eq[n] *= (float)height / pow(2, 28);

    eq[n] *= conf.userEQ[(int)floor(((double)n) * userEQ_keys_to_bars_ratio)];

    eq[n] /= log2(audio.FFTbassbufferSize);

    if (cut_off_frequency[n] < bass_cut_off) {
      // BASS
      bar_buffer[n] = 1;
      FFTbuffer_lower_cut_off[n] = relative_cut_off[n] * (audio.FFTbassbufferSize / 2);
      bass_cut_off_bar++;
      treble_cut_off_bar++;
      if (bass_cut_off_bar > 0)
        first_bar = false;

      eq[n] *= log2(audio.FFTbassbufferSize);
    } else if (cut_off_frequency[n] > bass_cut_off && cut_off_frequency[n] < treble_cut_off) {
      // MID
      bar_buffer[n] = 2;
      FFTbuffer_lower_cut_off[n] = relative_cut_off[n] * (audio.FFTmidbufferSize / 2);
      treble_cut_off_bar++;
      if ((treble_cut_off_bar - bass_cut_off_bar) == 1) {
        first_bar = true;
        FFTbuffer_upper_cut_off[n - 1] = relative_cut_off[n] * (audio.FFTbassbufferSize / 2);
      } else {
        first_bar = false;
      }

      eq[n] *= log2(audio.FFTmidbufferSize);
    } else {
      // TREBLE
      bar_buffer[n] = 3;
      FFTbuffer_lower_cut_off[n] = relative_cut_off[n] * (audio.FFTtreblebufferSize / 2);
      first_treble_bar++;
      if (first_treble_bar == 1) {
        first_bar = true;
        FFTbuffer_upper_cut_off[n - 1] = relative_cut_off[n] * (audio.FFTmidbufferSize / 2);
      } else {
        first_bar = false;
      }

      eq[n] *= log2(audio.FFTtreblebufferSize);
    }

    if (n > 0) {
      if (!first_bar) {
        FFTbuffer_upper_cut_off[n - 1] = FFTbuffer_lower_cut_off[n] - 1;

        // pushing the spectrum up if the exponential function gets "clumped" in the
        // bass and caluclating new cut off frequencies
        if (FFTbuffer_lower_cut_off[n] <= FFTbuffer_lower_cut_off[n - 1]) {

          FFTbuffer_lower_cut_off[n] = FFTbuffer_lower_cut_off[n - 1] + 1;
          FFTbuffer_upper_cut_off[n - 1] = FFTbuffer_lower_cut_off[n] - 1;

          if (bar_buffer[n] == 1)
            relative_cut_off[n] =
                (float)(FFTbuffer_lower_cut_off[n]) / ((float)audio.FFTbassbufferSize / 2);
          else if (bar_buffer[n] == 2)
            relative_cut_off[n] =
                (float)(FFTbuffer_lower_cut_off[n]) / ((float)audio.FFTmidbufferSize / 2);
          else if (bar_buffer[n] == 3)
            relative_cut_off[n] =
                (float)(FFTbuffer_lower_cut_off[n]) / ((float)audio.FFTtreblebufferSize / 2);

          cut_off_frequency[n] = relative_cut_off[n] * ((float)audio.rate / 2);
        }
      } else {
        if (FFTbuffer_upper_cut_off[n - 1] <= FFTbuffer_lower_cut_off[n - 1])
          FFTbuffer_upper_cut_off[n - 1] = FFTbuffer_lower_cut_off[n - 1] + 1;
      }
      upper_cut_off_frequency[n - 1] =
          cut_off_frequency[n]; // high_relative_cut_off * ((float)audio.rate / 2);
      center_frequencies[n - 1] =
          pow((cut_off_frequency[n - 1] * upper_cut_off_frequency[n - 1]), 0.5);
    }
  }

  // process: execute FFT and sort frequency bands
  pthread_mutex_lock(&audio.lock);
  fftw_execute(p_bass_l);
  fftw_execute(p_mid_l);
  fftw_execute(p_treble_l);
  pthread_mutex_unlock(&audio.lock);

  // process: separate frequency bands
  for (int n = 0; n < number_of_bars; n++) {

    temp_l[n] = 0;

    // process: add upp FFT values within bands
    for (int i = FFTbuffer_lower_cut_off[n]; i <= FFTbuffer_upper_cut_off[n]; i++) {
      if (n <= bass_cut_off_bar) {

        temp_l[n] += hypot(out_bass_l[i][0], out_bass_l[i][1]);

      } else if (n > bass_cut_off_bar && n <= treble_cut_off_bar) {

        temp_l[n] += hypot(out_mid_l[i][0], out_mid_l[i][1]);

      } else if (n > treble_cut_off_bar) {

        temp_l[n] += hypot(out_treble_l[i][0], out_treble_l[i][1]);
      }
    }

    // getting average multiply with sens and eq
    temp_l[n] /= FFTbuffer_upper_cut_off[n] - FFTbuffer_lower_cut_off[n] + 1;
    temp_l[n] *= conf.sens * eq[n];

    bars_left[n] = temp_l[n];
  }

  // process [filter]

  // if (p.monstercat) {
  //   if (p.stereo) {
  //     bars_left = monstercat_filter(bars_left, number_of_bars / 2, p.waves, p.monstercat);
  //     bars_right = monstercat_filter(bars_right, number_of_bars / 2, p.waves, p.monstercat);
  //   } else {
  //     bars_left = monstercat_filter(bars_left, number_of_bars, p.waves, p.monstercat);
  //   }
  // }

  // processing signal

  // bool senselow = true;

  for (int n = 0; n < number_of_bars; n++) {
    //   // mirroring stereo channels
    //   if (p.stereo) {
    //     if (n < number_of_bars / 2) {
    //       bars[n] = bars_left[number_of_bars / 2 - n - 1];
    //     } else {
    //       bars[n] = bars_right[n - number_of_bars / 2];
    //     }

    //   } else {
    bars[n] = bars_left[n];
  }

  // process [smoothing]: falloff
  // if (g > 0) {
  //   if (bars[n] < bars_last[n]) {
  //     bars[n] = bars_peak[n] - (g * fall[n] * fall[n]);
  //     if (bars[n] < 0)
  //       bars[n] = 0;
  //     fall[n]++;
  //   } else {
  //     bars_peak[n] = bars[n];
  //     fall[n] = 0;
  //   }

  //   bars_last[n] = bars[n];
  // }

  // process [smoothing]: integral
  //   if (p.integral > 0) {
  //     bars[n] = bars_mem[n] * integral + bars[n];
  //     bars_mem[n] = bars[n];

  //     int diff = height - bars[n];
  //     if (diff < 0)
  //       diff = 0;
  //     double div = 1 / (diff + 1);
  //     // bars[n] = bars[n] - pow(div, 10) * (height + 1);
  //     bars_mem[n] = bars_mem[n] * (1 - div / 20);
  //   }

  memcpy(previous_frame, bars, 256 * sizeof(int));

  for (size_t i = 0; i < number_of_bars; i++) {
    printf("%5d, ", bars[i]);
  }
  printf("\n");
}

int *spectrum_display_bars() { return bars_left; }

void init_plan(int bufferSize, double **in, double **in_raw, fftw_complex **out, fftw_plan *p) {
  *in = fftw_alloc_real(bufferSize);
  *in_raw = fftw_alloc_real(bufferSize);

  *out = fftw_alloc_complex(bufferSize / 2 + 1);
  memset(*out, 0, (bufferSize / 2 + 1) * sizeof(fftw_complex));

  *p = fftw_plan_dft_r2c_1d(bufferSize, *in, *out, FFTW_MEASURE);
}

void compute_param() {}

void compute_config() {}