//#ifdef HAVE_CONFIG_H
#include "config.h"
//#endif

#include "ssc_math.h"


unsigned isqrt32(unsigned int _val){
  unsigned b;
  unsigned g;
  int      bshift;

  g=0;
  bshift=(EC_ILOG(_val)-1)>>1;
  b=1U<<bshift;
  do{
    unsigned int t;
    t=(((unsigned int)g<<1)+b)<<bshift;
    if(t<=_val){
      g+=b;
      _val-=t;
    }
    b>>=1;
    bshift--;
  }
  while(bshift>=0);
  return g;
}

/** Reciprocal sqrt approximation in the range [0.25,1) (Q16 in, Q14 out) */
short audio_rsqrt_norm(int x)
{
   short n;
   short r;
   short r2;
   short y;
   /* Range of n is [-16384,32767] ([-0.5,1) in Q15). */
   n = x-32768;

   r = SSC_ADD16(23557, SSC_MULT16x16_Q15(n, SSC_ADD16(-13490, SSC_MULT16x16_Q15(n, 6713))));

   r2 = SSC_MULT16x16_Q15(r, r);
   y = SSC_SHL16(SSC_SUB16(SSC_ADD16(SSC_MULT16x16_Q15(r2, n), r2), 16384), 1);

   return SSC_ADD16(r, SSC_MULT16x16_Q15(r, SSC_MULT16x16_Q15(y,
              SSC_SUB16(SSC_MULT16x16_Q15(y, 12288), 16384))));
}

#define L1 32767
#define L2 -7651
#define L3 8277
#define L4 -626

static SSC_INLINE short _audio_cos_pi_2(short x)
{
   short x2;

   x2 = SSC_MULT16x16_P15(x,x);
   return SSC_ADD16(1,MIN16(32766,SSC_ADD32(SSC_SUB16(L1,x2), SSC_MULT16x16_P15(x2, SSC_ADD32(L2, SSC_MULT16x16_P15(x2, SSC_ADD32(L3, SSC_MULT16x16_P15(L4, x2
                                                                                ))))))));
}

#undef L1
#undef L2
#undef L3
#undef L4

short norm_cosine(int x)
{
	 return _audio_cos_pi_2(SSC_toSHORT(x));
}

/** Reciprocal approximation (Q15 input, Q16 output) */
int audio_rcp(int input_val)
{
   int log_integer;
   short norm;
   short mid_val;
   

   log_integer = EC_ILOG(input_val);
   /* norm is Q15 with range [0,1). */
   norm = SSC_VSHR32(input_val,log_integer-16)-32768;


   mid_val = (short)(SSC_MULT16x16_Q15(-15420, norm) + 30840);
   mid_val = (short)(mid_val - SSC_MULT16x16_Q15(mid_val, (short)((short)(mid_val-32768) + SSC_MULT16x16_Q15(mid_val, norm))));
   mid_val = (short)(mid_val- (short)(SSC_MULT16x16_Q15(mid_val,(short)(SSC_MULT16x16_Q15(mid_val, norm) + (short)(mid_val-32768))) + 1));


   return SSC_VSHR32((int)(mid_val), (log_integer-17) );

}