#include "math_op.h"

#include "basic_op.h"

#include "config.h"







#ifdef CRC_ON_8BIT

const unsigned char crc8_table[256] = 

{

	0, 7, 14, 9, 28, 27, 18, 21, 56, 63, 54, 49, 36, 35, 42, 45, 112, 119, 126, 121, 108, 

	107, 98, 101, 72, 79, 70, 65, 84, 83, 90, 93, 224, 231, 238, 233, 252, 251, 242, 245, 

	216, 223, 214, 209, 196, 195, 202, 205, 144, 151, 158, 153, 140, 139, 130, 133, 168, 

	175, 166, 161, 180, 179, 186, 189, 199, 192, 201, 206, 219, 220, 213, 210, 255, 248, 

	241, 246, 227, 228, 237, 234, 183, 176, 185, 190, 171, 172, 165, 162, 143, 136, 129, 

	134, 147, 148, 157, 154, 39, 32, 41, 46, 59, 60, 53, 50, 31, 24, 17, 22, 3, 4, 13, 10, 

	87, 80, 89, 94, 75, 76, 69, 66, 111, 104, 97, 102, 115, 116, 125, 122, 137, 142, 135, 

	128, 149, 146, 155, 156, 177, 182, 191, 184, 173, 170, 163, 164, 249, 254, 247, 240, 

	229, 226, 235, 236, 193, 198, 207, 200, 221, 218, 211, 212, 105, 110, 103, 96, 117, 

	114, 123, 124, 81, 86, 95, 88, 77, 74, 67, 68, 25, 30, 23, 16, 5, 2, 11, 12, 33, 38, 

	47, 40, 61, 58, 51, 52, 78, 73, 64, 71, 82, 85, 92, 91, 118, 113, 120, 127, 106, 109, 

	100, 99, 62, 57, 48, 55, 34, 37, 44, 43, 6, 1, 8, 15, 26, 29, 20, 19, 174, 169, 160, 

	167, 178, 181, 188, 187, 150, 145, 152, 159, 138, 141, 132, 131, 222, 217, 208, 215, 

	194, 197, 204, 203, 230, 225, 232, 239, 250, 253, 244, 243

};











unsigned char UpdateCRC8(unsigned char *pBuffer)

{



	short i;

	unsigned char crc = 0;



	for(i=1;i<27;i++)

	{

		crc = crc8_table[crc ^ pBuffer[i]];

	}

	crc &= 0xFF;

	return crc;



}

#ifndef HW_CODESIZE

short CheckCRC8(unsigned char *pBuffer)

{



	unsigned char crc = UpdateCRC8(pBuffer);



	if(crc != pBuffer[0])

	{

		return 2;

	} 

	return 0;

}

#endif

#endif









#ifndef HW_PITCH_OFF2



#ifndef HW_FRAC_DIV32_AIROHA

int frac_div32(int a, int b, int fixed_point)   // a = (a<<fixed_point), b= (b<<fixed_point)+1

{

	short rcp;

	int result, rem;

	int shift;



#ifndef AFTER_190328_OPTI

	if(b==0)

		b=1;

#endif





#ifndef HW_FOR_GCC

	shift = EC_ILOG(b)-(30-fixed_point);

#else

	asm volatile

		(

		"clz      %0,%1 \n\t"

		"RSB		%0,%0,#0x02 \n\t"

		:"=r"(shift)

		:"r"(b) 

		);



	shift = shift+fixed_point;

#endif

	b = SSC_VSHR(b,shift-fixed_point);   // use rMAC





#ifndef HW_CODESIZE

	rcp = ROUND16(speech_rcp(ROUND16(b,16)),3);

#else

	rcp = ROUND16((2147483647/ROUND16(b,16)),3);

#endif



	a = SSC_VSHR(a,shift-fixed_point);   // use rMAC

	result = SSC_MULT16x32_Q15(rcp, a);

#ifndef HW_CODESIZE

	rem = SSC_PSHR(a,2)-SSC_MULT32x32_Q31(result, b);

#else

	rem = SHR(a+2,2)-SSC_MULT32x32_Q31(result, b);

#endif

	result = SSC_ADD(result, SHL(SSC_MULT16x32_Q15(rcp, rem),2));







#ifndef ARMCC_CORTEX_ASSEM







///////////////////////////////////////////////////

	if (result >= 536870912)       /*  2^29 */

		return 2147483647;          /*  2^31 - 1 */

	else if (result <= -536870912) /* -2^29 */

		return -2147483647;         /* -2^31 */

	else

		return SHL(result, 2);

///////////////////////////////////////////////////







#else



#ifndef HW_FOR_GCC

	__ssat(result,30);

#else



//	result = ssat30(result);



	asm volatile

	(

		"ssat      %0,#30,%1 \n\t"

		:"=r"(result)

		:"r"(result) 

	);





#endif



	return SHL(result, 2);

#endif



}



#else



// frac_div32(best_xy,best_yy) with the condition  ==>    1) best_yy (b)>0,     2)best_yy (b)>best_xy (a)



short frac_div32(int a, int b)   // a = (a<<fixed_point), b= (b<<fixed_point)+1

{

	short result;

	short shift;

	



	unsigned int rMAC;

	shift = EC_ILOG(b) - 16;  // make b 16 bit value  b < (1<<16)

	b = SSC_VSHR(b,shift);   // b is maximum 1<<16

	a = SSC_VSHR(a,shift);   //  a<b, so a is maximum 1<<16



	



	rMAC = ((unsigned int)a)<<15;    // rMAC less than 1<<31,









	result = SSC_MULT32x32_Q31(rMAC, speech_rcp(b));

//	result = rMAC/b;

	

	return result;

}

#endif



#endif



#ifndef Merge_max_copy_opt_hifi3_ZH

#ifndef speech_maxabs16_opt_hifi3_ZH

short speech_maxabs16(const short *x, int len)

{

#ifndef HW_CODESIZE

   int i;

   short maxval = 0;

   short minval = 0;

   for (i=0;i<len;i++)

   {

      maxval = SPEECH_MAX(maxval, x[i]);

      minval = SPEECH_MIN(minval, x[i]);

   }

   return SPEECH_MAX(maxval,-minval);

#else

   int i;

   short maxval = 0;

   for (i=0;i<len;i++)

   {

	   maxval = SPEECH_MAX(maxval, ABS(x[i]));

   }



   return maxval;

#endif



}

#else

short speech_maxabs16(const short *x, int len)

{

	   int i;

	   short max[3], maxval;

	   ae_int16x4 maxv, tmpv;

	   ae_int16x4* ptx;

	   xtbool4 bool4;

	   maxv = AE_MOV16(0);



	   ptx = (ae_int16x4*)x;

	   for (i=0;i<len;i+=4 )

	   {

		   tmpv = AE_ABS16S(*ptx++);

		   bool4 = AE_LT16(maxv, tmpv);

		   AE_MOVT16X4(maxv, tmpv, bool4);

	   }

	   maxval = AE_MOVAD16_0(maxv);

	   max[0] = AE_MOVAD16_1(maxv);

	   max[1] = AE_MOVAD16_2(maxv);

	   max[2] = AE_MOVAD16_3(maxv);



	   for (i=0;i<3;i++)

	   {

		   maxval = SPEECH_MAX(maxval, max[i]);

	   }



	   return maxval;

}

#endif

#else

short speech_maxabs16_copy(const short *x, int len, int* ptin)

{

	   int i;

	   short max[3], maxval;

	   ae_int16x4 maxv, tmpv;

	   ae_int16x4* ptx, X;

	   ae_int32x2  tmpL1, tmpL2, *pty;



	   xtbool4 bool4;

	   maxv = AE_MOV16(0);



	   ptx = (ae_int16x4*)x;

	   pty = (ae_int32x2 *)(ptin);



	   for (i=0;i<len;i+=4 )

	   {

		   AE_L16X4_IP(X, ptx, 8);

		   tmpv = AE_ABS16S(X);

		   bool4 = AE_LT16(maxv, tmpv);

		   AE_MOVT16X4(maxv, tmpv, bool4);



			tmpL1 = AE_CVT32X2F16_32(X);

		    tmpL2 = AE_CVT32X2F16_10(X);

		    tmpL1 = AE_SRAI32(tmpL1, 12);

		    tmpL2 = AE_SRAI32(tmpL2, 12);

			AE_S32X2_IP(tmpL1, pty, 8);

			AE_S32X2_IP(tmpL2, pty, 8);



	   }

	   maxval = AE_MOVAD16_0(maxv);

	   max[0] = AE_MOVAD16_1(maxv);

	   max[1] = AE_MOVAD16_2(maxv);

	   max[2] = AE_MOVAD16_3(maxv);



	   for (i=0;i<3;i++)

	   {

		   maxval = SPEECH_MAX(maxval, max[i]);

	   }



	   return maxval;

}

#endif





#ifndef speech_maxabs32_opt_hifi3_ZH



#ifndef HW_PITCH_OFF2

int speech_maxabs32(const int *x, int len)

{

   int i;

#ifndef HW_HIFI3

   int maxval = 0;

   int minval = 0;

   for (i=0;i<len;i++)

   {

      maxval = SPEECH_MAX(maxval, x[i]);

      minval = SPEECH_MIN(minval, x[i]);

   }

   return SPEECH_MAX(maxval, -minval);

#else

   int maxval = 0;

   for (i=0;i<len;i++)

   {

	   maxval = SPEECH_MAX(maxval, ABS(x[i]));

   }



   return maxval;

#endif

}

#endif

#else

int speech_maxabs32(const int *x, int len)

{

   int i;

   ae_int32x2 maxv;

   ae_int32x2* ptx;

   int max1, max2;



   maxv = AE_MOV32(0);



   ptx = (ae_int32x2*)x;

   for (i=0;i<len;i+=2)

   {

	   maxv = AE_MAXABS32S(maxv, *ptx++);

   }

   max1 = AE_MOVAD32_H(maxv);

   max2 = AE_MOVAD32_L(maxv);



   return SPEECH_MAX(max1, max2);



}

#endif



/** Integer log in base2. Undefined for zero and negative numbers */



#ifndef HW_CODESIZE

short speech_ilog2(int x)

{

 //  return EC_ILOG(x)-1;



   return (30-AE_NSAZ32_L(x));



}

#endif





/** Base-2 logarithm approximation (log2(x)). (Q14 input, Q10 output) */

short speech_log2(int x)

{

   int i;

   short n, frac;







   static const short C[5] = {-6793, 15746, -5217, 2545, -1401};



#ifndef HW_CODESIZE

   if(x==0)

	   return -32767;

#endif



#ifndef HW_FOR_GCC

   i = EC_ILOG(x)+1;

#else

   asm volatile

	   (

	   "clz      %0,%1 \n\t"

	   "RSB		%0,%0,#0x21 \n\t"

	   :"=r"(i)

	   :"r"(x)

	   );

#endif

   n = SSC_VSHR(x,i-17)-49152;







   frac = SSC_ADD(C[0], SSC_MULT16x16_Q15(n, SSC_ADD(C[1], SSC_MULT16x16_Q15(n, SSC_ADD(C[2], SSC_MULT16x16_Q15(n, SSC_ADD(C[3], SSC_MULT16x16_Q15(n, C[4]))))))));

   return SHL(i-13,DB_SHIFT)+SHR(frac,4);



}







short speech_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_ADD(23557, SSC_MULT16x16_Q15(n, SSC_ADD(-13490, SSC_MULT16x16_Q15(n, 6713))));

   r2 = SSC_MULT16x16_Q15(r, r);

   y = SHL(SSC_SUB(SSC_ADD(SSC_MULT16x16_Q15(r2, n), r2), 16384), 1);

   return SSC_ADD(r, SSC_MULT16x16_Q15(r, SSC_MULT16x16_Q15(y,

              SSC_SUB(SSC_MULT16x16_Q15(y, 12288), 16384))));

}





/** Sqrt approximation (QX input, QX/2 output) */

int speech_sqrt(int x)

{

   int k;

   short n;

   int rt;

   static const short C[5] = {23175, 11561, -3011, 1699, -664};



#ifndef AFTER_190328_OPTI

   if (x==0)

      return 0;

   else if (x>=1073741824)

      return 32767;

#else

   if (x>=1073741824)

	   return 32767;

#endif





#ifndef HW_CODESIZE

   k = (speech_ilog2(x)>>1)-7;

#else



#ifndef VC_PROJ

   k = ((30-AE_NSAZ32_L(x))>>1)-7;

#else

    k = ((EC_ILOG(x)-1)>>1)-7;

#endif

#endif



   x = SSC_VSHR(x, (k<<1));

   n = x-32768;

   rt = SSC_ADD(C[0], SSC_MULT16x16_Q15(n, SSC_ADD(C[1], SSC_MULT16x16_Q15(n, SSC_ADD(C[2],

              SSC_MULT16x16_Q15(n, SSC_ADD(C[3], SSC_MULT16x16_Q15(n, (C[4])))))))));

   rt = SSC_VSHR(rt,7-k);

   return rt;

}



#ifndef AFTER_190327_OPTI

#define D0 16383

#define D1 22804

#define D2 14819

#define D3 10204

int speech_exp2_frac(short x)

{

   short frac;

   frac = SHL(x, 4);

   return SSC_ADD(D0, SSC_MULT16x16_Q15(frac, SSC_ADD(D1, SSC_MULT16x16_Q15(frac, SSC_ADD(D2 , SSC_MULT16x16_Q15(D3,frac))))));

}

#undef D0

#undef D1

#undef D2

#undef D3

#endif

/** Reciprocal approximation (Q15 input, Q16 output) */



#ifndef HW_CODESIZE

int speech_rcp(int x)

{



#ifndef HW_REMOVE_RCP_1

   int i;

   short n;

   short r;

   

   i = EC_ILOG(x);



   n = SSC_VSHR(x,i-16)-32768;

   r = SSC_ADD(30840, SSC_MULT16x16_Q15(-15420, n));

   r = SSC_SUB(r, SSC_MULT16x16_Q15(r,

             SSC_ADD(SSC_MULT16x16_Q15(r, n), SSC_ADD(r, -32768))));

   r = SSC_SUB(r, SSC_ADD(1, SSC_MULT16x16_Q15(r,

             SSC_ADD(SSC_MULT16x16_Q15(r, n), SSC_ADD(r, -32768)))));

   return SSC_VSHR(EXTEND32(r),i-17);

#else

   return (2147483647/x);

#endif

}

#endif



#ifndef OPT_KHW

int speech_div(int a, int b)

{

	return SSC_MULT32x32_Q31(a, speech_rcp(b));

}

#endif



#ifndef AFTER_190328_OPTI

#define L1 32767

#else

#define L1 32768

#endif



#define L2 -7651

#define L3 8277

#define L4 -626

short speech_cos_norm(short x)

{

	short x2;

#ifndef HW_ROUNDSKIP_MATH

	x2 = SSC_MULT16x16_P15(x,x);

	return SSC_ADD(1,SPEECH_MIN(32766,SSC_ADD(SSC_SUB(L1,x2), SSC_MULT16x16_P15(x2, SSC_ADD(L2, SSC_MULT16x16_P15(x2, SSC_ADD(L3, SSC_MULT16x16_P15(L4, x2

		))))))));

#else

#ifndef AFTER_190328_OPTI

	x2 = SSC_MULT16x16_Q15(x,x);

	return SSC_ADD(1,SPEECH_MIN(32766,SSC_ADD(SSC_SUB(L1,x2), SSC_MULT16x16_Q15(x2, SSC_ADD(L2, SSC_MULT16x16_Q15(x2, SSC_ADD(L3, SSC_MULT16x16_Q15(L4, x2

		))))))));

#else

	x2 = SSC_MULT16x16_Q15(x,x);

	return SPEECH_MIN(32767,SSC_ADD(SSC_SUB(L1,x2), SSC_MULT16x16_Q15(x2, SSC_ADD(L2, SSC_MULT16x16_Q15(x2, SSC_ADD(L3, SSC_MULT16x16_Q15(L4, x2

		)))))));

#endif

#endif

}

#undef L1

#undef L2

#undef L3

#undef L4