#include "Amuencha.hpp"
#include "sse_mathfun.h"

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/bessel.hpp>

#include <sys/stat.h>
#include <iostream>
#include <fstream>

namespace Amuencha
{
void Analyser::prepare(halp::setup info)
{
  sampling_rate = info.rate;
  samplerate_div_2pi = sampling_rate/two_pi;
  analyzer_setup();
}

void Analyser::operator()(halp::tick t)
{
  // Process the input buffer
  auto* in = inputs.audio[0];

  for(int j{0}; j < t.frames; j++)
  {

  }
}

void Analyser::analyzer_setup(float max_buffer_duration)
{
  using namespace boost::math::float_constants;
  using namespace std;

  // Prepare the windows
  //std::vector<std::vector<v4sf>> windowed_sines;
  windowed_sines.resize(frequencies.size());
  power_normalization_factors.resize(frequencies.size());

  int big_buffer_size = 0;

  for (int idx{0}; idx < frequencies.size(); ++idx)
  {
    // for each freq, span at least 20 periods for more precise measurements
    // This still gives reasonable latencies, e.g. 50ms at 400Hz, 100ms at 200Hz, 400ms at 50Hz...
    // Could also span more for even better measurements, with larger
    // computation cost and latency
    float f = frequencies[idx];
    int window_size = (int)(min(inputs.periods.value / f, max_buffer_duration * 0.001f)
                            * sampling_rate);
    vector<float> window(window_size);
    vector<float> window_deriv(window_size);

    if (!read_from_cache(window, window_deriv))
    {
      initialize_window(window);
      initialize_window_deriv(window_deriv);
      write_to_cache(window, window_deriv);
    }
    windowed_sines[idx].resize(window_size);
    float wsum = 0;

    for (int i{0}; i < window_size;)
    {
      if (i < window_size - 4)
      {
        v4sf tfs = {
            (float)(i - window_size - 1) / sampling_rate,
            (float)(i + 1 - window_size - 1) / sampling_rate,
            (float)(i + 2 - window_size - 1) / sampling_rate,
            (float)(i + 3 - window_size - 1) / sampling_rate
        };
        tfs *= (float)(-two_pi*f);
        v4sf sin_tf, cos_tf;
        sincos_ps(tfs, &sin_tf, &cos_tf);

        for (int j{0}; j < 3; ++j)
        {
          v4sf ws = {
              cos_tf[j] * window[i + j],
              sin_tf[j] * window[i + j],
              cos_tf[j] * window_deriv[i + j],
              sin_tf[j] * window_deriv[i + j]
          };
          windowed_sines[idx][i + j] = ws;
          wsum += window[i + j];
        }
        i += 4;
        continue;
      }
      float t = (float)(i - window_size - 1) / sampling_rate;
      float re = cosf(-two_pi * t * f);
      float im = sinf(-two_pi * t * f);
      v4sf ws = {
          re * window[i],
          im * window[i],
          re * window_deriv[i],
          im * window_deriv[i]
      };
      windowed_sines[idx][i] = ws;
      wsum += window[i];
      ++i;
    }
    power_normalization_factors[idx] = 1. / (wsum * wsum);
    big_buffer_size = max(big_buffer_size, window_size);
  }

  big_buffer.clear();
  // fill with 0 signal content to start with
  big_buffer.resize(big_buffer_size, 0.f);
}

void Analyser::initialize_window(std::vector<float>& window)
{
  // Kaiser window with a parameter of alpha=3 that nullifies the window on edges
  int size = window.size();
  const float two_over_N = 2. / size;
  const float alpha = 3.;
  const float alpha_pi = alpha * pi;
  const float inv_denom = 1. / boost::math::cyl_bessel_i(0., alpha_pi);
  for (int i{0}; i < size; ++i)
  {
    float p = i * two_over_N - 1.;
    window[i] = boost::math::cyl_bessel_i(0., alpha_pi * sqrt(1. - p * p)) * inv_denom;
  }
}

void Analyser::initialize_window_deriv(std::vector<float>& window)
{
  // Derivative of the Kaiser window with a parameter of alpha=3 that nullifies the window on edges
  int size = window.size();
  const float two_over_N = 2. / size;
  const float alpha = 3.;
  const float alpha_pi = alpha * pi;
  const float inv_denom = 1. / boost::math::cyl_bessel_i(0., alpha_pi);
  for (int i{1}; i < size; ++i)
  {
    float p = i * two_over_N - 1.;
    window[i] = boost::math::cyl_bessel_i(1., alpha_pi * sqrt(1. - p * p)) *
                inv_denom * alpha_pi / sqrt(1. - p * p) * (-p) * two_over_N;
  }
  // lim I1(x)/x as x->0  =  1/2
  window[0] = 0.5 * inv_denom * alpha_pi * alpha_pi * two_over_N;
}

bool Analyser::read_from_cache(std::vector<float> &window, std::vector<float> &window_deriv)
{
  auto it = mem_win_cache.find(window.size());

  if (it != mem_win_cache.end())
  {
    window = it->second;
    auto itd = mem_winderiv_cache.find(window.size());

    if (itd != mem_winderiv_cache.end())
    {
      window_deriv = itd->second;
      return true;
    }
    // else, load from disk
  }

  using namespace std;
  // TODO: make the cache location parametrizable (and an option to not use it)
  ifstream file(".amuencha_cache/w"+to_string(window.size())+".bin", ios::binary);
  if (file.is_open())
  {
    file.read(reinterpret_cast<char*>(&window[0]), window.size()*sizeof(float));
    file.read(reinterpret_cast<char*>(&window_deriv[0]), window_deriv.size()*sizeof(float));

    if (file.tellg() != (window.size() + window_deriv.size()) * sizeof(float))
    {
      cerr << "Error: invalid cache .amuencha_cache/w"+to_string(window.size())+".bin\n";
      return false;
    }
    return true;
  }
  return false;
}

void Analyser::write_to_cache(std::vector<float> &window, std::vector<float> &window_deriv)
{
  using namespace std;
#if defined(_WIN32) || defined(_WIN64)
  mkdir(".amuencha_cache");
#else
  mkdir(".amuencha_cache", 0755);
#endif
  ofstream file(".amuencha_cache/w"+to_string(window.size())+".bin", ios::binary|ios::trunc);
  file.write(reinterpret_cast<char*>(&window[0]), window.size()*sizeof(float));
  file.write(reinterpret_cast<char*>(&window_deriv[0]), window_deriv.size()*sizeof(float));
  mem_win_cache[window.size()] = window;
  mem_winderiv_cache[window.size()] = window_deriv;
}
}