/*---------------------------------------------------------------------------*/
/*
  File: sbfpfft.c

  Block floating point FFT utilities for Blackfin.
*/
/*---------------------------------------------------------------------------*/

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


#define FILE_ "SBFPFFT.C"


/*---------------------------------------------------------------------------*/
#ifdef SBFPFFT_ASM

extern void sbfpfft_normalize (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp, short bias_exp);
extern void sbfpfft_window (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp);
extern void sbfpfft_custom_window (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp, const short *neg_coefs, word coef_step);
extern void sbfpfft_execute (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp);
extern void sbfpfft_invert_scale (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp);
extern void sbfpfft_denormalize (t_complex_short *fft_buffer, word fft_size, short fft_exp, short result_exp);
extern void sbfpfft_sqr (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp);
extern void sbfpfft_sqrt (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp);
extern void sbfpfft_log (t_complex_short *fft_buffer, word fft_size, short fft_exp, word threshold);
extern short sbfpfft_magnitude (const t_complex_short *fft_buffer, word fft_size, short fft_exp, word *magnitude_buffer);
extern void sbfpfft_norm (const t_complex_short *fft_buffer, word fft_size, t_complex_short *result_buffer);
extern void sbfpfft_denorm (t_complex_short *fft_buffer, word fft_size, const word *magnitude_buffer);
extern void sbfpfft_mirror (t_complex_short *fft_buffer, word fft_size);
extern void sbfpfft_rotate_positive (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer);
extern void sbfpfft_rotate_negative (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer);
extern void sbfpfft_rotate_positive_norm (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer);
extern void sbfpfft_rotate_negative_norm (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer);
extern void sbfpfft_rotate_difference_norm (const t_complex_short *plus_angle_buffer, word fft_size,
  const t_complex_short *minus_angle_buffer, t_complex_short *result_buffer);

#else /* SBFPFFT_ASM */
/*---------------------------------------------------------------------------*/

#if (SBFPFFT_MAX_FFT_SIZE == 64)

static const word sbfpfft_minus_cos_table[SBFPFFT_MAX_FFT_SIZE / 4 + 1] =
{
  0x8000, 0x809e, 0x8276, 0x8583, 0x89be, 0x8f1d, 0x9592, 0x9d0e,
  0xa57e, 0xaecc, 0xb8e3, 0xc3a9, 0xcf04, 0xdad8, 0xe707, 0xf374,
  0x0000
};

#elif (SBFPFFT_MAX_FFT_SIZE == 128)

static const word sbfpfft_minus_cos_table[SBFPFFT_MAX_FFT_SIZE / 4 + 1] =
{
  0x8000, 0x8027, 0x809e, 0x8163, 0x8276, 0x83d6, 0x8583, 0x877b,
  0x89be, 0x8c4a, 0x8f1d, 0x9236, 0x9592, 0x9930, 0x9d0e, 0xa129,
  0xa57e, 0xaa0a, 0xaecc, 0xb3c0, 0xb8e3, 0xbe32, 0xc3a9, 0xc946,
  0xcf04, 0xd4e1, 0xdad8, 0xe0e6, 0xe707, 0xed38, 0xf374, 0xf9b8,
  0x0000
};

#elif (SBFPFFT_MAX_FFT_SIZE == 256)

static const word sbfpfft_minus_cos_table[SBFPFFT_MAX_FFT_SIZE / 4 + 1] =
{
  0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163, 0x81e2,
  0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b, 0x8894,
  0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236, 0x93dc,
  0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129, 0xa34c,
  0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0, 0xb64c,
  0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946, 0xcc21,
  0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6, 0xe3f4,
  0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8, 0xfcdc,
  0x0000
};

#elif (SBFPFFT_MAX_FFT_SIZE == 512)

static const word sbfpfft_minus_cos_table[SBFPFFT_MAX_FFT_SIZE / 4 + 1] =
{
  0x8000, 0x8002, 0x800a, 0x8016, 0x8027, 0x803e, 0x8059, 0x8079,
  0x809e, 0x80c8, 0x80f6, 0x812a, 0x8163, 0x81a0, 0x81e2, 0x822a,
  0x8276, 0x82c6, 0x831c, 0x8377, 0x83d6, 0x843a, 0x84a3, 0x8511,
  0x8583, 0x85fa, 0x8676, 0x86f6, 0x877b, 0x8805, 0x8894, 0x8927,
  0x89be, 0x8a5a, 0x8afb, 0x8ba0, 0x8c4a, 0x8cf8, 0x8dab, 0x8e62,
  0x8f1d, 0x8fdd, 0x90a1, 0x9169, 0x9236, 0x9307, 0x93dc, 0x94b5,
  0x9592, 0x9674, 0x9759, 0x9843, 0x9930, 0x9a22, 0x9b17, 0x9c11,
  0x9d0e, 0x9e0f, 0x9f14, 0xa01c, 0xa129, 0xa238, 0xa34c, 0xa463,
  0xa57e, 0xa69c, 0xa7bd, 0xa8e2, 0xaa0a, 0xab36, 0xac65, 0xad97,
  0xaecc, 0xb005, 0xb140, 0xb27f, 0xb3c0, 0xb505, 0xb64c, 0xb796,
  0xb8e3, 0xba33, 0xbb85, 0xbcda, 0xbe32, 0xbf8c, 0xc0e9, 0xc248,
  0xc3a9, 0xc50d, 0xc673, 0xc7db, 0xc946, 0xcab2, 0xcc21, 0xcd92,
  0xcf04, 0xd079, 0xd1ef, 0xd367, 0xd4e1, 0xd65c, 0xd7d9, 0xd958,
  0xdad8, 0xdc59, 0xdddc, 0xdf61, 0xe0e6, 0xe26d, 0xe3f4, 0xe57d,
  0xe707, 0xe892, 0xea1e, 0xebab, 0xed38, 0xeec6, 0xf055, 0xf1e4,
  0xf374, 0xf505, 0xf695, 0xf827, 0xf9b8, 0xfb4a, 0xfcdc, 0xfe6e,
  0x0000
};

#elif (SBFPFFT_MAX_FFT_SIZE == 1024)

static const word sbfpfft_minus_cos_table[SBFPFFT_MAX_FFT_SIZE / 4 + 1] =
{
  0x8000, 0x8001, 0x8002, 0x8006, 0x800a, 0x800f, 0x8016, 0x801e,
  0x8027, 0x8032, 0x803e, 0x804b, 0x8059, 0x8068, 0x8079, 0x808b,
  0x809e, 0x80b2, 0x80c8, 0x80de, 0x80f6, 0x8110, 0x812a, 0x8146,
  0x8163, 0x8181, 0x81a0, 0x81c1, 0x81e2, 0x8205, 0x822a, 0x824f,
  0x8276, 0x829d, 0x82c6, 0x82f1, 0x831c, 0x8349, 0x8377, 0x83a6,
  0x83d6, 0x8407, 0x843a, 0x846e, 0x84a3, 0x84d9, 0x8511, 0x8549,
  0x8583, 0x85be, 0x85fa, 0x8637, 0x8676, 0x86b6, 0x86f6, 0x8738,
  0x877b, 0x87c0, 0x8805, 0x884c, 0x8894, 0x88dd, 0x8927, 0x8972,
  0x89be, 0x8a0c, 0x8a5a, 0x8aaa, 0x8afb, 0x8b4d, 0x8ba0, 0x8bf5,
  0x8c4a, 0x8ca1, 0x8cf8, 0x8d51, 0x8dab, 0x8e06, 0x8e62, 0x8ebf,
  0x8f1d, 0x8f7d, 0x8fdd, 0x903e, 0x90a1, 0x9105, 0x9169, 0x91cf,
  0x9236, 0x929e, 0x9307, 0x9371, 0x93dc, 0x9448, 0x94b5, 0x9523,
  0x9592, 0x9603, 0x9674, 0x96e6, 0x9759, 0x97ce, 0x9843, 0x98b9,
  0x9930, 0x99a9, 0x9a22, 0x9a9c, 0x9b17, 0x9b94, 0x9c11, 0x9c8f,
  0x9d0e, 0x9d8e, 0x9e0f, 0x9e91, 0x9f14, 0x9f98, 0xa01c, 0xa0a2,
  0xa129, 0xa1b0, 0xa238, 0xa2c2, 0xa34c, 0xa3d7, 0xa463, 0xa4f0,
  0xa57e, 0xa60c, 0xa69c, 0xa72c, 0xa7bd, 0xa84f, 0xa8e2, 0xa976,
  0xaa0a, 0xaaa0, 0xab36, 0xabcd, 0xac65, 0xacfd, 0xad97, 0xae31,
  0xaecc, 0xaf68, 0xb005, 0xb0a2, 0xb140, 0xb1df, 0xb27f, 0xb31f,
  0xb3c0, 0xb462, 0xb505, 0xb5a8, 0xb64c, 0xb6f1, 0xb796, 0xb83c,
  0xb8e3, 0xb98b, 0xba33, 0xbadc, 0xbb85, 0xbc2f, 0xbcda, 0xbd86,
  0xbe32, 0xbedf, 0xbf8c, 0xc03a, 0xc0e9, 0xc198, 0xc248, 0xc2f8,
  0xc3a9, 0xc45b, 0xc50d, 0xc5c0, 0xc673, 0xc727, 0xc7db, 0xc890,
  0xc946, 0xc9fc, 0xcab2, 0xcb69, 0xcc21, 0xccd9, 0xcd92, 0xce4b,
  0xcf04, 0xcfbe, 0xd079, 0xd134, 0xd1ef, 0xd2ab, 0xd367, 0xd424,
  0xd4e1, 0xd59e, 0xd65c, 0xd71b, 0xd7d9, 0xd898, 0xd958, 0xda18,
  0xdad8, 0xdb99, 0xdc59, 0xdd1b, 0xdddc, 0xde9e, 0xdf61, 0xe023,
  0xe0e6, 0xe1a9, 0xe26d, 0xe330, 0xe3f4, 0xe4b9, 0xe57d, 0xe642,
  0xe707, 0xe7cd, 0xe892, 0xe958, 0xea1e, 0xeae4, 0xebab, 0xec71,
  0xed38, 0xedff, 0xeec6, 0xef8d, 0xf055, 0xf11c, 0xf1e4, 0xf2ac,
  0xf374, 0xf43c, 0xf505, 0xf5cd, 0xf695, 0xf75e, 0xf827, 0xf8ef,
  0xf9b8, 0xfa81, 0xfb4a, 0xfc13, 0xfcdc, 0xfda5, 0xfe6e, 0xff37,
  0x0000
};

#endif


void sbfpfft_normalize (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp, short bias_exp)
{
  word i, n;
  short s, t;

  n = 0;
  for (i = 0; i < fft_size; i++)
  {
    s = fft_buffer[i].re;
    if (s >= 0)
    {
      if (((word) s) > n)
        n = s;
    }
    else
    {
      if (((word) ~s) > n)
        n = (word) ~s;
    }
    s = fft_buffer[i].im;
    if (s >= 0)
    {
      if (((word) s) > n)
        n = s;
    }
    else
    {
      if (((word) ~s) > n)
        n = (word) ~s;
    }
  }
  s = bias_exp - (1 - sdsp_word_leading_zeroes (n));
  *p_fft_exp -= s;
  if (s > 0)
  {
    for (i = 0; i < fft_size; i++)
    {
      fft_buffer[i].re <<= s;
      fft_buffer[i].im <<= s;
    }
  }
  else if (s < 0)
  {
    s = -s;
    n = ((word) 0x8000) >> s;
    t = 0x7fff >> -bias_exp;
    for (i = 0; i < fft_size; i++)
    {
      s = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) n) + 0x4000)) >> 15);
      fft_buffer[i].re = (s <= t) ? s : t;
      s = (short)(((int32)(((int32)(fft_buffer[i].im)) * ((int32) n) + 0x4000)) >> 15);
      fft_buffer[i].im = (s <= t) ? s : t;
    }
  }
}


void sbfpfft_window (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp)
{
  word i, n, w;
  const short *p;

  n = (SBFPFFT_MAX_FFT_SIZE * sizeof(word)) >> (14 + 1 - sdsp_word_leading_zeroes (fft_size));
  i = 0;
  p = (const short *)(sbfpfft_minus_cos_table);
  do
  {
    w = 0x4000 + (*p >> 1);
    fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) w) + 0x4000)) >> 15);
    p = (const short *)(((const byte *) p) + n);
    i++;
  } while (i < (fft_size >> 2));
  p = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
  do
  {
    w = 0x4000 - (*p >> 1);
    fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) w) + 0x4000)) >> 15);
    p = (const short *)(((const byte *) p) - n);
    i++;
  } while (i < (fft_size >> 1));
  p = (const short *)(sbfpfft_minus_cos_table);
  do
  {
    w = 0x4000 - (*p >> 1);
    fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) w) + 0x4000)) >> 15);
    p = (const short *)(((const byte *) p) + n);
    i++;
  } while (i < (fft_size >> 1) + (fft_size >> 2));
  p = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
  do
  {
    w = 0x4000 + (*p >> 1);
    fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) w) + 0x4000)) >> 15);
    p = (const short *)(((const byte *) p) - n);
    i++;
  } while (i < fft_size);
}


void sbfpfft_custom_window (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp, const short *neg_coefs, word coef_step)
{
  word i, j;
  short s;

  j = 0;
  i = 0;
  do
  {
    s = neg_coefs[j];
    fft_buffer[i].re = (short)(((int32)(0x4000 - ((int32)(fft_buffer[i].re)) * ((int32) s))) >> 15);
    fft_buffer[i].im = (short)(((int32)(0x4000 - ((int32)(fft_buffer[i].im)) * ((int32) s))) >> 15);
    j += coef_step;
    i++;
  } while (i < fft_size / 2);
  do
  {
    s = neg_coefs[j];
    fft_buffer[i].re = (short)(((int32)(0x4000 - ((int32)(fft_buffer[i].re)) * ((int32) s))) >> 15);
    fft_buffer[i].im = (short)(((int32)(0x4000 - ((int32)(fft_buffer[i].im)) * ((int32) s))) >> 15);
    j -= coef_step;
    i++;
  } while (i < fft_size);
}


#define SBFPFFT_EXP_ADJ_UPDATE(exp_adj,x)\
{ int exp_tmp = (x); (exp_adj) |= exp_tmp ^ (exp_tmp >> 15); }


void sbfpfft_execute (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp)
{
  word group_count, butterfly_spacing, twiddle_spacing, scale_shift, scale_round, i, j, k;
  short minus_sin, minus_cos, re1, im1, re2, im2;
  int exp_adj;
  const short *p_minus_sin, *p_minus_cos;
  t_complex_short *p1, *p2;

  group_count = 1;
  butterfly_spacing = fft_size * sizeof(t_complex_short);
  twiddle_spacing = (SBFPFFT_MAX_FFT_SIZE * sizeof(word)) >> (14 + 1 - sdsp_word_leading_zeroes (fft_size));
  exp_adj = 0x7fff >> -(-2);
  p1 = fft_buffer;
  for (i = 0; i < fft_size; i++)
  {
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, p1->re);
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, p1->im);
    p1++;
  }
  do
  {
    exp_adj = 1 - sdsp_word_leading_zeroes (exp_adj);
    scale_shift = 2 + exp_adj;
    *p_fft_exp += scale_shift;
    exp_adj = 0x7fff >> -(-2);
    butterfly_spacing >>= 1;
    p1 = fft_buffer;
    if (scale_shift == 0)
    {
      for (i = 0; i < group_count; i++)
      {
        p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
        re1 = p1->re;
        im1 = p1->im;
        re2 = p2->re;
        im2 = p2->im;
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = re1 + re2));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = im1 + im2));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->re = re1 - re2));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->im = im1 - im2));
        p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
      }
      if (twiddle_spacing < ((SBFPFFT_MAX_FFT_SIZE * sizeof(word)) >> 1))
      {
        p_minus_cos = (const short *) sbfpfft_minus_cos_table;
        p_minus_sin = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
        for (i = 1; i < butterfly_spacing / (2 * sizeof(t_complex_short)); i++)
        {
          p_minus_cos = (const short *)(((const byte *) p_minus_cos) + twiddle_spacing);
          p_minus_sin = (const short *)(((const byte *) p_minus_sin) - twiddle_spacing);
          minus_cos = *p_minus_cos;
          minus_sin = *p_minus_sin;
          p1 -= fft_size - 1;
          for (j = 0; j < group_count; j++)
          {
            p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
            re1 = p1->re;
            im1 = p1->im;
            re2 = p2->re;
            im2 = p2->im;
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = re1 + re2));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = im1 + im2));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->re = (short)(((int32)(((int32) minus_cos) * ((int32)(re2 - re1)) +
                        ((int32) minus_sin) * ((int32)(im2 - im1)) + 0x4000)) >> 15)));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->im = (short)(((int32)(((int32) minus_cos) * ((int32)(im2 - im1)) -
                        ((int32) minus_sin) * ((int32)(re2 - re1)) + 0x4000)) >> 15)));
            p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
          }
        }
        p1 -= fft_size - 1;
        for (i = 0; i < group_count; i++)
        {
          p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
          re1 = p1->re;
          im1 = p1->im;
          re2 = p2->re;
          im2 = p2->im;
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = re1 + re2));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = im1 + im2));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->re = im1 - im2));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->im = re2 - re1));
          p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
        }
        p_minus_cos = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
        p_minus_sin = (const short *) sbfpfft_minus_cos_table;
        for (i = 1; i < butterfly_spacing / (2 * sizeof(t_complex_short)); i++)
        {
          p_minus_cos = (const short *)(((const byte *) p_minus_cos) - twiddle_spacing);
          p_minus_sin = (const short *)(((const byte *) p_minus_sin) + twiddle_spacing);
          minus_cos = *p_minus_cos;
          minus_sin = *p_minus_sin;
          p1 -= fft_size - 1;
          for (j = 0; j < group_count; j++)
          {
            p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
            re1 = p1->re;
            im1 = p1->im;
            re2 = p2->re;
            im2 = p2->im;
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = re1 + re2));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = im1 + im2));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->re = (short)(((int32)(((int32) minus_cos) * ((int32)(re1 - re2)) -
                        ((int32) minus_sin) * ((int32)(im1 - im2)) + 0x4000)) >> 15)));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->im = (short)(((int32)(((int32) minus_cos) * ((int32)(im1 - im2)) +
                        ((int32) minus_sin) * ((int32)(re1 - re2)) + 0x4000)) >> 15)));
            p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
          }
        }
      }
    }
    else
    {
      scale_round = 1 << (scale_shift - 1);
      for (i = 0; i < group_count; i++)
      {
        p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
        re1 = p1->re;
        im1 = p1->im;
        re2 = p2->re;
        im2 = p2->im;
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = (re1 + re2 + scale_round) >> scale_shift));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = (im1 + im2 + scale_round) >> scale_shift));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->re = (re1 - re2 + scale_round) >> scale_shift));
        SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->im = (im1 - im2 + scale_round) >> scale_shift));
        p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
      }
      if (twiddle_spacing < ((SBFPFFT_MAX_FFT_SIZE * sizeof(word)) >> 1))
      {
        p_minus_cos = (const short *) sbfpfft_minus_cos_table;
        p_minus_sin = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
        for (i = 1; i < butterfly_spacing / (2 * sizeof(t_complex_short)); i++)
        {
          p_minus_cos = (const short *)(((const byte *) p_minus_cos) + twiddle_spacing);
          p_minus_sin = (const short *)(((const byte *) p_minus_sin) - twiddle_spacing);
          minus_cos = *p_minus_cos;
          minus_sin = *p_minus_sin;
          p1 -= fft_size - 1;
          for (j = 0; j < group_count; j++)
          {
            p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
            re1 = p1->re;
            im1 = p1->im;
            re2 = p2->re;
            im2 = p2->im;
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = (re1 + re2 + scale_round) >> scale_shift));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = (im1 + im2 + scale_round) >> scale_shift));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->re = (short)(((int32)(((int32) minus_cos) * ((int32)((re2 - re1 + scale_round) >> scale_shift)) +
                        ((int32) minus_sin) * ((int32)((im2 - im1 + scale_round) >> scale_shift)) + 0x4000)) >> 15)));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->im = (short)(((int32)(((int32) minus_cos) * ((int32)((im2 - im1 + scale_round) >> scale_shift)) -
                        ((int32) minus_sin) * ((int32)((re2 - re1 + scale_round) >> scale_shift)) + 0x4000)) >> 15)));
            p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
          }
        }
        p1 -= fft_size - 1;
        for (i = 0; i < group_count; i++)
        {
          p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
          re1 = p1->re;
          im1 = p1->im;
          re2 = p2->re;
          im2 = p2->im;
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = (re1 + re2 + scale_round) >> scale_shift));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = (im1 + im2 + scale_round) >> scale_shift));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->re = (im1 - im2 + scale_round) >> scale_shift));
          SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p2->im = (re2 - re1 + scale_round) >> scale_shift));
          p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
        }
        p_minus_cos = (const short *)(sbfpfft_minus_cos_table + SBFPFFT_MAX_FFT_SIZE / 4);
        p_minus_sin = (const short *) sbfpfft_minus_cos_table;
        for (i = 1; i < butterfly_spacing / (2 * sizeof(t_complex_short)); i++)
        {
          p_minus_cos = (const short *)(((const byte *) p_minus_cos) - twiddle_spacing);
          p_minus_sin = (const short *)(((const byte *) p_minus_sin) + twiddle_spacing);
          minus_cos = *p_minus_cos;
          minus_sin = *p_minus_sin;
          p1 -= fft_size - 1;
          for (j = 0; j < group_count; j++)
          {
            p2 = (t_complex_short *)(((byte *) p1) + butterfly_spacing);
            re1 = p1->re;
            im1 = p1->im;
            re2 = p2->re;
            im2 = p2->im;
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->re = (re1 + re2 + scale_round) >> scale_shift));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj, (p1->im = (im1 + im2 + scale_round) >> scale_shift));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->re = (short)(((int32)(((int32) minus_cos) * ((int32)((re1 - re2 + scale_round) >> scale_shift)) -
                        ((int32) minus_sin) * ((int32)((im1 - im2 + scale_round) >> scale_shift)) + 0x4000)) >> 15)));
            SBFPFFT_EXP_ADJ_UPDATE (exp_adj,
              (p2->im = (short)(((int32)(((int32) minus_cos) * ((int32)((im1 - im2 + scale_round) >> scale_shift)) +
                        ((int32) minus_sin) * ((int32)((re1 - re2 + scale_round) >> scale_shift)) + 0x4000)) >> 15)));
            p1 = (t_complex_short *)(((byte *) p2) + butterfly_spacing);
          }
        }
      }
    }
    group_count <<= 1;
    twiddle_spacing <<= 1;
  } while (butterfly_spacing != sizeof(t_complex_short));

  j = 0;
  for (i = 0; i < fft_size - 1; i++)
  {
    if (j > i)
    {
      re1 = fft_buffer[i].re;
      fft_buffer[i].re = fft_buffer[j].re;
      fft_buffer[j].re = re1;
      im1 = fft_buffer[i].im;
      fft_buffer[i].im = fft_buffer[j].im;
      fft_buffer[j].im = im1;
    }
    k = fft_size >> 1;
    while (j >= k)
    {
      j -= k;
      k >>= 1;
    }
    j += k;
  }
}


void sbfpfft_invert_scale (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp)
{
  word i;

  *p_fft_exp -= 15 - sdsp_word_leading_zeroes (fft_size);
  for (i = 0; i < fft_size; i++)
    fft_buffer[i].im = -fft_buffer[i].im;
}


void sbfpfft_denormalize (t_complex_short *fft_buffer, word fft_size, short fft_exp, short result_exp)
{
  word i, n;
  short s, lo, hi;

  if (fft_exp < result_exp)
  {
    n = (result_exp - fft_exp < 16) ? ((word) 0x8000) >> (result_exp - fft_exp) : 0;
    for (i = 0; i < fft_size; i++)
    {
      fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) n) + 0x4000)) >> 15);
      fft_buffer[i].im = (short)(((int32)(((int32)(fft_buffer[i].im)) * ((int32) n) + 0x4000)) >> 15);
    }
  }
  else if (fft_exp > result_exp)
  {
    n = fft_exp - result_exp;
    if (n >= 16)
      n = 15;
    hi = 0x7fff >> n;
    lo = ~hi;
    for (i = 0; i < fft_size; i++)
    {
      s = fft_buffer[i].re;
      fft_buffer[i].re = (s < lo) ? -32768 : ((s > hi) ? 32767 : s << n);
      s = fft_buffer[i].im;
      fft_buffer[i].im = (s < lo) ? -32768 : ((s > hi) ? 32767 : s << n);
    }
  }
}


void sbfpfft_sqr (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp)
{
  short exp_adj;
  word i;

  exp_adj = -1;
  for (i = 0; i <= (fft_size >> 1); i++)
  {
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].re);
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].im);
  }
  if (exp_adj == 0)
  {
    (*p_fft_exp)++;
    for (i = 0; i <= (fft_size >> 1); i++)
    {
      ((dword *) fft_buffer)[i] = ((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) >> 1;
    }
  }
  else
  {
    for (i = 0; i <= (fft_size >> 1); i++)
    {
      ((dword *) fft_buffer)[i] = ((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) << 1;
    }
  }
}


void sbfpfft_sqrt (t_complex_short *fft_buffer, word fft_size, short *p_fft_exp)
{
  short exp_adj;
  word i;

  fft_buffer[0].re >>= 1;
  fft_buffer[0].im >>= 1;
  fft_buffer[fft_size >> 1].re >>= 1;
  fft_buffer[fft_size >> 1].im >>= 1;
  exp_adj = -1;
  for (i = 0; i <= (fft_size >> 1); i++)
  {
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].re);
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].im);
  }
  i = 0;
  if (exp_adj == 0)
  {
    (*p_fft_exp)++;
    do
    {
      fft_buffer[i].re = sdsp_sqrt (((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) >> 1);
      fft_buffer[i].im = 0;
      i++;
    } while (i <= (fft_size >> 1));
  }
  else
  {
    do
    {
      fft_buffer[i].re = sdsp_sqrt (((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) << 1);
      fft_buffer[i].im = 0;
      i++;
    } while (i <= (fft_size >> 1));
  }
  while (i < fft_size)
  {
    fft_buffer[i].re = fft_buffer[fft_size - i].re;
    fft_buffer[i].im = 0;
    i++;
  }
}


#define SBFPFFT_LOG_TABLE_SIZE_EXP        4
#define SBFPFFT_LOG_TABLE_SIZE            ((1 << SBFPFFT_LOG_TABLE_SIZE_EXP) + 1)

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


void sbfpfft_log (t_complex_short *fft_buffer, word fft_size, short fft_exp, word threshold)
{
  dword d;
  int32 bias;
  word i, j, n;

  fft_buffer[0].re >>= 1;
  fft_buffer[0].im >>= 1;
  fft_buffer[fft_size >> 1].re >>= 1;
  fft_buffer[fft_size >> 1].im >>= 1;
  bias = 0x600 * fft_exp - ((int32) threshold);
  i = 0;
  do
  {
    d = ((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
      ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im));
    n = sdsp_dword_leading_zeroes (d);
    d <<= n;
    j = (word)((d << 1) >> (32 - SBFPFFT_LOG_TABLE_SIZE_EXP));
    n = 0x6300 - 0x300 * n + sbfpfft_log_table[j] +
      ((word)(((dword)(((dword)((d >> (15 - SBFPFFT_LOG_TABLE_SIZE_EXP)) & 0xffff)) *
      ((dword)(sbfpfft_log_table[j+1] - sbfpfft_log_table[j])))) >> 16));
    fft_buffer[i].re = (n + bias > 0) ? (word)((n + bias) >> 1) : 0;
    fft_buffer[i].im = 0;
    i++;
  } while (i <= (fft_size >> 1));
  while (i < fft_size)
  {
    fft_buffer[i].re = fft_buffer[fft_size - i].re;
    fft_buffer[i].im = 0;
    i++;
  }
}


short sbfpfft_magnitude (const t_complex_short *fft_buffer, word fft_size, short fft_exp, word *magnitude_buffer)
{
  short exp_adj;
  word i;

  exp_adj = -1;
  for (i = 0; i < (fft_size >> 1); i++)
  {
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].re);
    SBFPFFT_EXP_ADJ_UPDATE (exp_adj, fft_buffer[i].im);
  }
  if (exp_adj == 0)
  {
    for (i = 0; i < (fft_size >> 1); i++)
    {
      magnitude_buffer[i] = sdsp_sqrt (((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) >> 1);
    }
    return (fft_exp + 1);
  }
  else
  {
    for (i = 0; i < (fft_size >> 1); i++)
    {
      magnitude_buffer[i] = sdsp_sqrt (((dword)(((int32)(fft_buffer[i].re)) * ((int32)(fft_buffer[i].re)) +
        ((int32)(fft_buffer[i].im)) * ((int32)(fft_buffer[i].im)))) << 1);
    }
    return (fft_exp);
  }
}

void sbfpfft_norm (const t_complex_short *fft_buffer, word fft_size, t_complex_short *result_buffer)
{
  word i;

  for (i = 0; i < (fft_size >> 1); i++)
    sdsp_norm_vector (fft_buffer[i].re, fft_buffer[i].im, &(result_buffer[i].re), &(result_buffer[i].im));
}


void sbfpfft_denorm (t_complex_short *fft_buffer, word fft_size, const word *magnitude_buffer)
{
  word i, n;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    n = magnitude_buffer[i];
    fft_buffer[i].re = (short)(((int32)(((int32)(fft_buffer[i].re)) * ((int32) n) + 0x4000)) >> 15);
    fft_buffer[i].im = (short)(((int32)(((int32)(fft_buffer[i].im)) * ((int32) n) + 0x4000)) >> 15);
  }
}


void sbfpfft_mirror (t_complex_short *fft_buffer, word fft_size)
{
  word i;

  fft_buffer[fft_size >> 1].re = 0;
  fft_buffer[fft_size >> 1].im = 0;
  for (i = (fft_size >> 1) + 1; i < fft_size; i++)
  {
    fft_buffer[i].re = fft_buffer[fft_size - i].re;
    fft_buffer[i].im = -fft_buffer[fft_size - i].im;
  }
}


void sbfpfft_rotate_positive (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer)
{
  word i;
  short re, im, s, t;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    s = angle_buffer[i].re;
    t = angle_buffer[i].im;
    re = fft_buffer[i].re;
    im = fft_buffer[i].im;
    fft_buffer[i].re = (short)(((int32)(((int32) s) * ((int32) re) - ((int32) t) * ((int32) im) + 0x4000)) >> 15);
    fft_buffer[i].im = (short)(((int32)(((int32) s) * ((int32) im) + ((int32) t) * ((int32) re) + 0x4000)) >> 15);
  }
}


void sbfpfft_rotate_negative (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer)
{
  word i;
  short re, im, s, t;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    s = angle_buffer[i].re;
    t = angle_buffer[i].im;
    re = fft_buffer[i].re;
    im = fft_buffer[i].im;
    fft_buffer[i].re = (short)(((int32)(((int32) s) * ((int32) re) + ((int32) t) * ((int32) im) + 0x4000)) >> 15);
    fft_buffer[i].im = (short)(((int32)(((int32) s) * ((int32) im) - ((int32) t) * ((int32) re) + 0x4000)) >> 15);
  }
}


void sbfpfft_rotate_positive_norm (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer)
{
  word i, n;
  short re, im, r, s, t;
  int32 l;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    s = angle_buffer[i].re;
    t = angle_buffer[i].im;
    re = fft_buffer[i].re;
    im = fft_buffer[i].im;
    r = (short)(((int32)(((int32) s) * ((int32) re) - ((int32) t) * ((int32) im) + 0x4000)) >> 15);
    im = (short)(((int32)(((int32) s) * ((int32) im) + ((int32) t) * ((int32) re) + 0x4000)) >> 15);
    n = (word)(((dword)(((int32) r) * ((int32) r) + ((int32) im) * ((int32) im))) >> 16);
    n = 0xbfff - n;
    l = ((int32)(((int32) r) * ((int32) n))) >> 15;
    fft_buffer[i].re = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
    l = ((int32)(((int32) im) * ((int32) n))) >> 15;
    fft_buffer[i].im = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
  }
}


void sbfpfft_rotate_negative_norm (t_complex_short *fft_buffer, word fft_size, const t_complex_short *angle_buffer)
{
  word i, n;
  short re, im, r, s, t;
  int32 l;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    s = angle_buffer[i].re;
    t = angle_buffer[i].im;
    re = fft_buffer[i].re;
    im = fft_buffer[i].im;
    r = (short)(((int32)(((int32) s) * ((int32) re) + ((int32) t) * ((int32) im) + 0x4000)) >> 15);
    im = (short)(((int32)(((int32) s) * ((int32) im) - ((int32) t) * ((int32) re) + 0x4000)) >> 15);
    n = (word)(((dword)(((int32) r) * ((int32) r) + ((int32) im) * ((int32) im))) >> 16);
    n = 0xbfff - n;
    l = ((int32)(((int32) r) * ((int32) n))) >> 15;
    fft_buffer[i].re = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
    l = ((int32)(((int32) im) * ((int32) n))) >> 15;
    fft_buffer[i].im = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
  }
}


void sbfpfft_rotate_difference_norm (const t_complex_short *plus_angle_buffer, word fft_size,
  const t_complex_short *minus_angle_buffer, t_complex_short *result_buffer)
{
  word i, n;
  short re, im, r, s, t;
  int32 l;

  for (i = 0; i < (fft_size >> 1); i++)
  {
    s = plus_angle_buffer[i].re;
    t = plus_angle_buffer[i].im;
    re = minus_angle_buffer[i].re;
    im = minus_angle_buffer[i].im;
    r = (short)(((int32)(((int32) s) * ((int32) re) + ((int32) t) * ((int32) im) + 0x4000)) >> 15);
    im = (short)(((int32)(((int32) t) * ((int32) re) - ((int32) s) * ((int32) im) + 0x4000)) >> 15);
    n = (word)(((dword)(((int32) r) * ((int32) r) + ((int32) im) * ((int32) im))) >> 16);
    n = 0xbfff - n;
    l = ((int32)(((int32) r) * ((int32) n))) >> 15;
    result_buffer[i].re = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
    l = ((int32)(((int32) im) * ((int32) n))) >> 15;
    result_buffer[i].im = (l > 32767) ? 32767 : ((l < -32768) ? -32768 : (short) l);
  }
}

#endif /* SBFPFFT_ASM */
/*---------------------------------------------------------------------------*/
/*
int sbfpfft_constructor(void)
{
  diva_task_size_check();

  diva_task_alloc_array(sbfpfft_global_fft_buffer,SBFPFFT_MAX_FFT_SIZE);

  return (0);
}

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