/*---------------------------------------------------------------------------*/
/*
  File: noisesup.cpp

  Noise suppression.
*/
/*---------------------------------------------------------------------------*/

#include "platform.h"
#include "noisesup.h"
#include "sdsputil.h"
#include "sbfpfft.h"


#define FILE_ "NOISESUP.CPP"


/*---------------------------------------------------------------------------*/

#define NOISESUPP_ENERGY_THRESHOLD_DEFAULT       0x0600

#define NOISESUPP_FFT_MIN_FREQUENCY              ((((dword) 200) * ((dword) NOISESUPP_FFT_SIZE_HALF)) / ((dword) NOISESUPP_SAMPLE_RATE_HALF))
#define NOISESUPP_FFT_MAX_FREQUENCY              ((((dword)(NOISESUPP_SAMPLE_RATE_HALF - 200)) * ((dword) NOISESUPP_FFT_SIZE_HALF)) / ((dword) NOISESUPP_SAMPLE_RATE_HALF))
#define NOISESUPP_FFT_MIN_FREQUENCY_SLOPE        0x4000  /* 72 dB over 12 taps */
#define NOISESUPP_FFT_MAX_FREQUENCY_SLOPE        0x4000  /* 72 dB over 12 taps */

#define NOISESUPP_FFT_SHAPE_GAIN                 0x0c00
#define NOISESUPP_FFT_SHAPE_THRESHOLD            0xffff
#define NOISESUPP_FFT_SHAPE_DIV_THRESHOLD        ((0x3fffffff / NOISESUPP_FFT_SHAPE_THRESHOLD) << 1)

#define NOISESUPP_SIGNAL_GRADIENT_THRESHOLD_HALF 0x0800

#define NOISESUPP_MAX_NOISE_ATTENUATION          0x0400  /* 30 dB */
#define NOISESUPP_MUSIC_SHAPE_CORNER             4
#define NOISESUPP_MUSIC_SHAPE_MIN_GAIN           0x0800


#define NOISESUPP_LOG_TABLE_SIZE_EXP        4
#define NOISESUPP_LOG_TABLE_SIZE            ((1 << NOISESUPP_LOG_TABLE_SIZE_EXP) + 1)

static const word noisesupp_log_table[NOISESUPP_LOG_TABLE_SIZE] =
{
  0x0000, 0x0043, 0x0083, 0x00be, 0x00f7, 0x012d, 0x0161, 0x0192,
  0x01c1, 0x01ee, 0x021a, 0x0244, 0x026c, 0x0293, 0x02b8, 0x02dd,
  0x0300
};


word noise_suppressor_log (dword sqr_mag, short fft_exp, short threshold)
{
  int32 l;
  word j, n;

  n = sdsp_dword_leading_zeroes (sqr_mag);
  sqr_mag <<= n;
  j = (word)((sqr_mag << 1) >> (32 - NOISESUPP_LOG_TABLE_SIZE_EXP));
  n = 0x6300 - 0x300 * n + noisesupp_log_table[j] +
    ((word)(((dword)(((dword)((sqr_mag >> (15 - NOISESUPP_LOG_TABLE_SIZE_EXP)) & 0xffff)) *
      ((dword)(noisesupp_log_table[j+1] - noisesupp_log_table[j])))) >> 16));
  l = n + 0x600 * ((int32) fft_exp) - threshold;
  return ((l > 0) ? ((word)(l >> 1)) : 0);
}


void noise_suppressor_init (t_noise_suppressor *p_noisesupp,
  t_complex_short *fft_buffer,
  word *magnitude_buffer, word *attenuations)
{
  word i, n;
  short *energy_history;
  
  p_noisesupp->fft_buffer = fft_buffer;
  p_noisesupp->magnitude_buffer = magnitude_buffer;
  p_noisesupp->attenuations = attenuations;
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->last_input[i] = 0;
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->last_output[i] = 0;
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->noise_magnitudes[i] = 0;
  for (n = 0; n < NOISESUPP_FREQ_GROUP_COUNT; n++)
  {
    energy_history = p_noisesupp->energy_history_table[n];
    for (i = 0; i < NOISESUPP_ENERGY_HISTORY_SIZE; i++)
      energy_history[i] = 0;
    p_noisesupp->energy_history_pos_table[n] = 0;
  }
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->sample_block[i] = 0;
  p_noisesupp->sample_pos = 0;
  p_noisesupp->voice_active = FALSE;
}


void noise_suppressor_process (t_noise_suppressor *p_noisesupp,
  short *input_sample_block, short *output_sample_block)
{
  word *shape_buffer;
  short s, t, fft_exp;
  short magnitude_block_exp;
  short energy, *energy_history;
  word gradient_valid_pos;
  int32 l;
  dword d, e;
  word i, j, k, n;

  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
  {
    p_noisesupp->fft_buffer[i].re = p_noisesupp->last_input[i];
    p_noisesupp->fft_buffer[i].im = 0;
  }
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
  {
    p_noisesupp->fft_buffer[NOISESUPP_FFT_SIZE_HALF + i].re = input_sample_block[i];
    p_noisesupp->fft_buffer[NOISESUPP_FFT_SIZE_HALF + i].im = 0;
    p_noisesupp->last_input[i] = input_sample_block[i];
  }

  fft_exp = 0;
  sbfpfft_normalize (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, &fft_exp, 0);

  sbfpfft_window (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, &fft_exp);

  sbfpfft_execute (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, &fft_exp);

  k = 0x8000;
  i = NOISESUPP_FFT_MIN_FREQUENCY + 1;
  do
  {
    i--;
    k = (word)(((dword)(((dword) k) * ((dword) NOISESUPP_FFT_MIN_FREQUENCY_SLOPE))) >> 15);
    p_noisesupp->fft_buffer[i].re = (short)(((int32)(((int32) p_noisesupp->fft_buffer[i].re) * ((int32) k))) >> 15);
    p_noisesupp->fft_buffer[i].im = (short)(((int32)(((int32) p_noisesupp->fft_buffer[i].im) * ((int32) k))) >> 15);
  } while (i != 0);
  k = 0x8000;
  i = NOISESUPP_FFT_MAX_FREQUENCY;
  do
  {
    i++;
    k = (word)(((dword)(((dword) k) * ((dword) NOISESUPP_FFT_MAX_FREQUENCY_SLOPE))) >> 15);
    p_noisesupp->fft_buffer[i].re = (short)(((int32)(((int32) p_noisesupp->fft_buffer[i].re) * ((int32) k))) >> 15);
    p_noisesupp->fft_buffer[i].im = (short)(((int32)(((int32) p_noisesupp->fft_buffer[i].im) * ((int32) k))) >> 15);
  } while (i != NOISESUPP_FFT_SIZE_HALF);

  sbfpfft_invert_scale (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, &fft_exp);

  magnitude_block_exp = sbfpfft_magnitude (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, fft_exp,
    p_noisesupp->magnitude_buffer);

  p_noisesupp->voice_active = FALSE;
  gradient_valid_pos = NOISESUPP_FFT_SIZE_HALF;
  k = NOISESUPP_FREQ_GROUP_TAPS;
  n = 1;
  do
  {
    energy_history = p_noisesupp->energy_history_table[n];
    j = 0;
    for (i = 0; i < NOISESUPP_FREQ_GROUP_TAPS; i++)
    {
      if (p_noisesupp->magnitude_buffer[k] > j)
        j = p_noisesupp->magnitude_buffer[k];
      k++;
    }
    energy = noise_suppressor_log (((dword) j) * ((dword) j), magnitude_block_exp, NOISESUPP_ENERGY_THRESHOLD_DEFAULT);
    energy_history[p_noisesupp->energy_history_pos_table[n]] = energy;
    p_noisesupp->energy_history_pos_table[n] = (p_noisesupp->energy_history_pos_table[n] < NOISESUPP_ENERGY_HISTORY_SIZE - 1) ?
      p_noisesupp->energy_history_pos_table[n] + 1 : 0;
    s = 0x7fff;
    t = 0;
    for (i = 0; i < NOISESUPP_ENERGY_HISTORY_SIZE; i++)
    {
      j = p_noisesupp->energy_history_pos_table[n] + i;
      if (j >= NOISESUPP_ENERGY_HISTORY_SIZE)
        j -= NOISESUPP_ENERGY_HISTORY_SIZE;
      if (energy_history[j] < s)
        s = energy_history[j];
      if (/*(i >= NOISESUPP_ENERGY_HISTORY_SIZE / 2) &&*/ (energy_history[j] > t))
        t = energy_history[j];
    }
    if (((word)(t - s)) >= NOISESUPP_SIGNAL_GRADIENT_THRESHOLD_HALF)
    {
      if (gradient_valid_pos == NOISESUPP_FFT_SIZE_HALF)
      {
        gradient_valid_pos = k - NOISESUPP_FREQ_GROUP_TAPS;
        p_noisesupp->voice_active = TRUE;
      }
    }
    n++;
  } while (n < NOISESUPP_FREQ_GROUP_COUNT);

  shape_buffer = (word *)(&(p_noisesupp->fft_buffer[NOISESUPP_FFT_SIZE_HALF].re));
  e = 0;
  i = NOISESUPP_FFT_SIZE_HALF;
  do
  {
    i--;
/* do not use this code since the Blackfin compiler makes a mistake
    e += (((dword)(p_noisesupp->magnitude_buffer[i])) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) -
      ((dword)(e >> 16)) * ((dword) NOISESUPP_FFT_SHAPE_GAIN)) << 1;
*/
    e -= ((dword)(e >> 16)) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) << 1;
    e += ((dword)(p_noisesupp->magnitude_buffer[i])) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) << 1;
    shape_buffer[i] = (word)(e >> 16);
  } while (i != 0);
  n = 0;
  e = 0;
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
  {
/* do not use this code since the Blackfin compiler makes a mistake
    e += (((dword)(p_noisesupp->magnitude_buffer[i])) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) -
      ((dword)(e >> 16)) * ((dword) NOISESUPP_FFT_SHAPE_GAIN)) << 1;
*/
    e -= ((dword)(e >> 16)) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) << 1;
    e += ((dword)(p_noisesupp->magnitude_buffer[i])) * ((dword) NOISESUPP_FFT_SHAPE_GAIN) << 1;
    j = (e >> 16) + shape_buffer[i];
    if ((i < gradient_valid_pos) || (j < n))
      n = j;
    shape_buffer[i] = (n >= NOISESUPP_FFT_SHAPE_DIV_THRESHOLD) ? 0xffff :
      ((word)(((dword)(((dword) n) * ((dword) NOISESUPP_FFT_SHAPE_THRESHOLD))) >> 15));
  }
/*
k = 0;
d = 0;
e = 0;
n = 0;
j = 0;
for (i = 4; i < NOISESUPP_FFT_SIZE_HALF - 4; i++)
{
  if (p_noisesupp->magnitude_buffer[i] > (shape_buffer[i] >> 1))
    d++;
  if (p_noisesupp->magnitude_buffer[i] > shape_buffer[i])
    k++;
  if ((p_noisesupp->magnitude_buffer[i] >> 1) > shape_buffer[i])
    e++;
  if (p_noisesupp->magnitude_buffer[i] > n)
    n = p_noisesupp->magnitude_buffer[i];
  if (shape_buffer[i] > j)
    j = shape_buffer[i];
}
dprintf ("sup %ld %d %ld %d %d", (long) d, (int) k, (long) e, (int) n, (int) j);
*/
  for (i = 4; i < NOISESUPP_FFT_SIZE_HALF - 4; i++)
  {
    d = p_noisesupp->magnitude_buffer[i];
    s = magnitude_block_exp;
    d = (s >= 0) ? d << s : d >> -s;
    if ((d < (p_noisesupp->noise_magnitudes[i] >> 8))
     || ((i < gradient_valid_pos)
      && (p_noisesupp->magnitude_buffer[i-4] <= shape_buffer[i-4])
      && (p_noisesupp->magnitude_buffer[i-3] <= shape_buffer[i-3])
      && (p_noisesupp->magnitude_buffer[i-2] <= shape_buffer[i-2])
      && (p_noisesupp->magnitude_buffer[i-1] <= shape_buffer[i-1])
      && (p_noisesupp->magnitude_buffer[i] <= shape_buffer[i])
      && (p_noisesupp->magnitude_buffer[i+1] <= shape_buffer[i+1])
      && (p_noisesupp->magnitude_buffer[i+2] <= shape_buffer[i+2])
      && (p_noisesupp->magnitude_buffer[i+3] <= shape_buffer[i+3])
      && (p_noisesupp->magnitude_buffer[i+4] <= shape_buffer[i+4])))
    {
      p_noisesupp->noise_magnitudes[i] += (d - (p_noisesupp->noise_magnitudes[i] >> 8)) << 5;
    }
  }

  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->attenuations[i] = NOISESUPP_MAX_NOISE_ATTENUATION;
  i = 1;
  k = 0;
  while (i < NOISESUPP_FFT_SIZE_HALF - 1)
  {
    d = p_noisesupp->magnitude_buffer[i];
    s = magnitude_block_exp;
    d = (s >= 0) ? d << s : d >> -s;
    e = p_noisesupp->noise_magnitudes[i-1];
    if (p_noisesupp->noise_magnitudes[i] > e)
      e = p_noisesupp->noise_magnitudes[i];
    if (p_noisesupp->noise_magnitudes[i+1] > e)
      e = p_noisesupp->noise_magnitudes[i+1];
    e >>= 7;
    if (d <= e)
    {
      k = (word)(((dword)(((dword)(0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN)) * ((dword) k))) >> 15);
    }
    else
    {
      s = 1 - sdsp_dword_leading_zeroes (d);
      j = (word)((e << -s) >> 16);
      d = (((dword)(j >> 1)) * ((dword) NOISESUPP_MUSIC_SHAPE_CORNER)) << 1;
      if (d > 0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN)
        d = 0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN;
      k = (word)(((dword)(((dword)(0x8000 - d)) * ((dword)(0x8000 - j)) + ((dword) d) * ((dword) k))) >> 15);
    }
    if (k > p_noisesupp->attenuations[i])
      p_noisesupp->attenuations[i] = k;
    i++;
  }
  k = 0;
  while (i != 1)
  {
    i--;
    d = p_noisesupp->magnitude_buffer[i];
    s = magnitude_block_exp;
    d = (s >= 0) ? d << s : d >> -s;
    e = p_noisesupp->noise_magnitudes[i-1];
    if (p_noisesupp->noise_magnitudes[i] > e)
      e = p_noisesupp->noise_magnitudes[i];
    if (p_noisesupp->noise_magnitudes[i+1] > e)
      e = p_noisesupp->noise_magnitudes[i+1];
    e >>= 7;
    if (d <= e)
    {
      k = (word)(((dword)(((dword)(0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN)) * ((dword) k))) >> 15);
    }
    else
    {
      s = 1 - sdsp_dword_leading_zeroes (d);
      j = (word)((e << -s) >> 16);
      d = (((dword)(j >> 1)) * ((dword) NOISESUPP_MUSIC_SHAPE_CORNER)) << 1;
      if (d > 0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN)
        d = 0x8000 - NOISESUPP_MUSIC_SHAPE_MIN_GAIN;
      k = (word)(((dword)(((dword)(0x8000 - d)) * ((dword)(0x8000 - j)) + ((dword) d) * ((dword) k))) >> 15);
    }
    if (k > p_noisesupp->attenuations[i])
      p_noisesupp->attenuations[i] = k;
    if (p_noisesupp->attenuations[i] > 0x7fff)
      p_noisesupp->attenuations[i] = 0x7fff;
  }

  sbfpfft_denorm (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, p_noisesupp->attenuations);

  sbfpfft_mirror (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE);

  sbfpfft_execute (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, &fft_exp);

  sbfpfft_denormalize (p_noisesupp->fft_buffer, NOISESUPP_FFT_SIZE, fft_exp, 0);

  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
  {
    l = p_noisesupp->last_output[i] + p_noisesupp->fft_buffer[i].re;
    if (l < -32768)
      s = -32768;
    else if (l > 32767)
      s = 32767;
    else
      s = (short) l;
    output_sample_block[i] = s;
  }
  for (i = 0; i < NOISESUPP_FFT_SIZE_HALF; i++)
    p_noisesupp->last_output[i] = p_noisesupp->fft_buffer[NOISESUPP_FFT_SIZE_HALF + i].re;
}


void noise_suppressor_exec (t_noise_suppressor *p_noisesupp,
  short *sample_buffer, word sample_count)
{
  word i, n;
  short s;

  i = 0;
  do
  {
    n = sample_count - i;
    if (n > NOISESUPP_FFT_SIZE / 2 - p_noisesupp->sample_pos)
      n = NOISESUPP_FFT_SIZE / 2 - p_noisesupp->sample_pos;
    n += i;
    while (i < n)
    {
      s = p_noisesupp->sample_block[p_noisesupp->sample_pos];
      p_noisesupp->sample_block[p_noisesupp->sample_pos] = sample_buffer[i];
      sample_buffer[i] = s;
      (p_noisesupp->sample_pos)++;
      i++;
    }
    if (p_noisesupp->sample_pos == NOISESUPP_FFT_SIZE / 2)
    {
      p_noisesupp->sample_pos = 0;
      noise_suppressor_process (p_noisesupp,
        p_noisesupp->sample_block, p_noisesupp->sample_block);
    }
  } while (i < sample_count);
}


/*---------------------------------------------------------------------------*/
