/*
 *  Copyright 1994-2022 Olivier Girondel
 *
 *  This file is part of lebiniou.
 *
 *  lebiniou is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  lebiniou is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with lebiniou. If not, see <http://www.gnu.org/licenses/>.
 */

#include "biniou.h"
#include "pthread_utils.h"

// All input plugins are stereo
// #define WITH_MONO_INPUT

float phase = 1.0;

#define HISTORY_SIZE  48000  /* ~1s of data */
#define PEAK_ABOVE_AVG 0.5   /* ratio how many (avg-max) over avg to be considered peak */
#define PEAK_DURATION 100    /* if 'peak' is this near previous, it is considered same peak continued */
#define PEAK_MAX_COUNT 1000  /* After detecting given amount of peaks give up */

/* We want treshold where we have roughly 4 peaks / second.
   There is no best value, but 4 peaks/second is reasonable compromise
   before rewriting this system properly. */
#define PEAKS_PER_SECOND 4   /* how many peaks we try to find */
#define PEAK_COUNT_ON_HISTORY (HISTORY_SIZE / PEAKS_PER_SECOND)

/* history ring buffer for peak detection */
static double history_buff[3][HISTORY_SIZE] = { { 0 } };
static int history_wr = 0; /* write index of history buffer */
static int history_rd = 0; /* read index of history buffer */


static inline void
history_add(double mono, double left, double right)
{
  history_buff[A_MONO][history_wr] = mono;
  history_buff[A_LEFT][history_wr] = left;
  history_buff[A_RIGHT][history_wr] = right;
  history_wr++;
  if (history_wr >= HISTORY_SIZE) {
    history_wr = 0;
  }
  if (history_wr == history_rd) {
    history_rd++;
    if (history_rd >= HISTORY_SIZE) {
      history_rd = 0;
    }
  }
}


inline double
Input_clamp(const double val)
{
  if (val < -1.0) {
    return -1.0;
  } else if (val > 1.0) {
    return 1.0;
  } else {
    return val;
  }
}


Input_t *
Input_new(const uint32_t size)
{
  if (size < 64) {
    xerror("%s: Input size should be >= 64 (wanted %d)\n", __func__, size);
  }

  Input_t *input = xcalloc(1, sizeof(Input_t));
  uint8_t c;

  xpthread_mutex_init(&input->mutex, NULL);

  input->size = size;
  input->size2 = 0;
  input->mute = 0;
  input->spectrum_size = (input->size / 2) + 1;

  VERBOSE(printf("[i] %s: data size= %d, power spectrum size= %d\n",
                 __func__, input->size, input->spectrum_size));

  for (c = 0; c < 3; c++) {
    uint16_t i;

    input->data[c]   = fftw_alloc_real(input->size);
    for (i = 0; i < input->size; i++) {
      input->data[c][i] = 0;
    }
    input->data2[c]  = xcalloc(HISTORY_SIZE, sizeof(double));
    input->data_u[c] = xcalloc(input->size, sizeof(double));
    input->out[c]    = fftw_alloc_complex(input->spectrum_size);
    for (i = 0; i < input->spectrum_size; i++) {
      input->out[c][i] = 0;
    }
    input->plan_fft[c] = fftw_plan_dft_r2c_1d(input->size,
                         input->data[c],
                         input->out[c],
                         FFTW_MEASURE);

    input->spectrum[c]     = xcalloc(input->spectrum_size, sizeof(double));
    input->spectrum_log[c] = xcalloc(input->spectrum_size, sizeof(double));
  }

#ifdef DEBUG
  input->timer = Timer_new("input");
#endif

  input->volume_scale = 1.0;

  return input;
}


void
Input_delete(Input_t *input)
{
  for (int c = 0; c < 3; c++) {
    fftw_free(input->data[c]);
    xfree(input->data2[c]);
    xfree(input->data_u[c]);
    fftw_free(input->out[c]);
    xfree(input->spectrum[c]);
    xfree(input->spectrum_log[c]);
    fftw_destroy_plan(input->plan_fft[c]);
  }

#ifdef DEBUG
  Timer_delete(input->timer);
#endif

  xpthread_mutex_destroy(&input->mutex);
  xfree(input);
  fftw_cleanup();
}


static int
Input_seek_max_spectrum(Input_t *input, int c)
{
  uint32_t i;
  uint16_t new_max = 0;

  /* Input_reset_max_spectrum(input); */
  input->max_spectrum[c] = -1.0;

  /* start at 1 to avoid power spectrum value at index 0 */
  for (i = 1; i < input->spectrum_size; i++)
    if (input->spectrum[c][i] > input->max_spectrum[c]) {
      input->max_spectrum[c] = input->spectrum[c][i];
      new_max = i;
    }

  return new_max;
}


static void
Input_do_fft(Input_t *input)
{
  /* const int N = input->size; */
  /* const int even = (N % 2 == 0); */
  int c;
  uint32_t k;

#ifdef DEBUG
  Timer_start(input->timer);
#endif

  for (c = 0; c < 3; c++) {
    fftw_execute(input->plan_fft[c]);
  }

  for (c = 0; c < 3; c++) {
    for (k = 0; k < input->spectrum_size; k++) {
      input->out[c][k] /= input->size;
      // normalize
      input->spectrum[c][k] = cabs(input->out[c][k]) * M_SQRT1_2; // == / sqrtf(2.0);
    }
  }

  for (c = 0; c < 3; c++) {
    int new_max = Input_seek_max_spectrum(input, c);

    for (k = 0; k < input->spectrum_size; k++) {
      /* log1p(x)=logf(x+1) */
      input->spectrum_log[c][k] = log1p(input->spectrum[c][k]) / (M_LN2 / M_LN10);
    }

    input->max_spectrum_log[c] = input->spectrum_log[c][new_max];

#ifdef DEBUG_INPUT_SPECTRUM
    if (c == A_MONO) {
      printf("[s] %d % 3d\tspectrum: %f\tspectrum_log: %f\n",
             c, new_max,
             input->max_spectrum[c],
             input->max_spectrum_log[c]);
    }
#endif
  }

#ifdef DEBUG_INPUT_FFTW
  printf("[i] FFT time: %f ms\n", Timer_elapsed(input->timer) * 1000);
#endif
}


void
Input_set(Input_t *input, uint8_t mode)
{
  /* mode:
   * A_MONO   => copy A_MONO to A_LEFT and A_RIGHT
   * A_STEREO => compute A_MONO as the average of A_LEFT and A_RIGHT
   */
#ifdef WITH_MONO_INPUT
  if (mode == A_MONO) {
    for (uint32_t i = 0; i < input->size; i++) {
      /* set unsigned value */
      input->data_u[A_MONO][i] = fabs(Input_clamp(input->data[A_MONO][i]));

      /* scale value */
      input->data[A_MONO][i] *= input->volume_scale;

      /* copy to A_LEFT and A_RIGHT */
      input->data[A_LEFT][i] = input->data[A_RIGHT][i] = input->data[A_MONO][i];
      input->data_u[A_LEFT][i] = input->data_u[A_RIGHT][i] = input->data_u[A_MONO][i];
      history_add(input->data[A_MONO][i], input->data[A_LEFT][i], input->data[A_RIGHT][i]);
    }
  } else {
#endif
    assert(mode == A_STEREO);
    for (uint32_t i = 0; i < input->size; i++) {
      /* set unsigned values */
      input->data_u[A_LEFT][i] = fabs(Input_clamp(input->data[A_LEFT][i]));
      input->data_u[A_RIGHT][i] = fabs(Input_clamp(input->data[A_RIGHT][i]));

      /* scale values */
      input->data[A_LEFT][i] *= input->volume_scale;
      input->data[A_RIGHT][i] *= input->volume_scale;

      /* compute A_MONO from A_LEFT and A_RIGHT */
      input->data[A_MONO][i] = (input->data[A_LEFT][i] + (phase * input->data[A_RIGHT][i])) / 2;
      input->data_u[A_MONO][i] = (input->data_u[A_LEFT][i] + input->data_u[A_RIGHT][i]) / 2;
      history_add(input->data[A_MONO][i], input->data[A_LEFT][i], input->data[A_RIGHT][i]);
    }
#ifdef WITH_MONO_INPUT
  }
#endif

  Input_do_fft(input);
}


static inline void
do_roulette(Input_t *input)
{
  INC(input->roulette, input->size);
}


inline float
Input_random_s_u_float(Input_t *input)
{
  /* random float [-1..+1] */
  float f = input->data[A_MONO][input->roulette];
  do_roulette(input);
  return f;
}


inline float
Input_random_u_u_float(Input_t *input)
{
  /* random float [0..1] */
  float f = input->data_u[A_MONO][input->roulette];
  do_roulette(input);
  return f;
}


inline float
Input_random_float_range(Input_t *input, const float min, const float max)
{
#ifdef DEBUG
  if (max <= min) {
    xerror("Input_random_short_range: max %f <= min %f\n", max, min);
  }
#endif
  /* random short */
  float rnd = input->data_u[A_MONO][input->roulette];
  float f = min + rnd * (max - min);
  do_roulette(input);
  return f;
}


inline short
Input_random_short_range(Input_t *input, const short min, const short max)
{
  /* random short */
  short s;
  float rnd = input->data_u[A_MONO][input->roulette];
#ifdef DEBUG
  if (max <= min) {
    xerror("Input_random_short_range: max %d <= min %d\n", max, min);
  }
#endif
  s = min + rnd * (max - min);
  do_roulette(input);
  return s;
}


inline Pixel_t
Input_random_color(Input_t *input)
{
  float col = (PIXEL_MAXVAL >> 1) * (1 + input->data[A_MONO][input->roulette]);
  do_roulette(input);
  return (col < PIXEL_MINVAL) ? PIXEL_MINVAL : (col > PIXEL_MAXVAL) ? PIXEL_MAXVAL : (Pixel_t)col;
}


inline float
Input_get_volume(Input_t *input)
{
  return input->volume;
}


void
Input_process(Input_t *input)
{
  uint32_t i;
  uint32_t j;
  uint32_t count;

  double tmp = 0.0;
  double sum = 0.0;
  double sqsum = 0.0;
  uint32_t peaks[2][PEAK_MAX_COUNT] = { { 0 } };
  double peak_val[PEAK_MAX_COUNT] = { 0 } ;
  double limit;

  assert(NULL != input);

  if (!xpthread_mutex_lock(&input->mutex)) {
    /* run through history to get history averages and max */
    input->maxpeak = 0.0;
    sum = 0.0;
    for (i = 0; i < HISTORY_SIZE; i++) {
      tmp = fabs(history_buff[A_MONO][i]);
      if (tmp > input->maxpeak) {
        input->maxpeak = tmp;
      }
      sum += tmp;
    }
    input->average = sum / HISTORY_SIZE;

    /* one more loop on whole history, to collect peaks we have */
    j = 0;
    tmp = -1.0;
    for (i = 0; i < HISTORY_SIZE; i++) {
      sum = fabs(history_buff[A_MONO][i]);
      if (sum > (input->average + (input->maxpeak - input->average) * PEAK_ABOVE_AVG)) {
        if (tmp < 0.0) { /* new peak */
          /* if we are very near previous peak, continue with it */
          if ((j > 0) && (i < (peaks[1][j-1] + PEAK_DURATION))) {
            tmp = (peak_val[j-1] > sum) ? peak_val[j-1] : sum;
            peaks[1][j-1] = i;
          } else { /* really new peak */
            peaks[0][j] = i;
            tmp = sum;
            j++;
            if (j >= PEAK_MAX_COUNT) {
              xdebug("**** Got more than %d peaks\n", PEAK_MAX_COUNT);
              break;
            }
          }
        } else { /* still in same peak */
          if (sum > tmp) {
            tmp = sum;
          }
          peaks[1][j-1] = i;
        }
      } else { /* not in peak */
        if (tmp > 0.0) { /* first non-peak value after peak */
          peak_val[j-1] = tmp;
          tmp = -1.0;
        }
      }
    }
    /* now loop the found peaks, and try to find suitable limit */
    count = 0;
    for (limit = 0.99;
         count < PEAK_COUNT_ON_HISTORY && limit > PEAK_ABOVE_AVG;
         limit -= 0.01) {
      count = 0;
      for (i = 0; i < j; i++) {
        if (peak_val[i] >
            (input->average + (input->maxpeak - input->average) * limit)) {
          count++;
        }
      }
    }

    /* copy data for plugin usage from ring buffer and get peaks */
    input->size2 = 0;
    input->curpeak = 0.0;
    sum = 0.0;
    sqsum = 0.0;
    while (history_rd != history_wr) {
      input->data2[A_MONO][input->size2] = history_buff[A_MONO][history_rd];
      input->data2[A_LEFT][input->size2] = history_buff[A_LEFT][history_rd];
      input->data2[A_RIGHT][input->size2] = history_buff[A_RIGHT][history_rd];
      tmp = fabs(history_buff[A_MONO][history_rd]);
      sum += tmp;
      sqsum += 100 * tmp * tmp;
      if (tmp > input->curpeak) {
        input->curpeak = tmp;
      }
      history_rd++;
      input->size2++;
      if (history_rd >= HISTORY_SIZE) {
        history_rd = 0;
      }
    }
    input->volume = sum / input->size2;
    input->rms = sqrt(sqsum/input->size2);
    if (input->curpeak >
        (input->average + (input->maxpeak - input->average) * limit)) {
      input->on_beat = 1;
    } else {
      input->on_beat = 0;
    }
    input->peakpower = input->curpeak*input->curpeak;
    xpthread_mutex_unlock(&input->mutex);
  }
}


uint8_t
Input_toggle_mute(Input_t *input)
{
  input->mute = !input->mute;

  return input->mute;
}


void
Input_volume_upscale(Input_t *input)
{
  input->volume_scale += VOLUME_SCALE_STEP;
#ifdef DEBUG
  VERBOSE(printf("[i] Volume scale: %.1f\n", input->volume_scale));
#endif
}


void
Input_volume_downscale(Input_t *input)
{
  if (input->volume_scale >= (2 * VOLUME_SCALE_STEP)) {
    input->volume_scale -= VOLUME_SCALE_STEP;
#ifdef DEBUG
    VERBOSE(printf("[i] Volume scale: %.1f\n", input->volume_scale));
#endif
  }
}


void
Input_set_volume_scale(Input_t *input, const double vs)
{
  input->volume_scale = vs;
#ifdef DEBUG
  VERBOSE(printf("[i] Volume scale: %.1f\n", input->volume_scale));
#endif
}


double
compute_avg_abs(double *buf, uint32_t a, uint32_t b)
{
  if (b < a) {
    return 0.0;
  }

  double sum = 0.0;
  for (uint32_t n = a; n < b; n++) {
    sum += fabs(buf[n]);
  }

  return sum / (b - a + 1);
}


uint16_t
compute_avg_freq_id(Input_t *input, double spectrum_low_treshold_factor)
{
  assert(NULL != input);

  /* find the frequency having the highest value of the spectrum */
  double spectrum_max = 0;
  for (uint16_t i = 0; i < input->spectrum_size; i++)
    if (input->spectrum_log[A_MONO][i] > spectrum_max) {
      spectrum_max = input->spectrum_log[A_MONO][i];
    }

  double spectrum_low_treshold = spectrum_max * spectrum_low_treshold_factor;

  /* find average frequency, based on frequencies higher than spectrum_low_treshold */
  double average_freq_id = 0;
  double spectrum_sum = 0;
  for (uint16_t i = 0; i < input->spectrum_size; i++) {
    if (input->spectrum_log[A_MONO][i] > spectrum_low_treshold) {
      spectrum_sum += input->spectrum_log[A_MONO][i];
      average_freq_id += (double)(i+1) * input->spectrum_log[A_MONO][i];
    }
  }
  /* avoid 0 division and compute average frequency id */
  if (spectrum_sum == 0) {
    average_freq_id = 0;
  } else {
    average_freq_id /= spectrum_sum ;
    average_freq_id = round(average_freq_id);
  }

  return (uint16_t)average_freq_id;
}
