/**********************************************************************

  Audacity: A Digital Audio Editor

  Reverb_libSoX.h
  Stereo reverberation effect from libSoX,
  adapted for Audacity

  Copyright (c) 2007-2013 robs@users.sourceforge.net
  Licence: LGPL v2.1
  Filter configuration based on freeverb by Jezar Wakefield.

**********************************************************************/

#include <cstring>
#include <cstdlib>
#ifdef __WXMSW__
   #define M_LN10   2.30258509299404568402 /* log_e 10 */
#else
   #include <cmath>
#endif
#include <algorithm>
using std::min;
using std::max;

#define array_length(a) (sizeof(a)/sizeof(a[0]))
#define dB_to_linear(x) exp((x) * M_LN10 * 0.05)
#define midi_to_freq(n) (440 * pow(2,((n)-69)/12.))
#define FIFO_SIZE_T size_t
#define FIFO_MIN 0x4000
#define fifo_read_ptr(f) fifo_read(f, (FIFO_SIZE_T)0, NULL)
#define lsx_zalloc(var, n) var = (float *)calloc(n, sizeof(*var))
#define filter_advance(p) if (--(p)->ptr < (p)->buffer) (p)->ptr += (p)->size
#define filter_delete(p) free((p)->buffer)

typedef struct {
   char * data;
   size_t allocation;   /* Number of bytes allocated for data. */
   size_t item_size;    /* Size of each item in data */
   size_t begin;        /* Offset of the first byte to read. */
   size_t end;          /* 1 + Offset of the last byte to read. */
} fifo_t;

static void fifo_clear(fifo_t * f)
{
   f->end = f->begin = 0;
}

static void fifo_clear_and_zero(fifo_t* f)
{
   f->end = f->begin = 0;
   memset(f->data, 0, f->allocation);
}


static void * fifo_reserve(fifo_t * f, FIFO_SIZE_T n)
{
   n *= f->item_size;

   if (f->begin == f->end)
      fifo_clear(f);

   while (1) {
      if (f->end + n <= f->allocation) {
         void *p = f->data + f->end;

         f->end += n;
         return p;
      }
      if (f->begin > FIFO_MIN) {
         memmove(f->data, f->data + f->begin, f->end - f->begin);
         f->end -= f->begin;
         f->begin = 0;
         continue;
      }
      f->allocation += n;
      f->data = (char *)realloc(f->data, f->allocation);
   }
}

static void * fifo_write(fifo_t * f, FIFO_SIZE_T n, void const * data)
{
   void * s = fifo_reserve(f, n);
   if (data)
      memcpy(s, data, n * f->item_size);
   return s;
}

static void * fifo_read(fifo_t * f, FIFO_SIZE_T n, void * data)
{
   char * ret = f->data + f->begin;
   n *= f->item_size;
   if (n > (FIFO_SIZE_T)(f->end - f->begin))
      return NULL;
   if (data)
      memcpy(data, ret, (size_t)n);
   f->begin += n;
   return ret;
}

static void fifo_delete(fifo_t * f)
{
   free(f->data);
}

static void fifo_create(fifo_t * f, FIFO_SIZE_T item_size)
{
   f->item_size = item_size;
   f->allocation = FIFO_MIN;
   f->data = (char *)malloc(f->allocation);
   fifo_clear(f);
}

typedef struct {
   size_t  size;
   float   * buffer, * ptr;
   float   store;
} filter_t;

static float comb_process(filter_t * p,  /* gcc -O2 will inline this */
      float const * input, float const * feedback, float const * hf_damping)
{
   float output = *p->ptr;
   p->store = output + (p->store - output) * *hf_damping;
   *p->ptr = *input + p->store * *feedback;
   filter_advance(p);
   return output;
}

static float allpass_process(filter_t * p,  /* gcc -O2 will inline this */
      float const * input)
{
   float output = *p->ptr;
   *p->ptr = *input + output * .5;
   filter_advance(p);
   return output - *input;
}

typedef struct {double b0, b1, a1, i1, o1;} one_pole_t;

static float one_pole_process(one_pole_t * p, float i0)
{
   float o0 = i0*p->b0 + p->i1*p->b1 - p->o1*p->a1;
   p->i1 = i0;
   return p->o1 = o0;
}

static const size_t /* Filter delay lengths in samples (44100Hz sample-rate) */
   comb_lengths[] = {1116, 1188, 1277, 1356, 1422, 1491, 1557, 1617},
   allpass_lengths[] = {225, 341, 441, 556}, stereo_adjust = 12;

typedef struct {
   filter_t comb   [array_length(comb_lengths)];
   filter_t allpass[array_length(allpass_lengths)];
   one_pole_t one_pole[2];
} filter_array_t;

static void one_pole_init(filter_array_t* p, double rate,
    double fc_highpass, double fc_lowpass)
{
   { /* EQ: highpass */
      one_pole_t* q = &p->one_pole[0];
      q->a1 = -exp(-2 * M_PI * fc_highpass / rate);
      q->b0 = (1 - q->a1) / 2, q->b1 = -q->b0;
   }
   { /* EQ: lowpass */
      one_pole_t* q = &p->one_pole[1];
      q->a1 = -exp(-2 * M_PI * fc_lowpass / rate);
      q->b0 = 1 + q->a1, q->b1 = 0;
   }
}


static void filter_array_allocate(filter_array_t* p, double rate,
   double scale, double offset)
{
   size_t i;
   double r = rate * (1 / 44100.); /* Compensate for actual sample-rate */

   for (i = 0; i < array_length(comb_lengths); ++i)
   {
      filter_t* pcomb = &p->comb[i];
      pcomb->size = (size_t)(scale * r * (comb_lengths[i] + stereo_adjust * offset) + .5);
      pcomb->ptr = lsx_zalloc(pcomb->buffer, pcomb->size);
   }
   for (i = 0; i < array_length(allpass_lengths); ++i)
   {
      filter_t* pallpass = &p->allpass[i];
      pallpass->size = (size_t)(r * (allpass_lengths[i] + stereo_adjust * offset) + .5);
      pallpass->ptr = lsx_zalloc(pallpass->buffer, pallpass->size);
   }
}

static void filter_t_resize(filter_t* p, size_t newSize);

static void filter_array_init(filter_array_t* p, double rate,
   double scale, double offset)
{
   size_t i;
   double r = rate * (1 / 44100.); /* Compensate for actual sample-rate */

   for (i = 0; i < array_length(comb_lengths); ++i, offset = -offset)
   {
      filter_t* pcomb = &p->comb[i];
      size_t newSize = (size_t)(scale * r * (comb_lengths[i] + stereo_adjust * offset) + .5);
      filter_t_resize(pcomb, newSize);
   }
   for (i = 0; i < array_length(allpass_lengths); ++i, offset = -offset)
   {
      filter_t* pallpass = &p->allpass[i];
      size_t newSize = (size_t)(r * (allpass_lengths[i] + stereo_adjust * offset) + .5);
      filter_t_resize(pallpass, newSize);
   }
}



static void filter_array_create(filter_array_t * p, double rate,
      double scale, double offset)
{
   size_t i;
   double r = rate * (1 / 44100.); /* Compensate for actual sample-rate */

   for (i = 0; i < array_length(comb_lengths); ++i, offset = -offset)
   {
      filter_t * pcomb = &p->comb[i];
      pcomb->size = (size_t)(scale * r * (comb_lengths[i] + stereo_adjust * offset) + .5);
      pcomb->ptr = lsx_zalloc(pcomb->buffer, pcomb->size);
   }
   for (i = 0; i < array_length(allpass_lengths); ++i, offset = -offset)
   {
      filter_t * pallpass = &p->allpass[i];
      pallpass->size = (size_t)(r * (allpass_lengths[i] + stereo_adjust * offset) + .5);
      pallpass->ptr = lsx_zalloc(pallpass->buffer, pallpass->size);
   }
}

static void filter_array_process(filter_array_t * p,
      size_t length, float const * input, float * output,
      float const * feedback, float const * hf_damping, float const * gain)
{
   while (length--) {
      float out = 0, in = *input++;

      size_t i = array_length(comb_lengths) - 1;
      do out += comb_process(p->comb + i, &in, feedback, hf_damping);
      while (i--);

      i = array_length(allpass_lengths) - 1;
      do out = allpass_process(p->allpass + i, &out);
      while (i--);

      out = one_pole_process(&p->one_pole[0], out);
      out = one_pole_process(&p->one_pole[1], out);
      *output++ = out * *gain;
   }
}

static void filter_array_delete(filter_array_t * p)
{
   size_t i;

   for (i = 0; i < array_length(allpass_lengths); ++i)
      filter_delete(&p->allpass[i]);
   for (i = 0; i < array_length(comb_lengths); ++i)
      filter_delete(&p->comb[i]);
}

typedef struct {
   float feedback;
   float hf_damping;
   float gain;
   fifo_t input_fifo;
   filter_array_t chan[2];
   float * out[2];
   bool initializedWithZeroDepth;
} reverb_t;


// Resizes a filter, resetting its contents
static void filter_t_resize_resetting(filter_t* p, size_t newSize)
{
   memset(p->buffer, 0, newSize * sizeof(float));
   p->ptr = p->buffer;
   p->store = 0;
   p->size = newSize;
}


// Resizes a filter, trying to keep as much of its history as possible.
static void filter_t_resize_preserving(filter_t* p, size_t newSize)
{
   // Imagine we have this filter_t as input:
   //
   //                  ptr goes from right to left and when falling off the left side, it wraps back to the right;
   //    <--ptr--      it reads the most distant sample in the past, and after reading it will write a new sample.
   //        |         Imagine someone just recorded "A B C D E", and now we are going to read "A", but a resize request comes.
   //        v         
   //  | C B A E D |
   //
   
   // Depending on the new requested size and where ptr is, we can have these cases:
   // 
   //        v
   //  | C B A 0 E D |  bigger size: right-shift what is right of ptr,
   //                   and fill the resulting gap with zeros.
   //
   //                   (with variant: ptr is already at the old right edge - then shift nothing, just append zeros)
   //        v
   //  | C B A D |      smaller size, and ptr is within it (but not at the edge): left-shift what is to the right of ptr,
   //                   discarding samples.
   // 
   //        V
   //  | C B A |        smaller size, and ptr would be at the new right edge; do nothing.
   // 
   //      v
   //  | B A |          smaller size, and ptr would be beyond the new right edge: left-shift what is ahead of ptr,
   //                   AND move ptr to size-1

   assert(newSize > 0);

   const int    sizeDiff = ((ssize_t)newSize - (ssize_t)p->size);
   const size_t ptrPos   = (p->ptr  - p->buffer);

   const size_t numSamplesBehindPtr = (p->size - 1) - ptrPos;

   if (sizeDiff > 0)
   {
      // case: bigger size

      if (numSamplesBehindPtr > 0)
      {
         // right-shift what is right of ptr
         memcpy(p->ptr + 1 + sizeDiff, p->ptr + 1, numSamplesBehindPtr * sizeof(float));
      }      

      // fill the created gap with zeros
      memset(p->ptr + 1, 0, sizeDiff * sizeof(float));
   }
   else if (sizeDiff < 0)
   {
      // case: smaller size
            
      if (ptrPos < newSize-1)
      {
         size_t lenOfBlockToShift = newSize - 1 - ptrPos;
         float* ptrToBlockToShift = p->buffer + p->size - lenOfBlockToShift;
         memcpy(p->ptr + 1, ptrToBlockToShift, lenOfBlockToShift * sizeof(float));         
      }
      else if (ptrPos == newSize - 1)
      {
         // sub-case: ptr is at the new edge - no shifting to do, and ptr can stay where it is
      }
      else
      {
         // sub-case: ptr would be beyond the new edge
         // left-shift what is ahead of ptr and make ptr point to the new edge
         memcpy(p->buffer, p->ptr - newSize + 1, newSize * sizeof(float));
         p->ptr = p->buffer + newSize - 1;
      }
   }

   p->size = newSize;
}


static void filter_t_resize(filter_t* p, size_t newSize)
{
   // Choose your resize method.
   //
   // When moving the Room Size slider:
   //
   // - using resize_resetting, you will not hear previous reverberations,
   //   but at the same time you will not hear sound artifacts
   //
   // - using resize_preserving, you will hear previous reverberations,
   //   but sometimes you might hear sound artifacts

#if 1
   filter_t_resize_resetting(p, newSize);
#else
   filter_t_resize_preserving(p, newSize);
#endif

}


static void reverb_allocate(reverb_t* p, double sample_rate_Hz, size_t buffer_size, float** out)
{
   memset(p, 0, sizeof(*p));

   // Input queue
   fifo_create(&p->input_fifo, sizeof(float));

   // Outputs
   out[0] = lsx_zalloc(p->out[0], buffer_size);
   out[1] = lsx_zalloc(p->out[1], buffer_size);

   // Allpass & Comb filters
   // pass the numbers that will cause the maximum possible allocation of the
   // buffer just once, even if it is in excess of what is required by the
   // settings passed in initialization after the allocation
   filter_array_allocate(p->chan + 0, sample_rate_Hz, 1.0, 0.0);
   filter_array_allocate(p->chan + 1, sample_rate_Hz, 1.0, 1.0);
}

// Some of the params can be set without having to re-init the input fifo
// or the comb/allpass filters.
static void reverb_set_simple_params
(
   reverb_t* p,
   double sample_rate_Hz,
   double wet_gain_dB,
   double reverberance,
   double hf_damping,
   double tone_low,       /* % */
   double tone_high       /* % */
)
{
   // Feedback, Damping, Gain
   double a = -1 / log(1 - /**/.3 /**/);           /* Set minimum feedback */
   double b = 100 / (log(1 - /**/.98/**/) * a + 1);  /* Set maximum feedback */

   p->feedback = 1 - exp((reverberance - b) / (a * b));
   p->hf_damping = hf_damping / 100 * .3 + .2;
   p->gain = dB_to_linear(wet_gain_dB) * .015;


   // LP-HP Filters
   double fc_highpass = midi_to_freq(72 - tone_low / 100 * 48);
   double fc_lowpass = midi_to_freq(72 + tone_high / 100 * 48);
   one_pole_init(&p->chan[0], sample_rate_Hz, fc_highpass, fc_lowpass);
   one_pole_init(&p->chan[1], sample_rate_Hz, fc_highpass, fc_lowpass);
}


static void reverb_init
(
   reverb_t* p,
   double sample_rate_Hz,
   double wet_gain_dB,
   double room_scale,     /* % */
   double reverberance,
   double hf_damping,
   double pre_delay_ms,
   double stereo_depth,
   double tone_low,       /* % */
   double tone_high       /* % */
)
{
   // Input queue
   fifo_clear_and_zero(&p->input_fifo);
   size_t delay = pre_delay_ms / 1000 * sample_rate_Hz + .5;
   memset(fifo_write(&p->input_fifo, delay, 0), 0, delay * sizeof(float));

   reverb_set_simple_params(p, sample_rate_Hz, wet_gain_dB, reverberance, hf_damping, tone_low, tone_high);

   // Allpass & Comb filters
   double scale = room_scale / 100 * .9 + .1;
   double depth = stereo_depth / 100;
   for (size_t i = 0; i <= ceil(depth); ++i)
   {
      filter_array_init(p->chan + i, sample_rate_Hz, scale, i * depth);
   }

   // Remember if stereo depth was set to 0, so we do not have to process twice
   p->initializedWithZeroDepth = (stereo_depth == 0.0);

}


static void reverb_create(reverb_t * p, double sample_rate_Hz,
      double wet_gain_dB,
      double room_scale,     /* % */
      double reverberance,   /* % */
      double hf_damping,     /* % */
      double pre_delay_ms,
      double stereo_depth,
      double tone_low,       /* % */
      double tone_high,      /* % */
      size_t buffer_size,
      float * * out)
{
   reverb_allocate(p, sample_rate_Hz, buffer_size, out);

   reverb_init(p, sample_rate_Hz, wet_gain_dB, room_scale, reverberance, hf_damping, pre_delay_ms, stereo_depth, tone_low, tone_high);
}

static void reverb_process(reverb_t * p, size_t length)
{
   filter_array_process(p->chan, length, (float *) fifo_read_ptr(&p->input_fifo), p->out[0], &p->feedback, &p->hf_damping, &p->gain);

   if (!p->initializedWithZeroDepth)
      filter_array_process(p->chan + 1, length, (float*)fifo_read_ptr(&p->input_fifo), p->out[1], &p->feedback, &p->hf_damping, &p->gain);

   fifo_read(&p->input_fifo, length, NULL);
}

static void reverb_delete(reverb_t * p)
{
   size_t i;
   for (i = 0; i < 2 && p->out[i]; ++i) {
      free(p->out[i]);
      filter_array_delete(p->chan + i);
   }
   fifo_delete(&p->input_fifo);
}

static void reverb_clear(reverb_t* p)
{
   for (size_t c = 0; c < 2; c++)
   {
      auto& chn = p->chan[c];

      chn.one_pole[0].i1 = 0.0;
      chn.one_pole[0].o1 = 0.0;

      chn.one_pole[1].i1 = 0.0;
      chn.one_pole[1].o1 = 0.0;

      for (size_t combIndex = 0; combIndex < array_length(comb_lengths); combIndex++)
      {
         auto& comb = chn.comb[combIndex];

         memset(comb.buffer, 0, comb.size * sizeof(float));

         comb.store = 0.0f;
      }

      for (size_t allpassIndex = 0; allpassIndex < array_length(allpass_lengths); allpassIndex++)
      {
         auto& allpass = chn.allpass[allpassIndex];

         memset(allpass.buffer, 0, allpass.size * sizeof(float));

         allpass.store = 0.0f;
      }
   } // loop on channels
      
   fifo_clear( &(p->input_fifo) );
}

