/* Accurate Vibrato effect 2
 * Copyright (c) 2020, Mark Tillotson, markt@chaos.org.uk
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <math.h>
#include <stdint.h>
#include <Arduino.h>
#include "accurate_vibrato2.h"



#define BUF_MASK 0x1FF    // 4 blocks
#if AUDIO_BLOCK_SAMPLES != 128
#  error "Only works for AUDIO_BLOCK_SAMPLES == 128 currently"
#endif

#define WORD_MAX 4294967296.0

#define MAX_DEVIATION (190 - VIBRATO_SINC_SAMPLES)

float AudioEffectHighQualityVibrato::cos_table [VIBRATO_SINC_SAMPLES+1] ;

AudioEffectHighQualityVibrato::AudioEffectHighQualityVibrato(void) : AudioStream(1, inputQueueArray)
{
    mod_phase_acc = 0 ;
    modulation (5.0, 2.0) ;  // 5Hz, 2% vibrato  (6% is a semitone)
    for (int i = 0 ; i < 4 * AUDIO_BLOCK_SAMPLES ; i++)
      buffer[i] = 0.0 ;
    // interpolation from buffer with max offset +/- 1.5 * 128, so 3 buffers needed + 1 for buffering
    bptr = 3 * AUDIO_BLOCK_SAMPLES / 2 ;
    cptr = 0 ;
    while (cptr < 3 * AUDIO_BLOCK_SAMPLES)  // insert new data starting at last block, with first 3 doing the tap
      buffer [cptr++] = 0 ;

    // initialize cosine taper table.
    if (cos_table [0] == 0.0)
    {
      for (int m = 0 ; m <= VIBRATO_SINC_SAMPLES+1 ; m++)
        if (m < VIBRATO_SINC_SAMPLES_FULL)
          cos_table [m] = 1.0 ;
        else
          cos_table [m] = 0.5 * (1.0 + cos ((m - VIBRATO_SINC_SAMPLES_FULL) * M_PI / \
					    (VIBRATO_SINC_SAMPLES - VIBRATO_SINC_SAMPLES_FULL))) ;
    }
}

void AudioEffectHighQualityVibrato::modulation (float hertz, float percent)
{
  if (hertz < 0.5) hertz = 0.5 ;
  if (hertz > 20.0) hertz = 20.0 ;
  if (percent > 10.0) percent = 10.0 ;

  mod_phase_inc = (uint32_t) int (round (hertz * WORD_MAX / AUDIO_SAMPLE_RATE)) ;
  float mod_d = (percent / 100.0) * AUDIO_SAMPLE_RATE / (2 * M_PI * hertz) ;
  if (mod_d > MAX_DEVIATION) mod_d = MAX_DEVIATION ;
  if (mod_d < 0.0) mod_d = 0.0 ;
  mod_depth = mod_d ;
    
  //Serial.printf ("setup vibrato, hertz %f, percent %f, inx %i, mod_d %f\n", hertz, percent, mod_phase_inc, mod_d) ;
}


void AudioEffectHighQualityVibrato::update(void)
{
  audio_block_t * vibrato = allocate () ;
  if (vibrato == NULL)
    return ;

  int16_t * data ;
  audio_block_t * new_block = receiveReadOnly (0) ;
  // copy new_block to circular buffer or zero
  if (new_block != NULL)
  {
    data = new_block->data ;
    for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
      buffer [cptr++] = data [i] ;
    release (new_block) ;
  }
  else
  {
    for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
      buffer [cptr++] = 0 ;
  }
  cptr &= BUF_MASK ;

  data = vibrato->data ;
  for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
  {
      float mod = mod_depth * sin (mod_phase_acc / WORD_MAX * 2 * M_PI) ;  // scale by mod depth

      int intpart = int (floor (mod)) ;
      float fract = mod - intpart ;   // fractional part of sample offset
     
      if (fract == 0.0)  // perfectly aligned case, would involve div by zero if we didn't handle specially.
      {
        data [i] = buffer [(bptr + intpart) & BUF_MASK] ;
      }
      else
      {
	float sum = 0.0;
	/*
        float sine = sin (- fract * M_PI) ;  // only need one sine call for every point as spaced pi apart.
        // inner loop, this is where to optimize!
        for (int m = -VIBRATO_SINC_SAMPLES ; m <= VIBRATO_SINC_SAMPLES ; m++)
        {
          float sinc = sine / (m - fract) ;
          sinc *= cos_table [m < 0 ? -m : m] ;
          sum += sinc * buffer [(bptr + intpart + m) & BUF_MASK] ;  // add sinc wave samples from buffer
          sine = -sine ; // alternates sign for sinc
        }
        */

	// optimized the loop a bit, remove conditionals and value sign alternation
        float y = sin (- fract * M_PI) ;  // numerator for sinc, pre-scaled by value
        int32_t p = bptr + intpart ;
        int32_t m = -VIBRATO_SINC_SAMPLES ;  // iterate m from -SINC_SAMPLES to +SINC_SAMPLES
        float   zz = m - fract ;   // denominator for sinc
        while (true)  // loop through -ve m
        {
          float sinc = y / zz * cos_table [-m] ; // even index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz * cos_table [-m] ;     // odd index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= -VIBRATO_SINC_SAMPLES_FULL)
            break ;  
	}
        while (true)  // loop through -ve m
        {
          float sinc = y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= 0)
            break ;   // m no longer negative
        }
        // m is zero now
        zz = -fract ;  // need the denominator properly accurate for when m == 0
        while (true)  // loop through +ve/zero m
        {
          float sinc = y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= VIBRATO_SINC_SAMPLES_FULL)
            break ;
        }
        while (true)  // loop through +ve/zero m
        {
          float sinc = y / zz * cos_table [m] ; // even index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          if (m >= VIBRATO_SINC_SAMPLES)
            break ;
          m ++ ; zz ++ ;
          sinc = -y / zz * cos_table [m] ;      // odd index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
        }

	data [i] = int (round (sum / M_PI)) ;
      }
      mod_phase_acc += mod_phase_inc ;   // advance modulation phase
      bptr ++ ;
  }


  transmit (vibrato, 0) ;
  release (vibrato) ;
}/* Accurate Vibrato effect 2
 * Copyright (c) 2020, Mark Tillotson, markt@chaos.org.uk
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <math.h>
#include <stdint.h>
#include <Arduino.h>
#include "accurate_vibrato2.h"



#define BUF_MASK 0x1FF    // 4 blocks
#if AUDIO_BLOCK_SAMPLES != 128
#  error "Only works for AUDIO_BLOCK_SAMPLES == 128 currently"
#endif

#define WORD_MAX 4294967296.0

#define MAX_DEVIATION (190 - VIBRATO_SINC_SAMPLES)

float AudioEffectHighQualityVibrato::cos_table [VIBRATO_SINC_SAMPLES+1] ;

AudioEffectHighQualityVibrato::AudioEffectHighQualityVibrato(void) : AudioStream(1, inputQueueArray)
{
    mod_phase_acc = 0 ;
    modulation (5.0, 2.0) ;  // 5Hz, 2% vibrato  (6% is a semitone)
    for (int i = 0 ; i < 4 * AUDIO_BLOCK_SAMPLES ; i++)
      buffer[i] = 0.0 ;
    // interpolation from buffer with max offset +/- 1.5 * 128, so 3 buffers needed + 1 for buffering
    bptr = 3 * AUDIO_BLOCK_SAMPLES / 2 ;
    cptr = 0 ;
    while (cptr < 3 * AUDIO_BLOCK_SAMPLES)  // insert new data starting at last block, with first 3 doing the tap
      buffer [cptr++] = 0 ;

    // initialize cosine taper table.
    if (cos_table [0] == 0.0)
    {
      for (int m = 0 ; m <= VIBRATO_SINC_SAMPLES+1 ; m++)
        if (m < VIBRATO_SINC_SAMPLES_FULL)
          cos_table [m] = 1.0 ;
        else
          cos_table [m] = 0.5 * (1.0 + cos ((m - VIBRATO_SINC_SAMPLES_FULL) * M_PI / \
					    (VIBRATO_SINC_SAMPLES - VIBRATO_SINC_SAMPLES_FULL))) ;
    }
}

void AudioEffectHighQualityVibrato::modulation (float hertz, float percent)
{
  if (hertz < 0.5) hertz = 0.5 ;
  if (hertz > 20.0) hertz = 20.0 ;
  if (percent > 10.0) percent = 10.0 ;

  mod_phase_inc = (uint32_t) int (round (hertz * WORD_MAX / AUDIO_SAMPLE_RATE)) ;
  float mod_d = (percent / 100.0) * AUDIO_SAMPLE_RATE / (2 * M_PI * hertz) ;
  if (mod_d > MAX_DEVIATION) mod_d = MAX_DEVIATION ;
  if (mod_d < 0.0) mod_d = 0.0 ;
  mod_depth = mod_d ;
    
  //Serial.printf ("setup vibrato, hertz %f, percent %f, inx %i, mod_d %f\n", hertz, percent, mod_phase_inc, mod_d) ;
}


void AudioEffectHighQualityVibrato::update(void)
{
  audio_block_t * vibrato = allocate () ;
  if (vibrato == NULL)
    return ;

  int16_t * data ;
  audio_block_t * new_block = receiveReadOnly (0) ;
  // copy new_block to circular buffer or zero
  if (new_block != NULL)
  {
    data = new_block->data ;
    for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
      buffer [cptr++] = data [i] ;
    release (new_block) ;
  }
  else
  {
    for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
      buffer [cptr++] = 0 ;
  }
  cptr &= BUF_MASK ;

  data = vibrato->data ;
  for (int i = 0 ; i < AUDIO_BLOCK_SAMPLES ; i++)
  {
      float mod = mod_depth * sin (mod_phase_acc / WORD_MAX * 2 * M_PI) ;  // scale by mod depth

      int intpart = int (floor (mod)) ;
      float fract = mod - intpart ;   // fractional part of sample offset
     
      if (fract == 0.0)  // perfectly aligned case, would involve div by zero if we didn't handle specially.
      {
        data [i] = buffer [(bptr + intpart) & BUF_MASK] ;
      }
      else
      {
	float sum = 0.0;
	/*
        float sine = sin (- fract * M_PI) ;  // only need one sine call for every point as spaced pi apart.
        // inner loop, this is where to optimize!
        for (int m = -VIBRATO_SINC_SAMPLES ; m <= VIBRATO_SINC_SAMPLES ; m++)
        {
          float sinc = sine / (m - fract) ;
          sinc *= cos_table [m < 0 ? -m : m] ;
          sum += sinc * buffer [(bptr + intpart + m) & BUF_MASK] ;  // add sinc wave samples from buffer
          sine = -sine ; // alternates sign for sinc
        }
        */

	// optimized the loop a bit, remove conditionals and value sign alternation
        float y = sin (- fract * M_PI) ;  // numerator for sinc, pre-scaled by value
        int32_t p = bptr + intpart ;
        int32_t m = -VIBRATO_SINC_SAMPLES ;  // iterate m from -SINC_SAMPLES to +SINC_SAMPLES
        float   zz = m - fract ;   // denominator for sinc
        while (true)  // loop through -ve m
        {
          float sinc = y / zz * cos_table [-m] ; // even index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz * cos_table [-m] ;     // odd index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= -VIBRATO_SINC_SAMPLES_FULL)
            break ;  
	}
        while (true)  // loop through -ve m
        {
          float sinc = y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= 0)
            break ;   // m no longer negative
        }
        // m is zero now
        zz = -fract ;  // need the denominator properly accurate for when m == 0
        while (true)  // loop through +ve/zero m
        {
          float sinc = y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          sinc = -y / zz ;
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
          if (m >= VIBRATO_SINC_SAMPLES_FULL)
            break ;
        }
        while (true)  // loop through +ve/zero m
        {
          float sinc = y / zz * cos_table [m] ; // even index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          if (m >= VIBRATO_SINC_SAMPLES)
            break ;
          m ++ ; zz ++ ;
          sinc = -y / zz * cos_table [m] ;      // odd index
          sum += sinc * buffer [(p+m) & BUF_MASK] ;
          m ++ ; zz ++ ;
        }

	data [i] = int (round (sum / M_PI)) ;
      }
      mod_phase_acc += mod_phase_inc ;   // advance modulation phase
      bptr ++ ;
  }


  transmit (vibrato, 0) ;
  release (vibrato) ;
}