*
 *	 PURPOSE:
 *
 *	   Compute the square root of x.
 *
 *
 *	 INPUTS:
 *
 *	   x
 *					   16 bit short signed integer (Shortword).
 *	   Q
 *					   16 bit short signed integer (Shortword) represents
 *					   Q value of x.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   temp
 *					   16 bit short signed integer (Shortword) in same Q.
 *
 *************************************************************************/
Shortword sqrt_fxp(Shortword x, Shortword Q)
{
	Shortword	temp;


	if (!x)
		return((Shortword) 0);

	temp = shr(log10_fxp(x, Q), 1);                        /* temp is now Q12 */
	temp = pow10_fxp(temp, Q);
	return(temp);
}


/***************************************************************************
 *
 *	 FUNCTION NAME: L_sqrt_fxp
 *
 *	 PURPOSE:
 *
 *	   Compute the square root of x.
 *
 *
 *	 INPUTS:
 *
 *	   x
 *					   32 bit long signed integer (Longword).
 *	   Q
 *					   16 bit short signed integer (Shortword) represents
 *					   Q value of x.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   temp
 *					   16 bit short signed integer (Shortword) in same Q.
 *
 *************************************************************************/
Shortword L_sqrt_fxp(Longword x, Shortword Q)
{
	Shortword	temp;


	if (!x)
		return((Shortword) 0);

	temp = L_log10_fxp(x, Q);
	/* temp in Q11, pow10 treat it as Q12, => no need to shr by 1. */
	temp = pow10_fxp(temp, Q);

	return(temp);
}


/***************************************************************************
 *
 *	 FUNCTION NAME: L_pow_fxp
 *
 *	 PURPOSE:
 *
 *	   Compute the value of x raised to the power 'power'.
 *
 *
 *	 INPUTS:
 *
 *	   x
 *					   32 bit long signed integer (Longword).
 *	   power
 *					   16 bit short signed integer (Shortword) in Q15.
 *	   Q_in
 *					   16 bit short signed integer (Shortword) represents
 *					   Q value of x.
 *	   Q_out
 *					   16 bit short signed integer (Shortword) represents
 *					   required Q value of returned result.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   temp
 *					   16 bit short signed integer (Shortword).
 *
 *************************************************************************/
Shortword L_pow_fxp(Longword x, Shortword power, Shortword Q_in,
					Shortword Q_out)
{
	Shortword	temp;


	if (!x)
		return((Shortword) 0);

	temp = L_log10_fxp(x, Q_in);                               /* temp in Q11 */
	temp = mult(power, shl(temp, 1));                          /* temp in Q12 */
	temp = pow10_fxp(temp, Q_out);
	return(temp);
}


/***************************************************************************
 *
 *	 FUNCTION NAME: sin_fxp
 *
 *	 PURPOSE:
 *
 *	   Compute the sine of x whose value is expressed in radians/PI.
 *
 *
 *	 INPUTS:
 *
 *	   x
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   ty
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *************************************************************************/

Shortword sin_fxp(Shortword x)
{
	static const Shortword	table[129] = {
			0,	 402,	804,  1206,  1608,	2009,  2411,  2811,  3212,
		 3612,	4011,  4410,  4808,  5205,	5602,  5998,  6393,  6787,
		 7180,	7571,  7962,  8351,  8740,	9127,  9512,  9896, 10279,
		10660, 11039, 11417, 11793, 12167, 12540, 12910, 13279, 13646,
		14010, 14373, 14733, 15091, 15447, 15800, 16151, 16500, 16846,
		17190, 17531, 17869, 18205, 18538, 18868, 19195, 19520, 19841,
		20160, 20475, 20788, 21097, 21403, 21706, 22006, 22302, 22595,
		22884, 23170, 23453, 23732, 24008, 24279, 24548, 24812, 25073,
		25330, 25583, 25833, 26078, 26320, 26557, 26791, 27020, 27246,
		27467, 27684, 27897, 28106, 28311, 28511, 28707, 28899, 29086,
		29269, 29448, 29622, 29792, 29957, 30118, 30274, 30425, 30572,
		30715, 30853, 30986, 31114, 31238, 31357, 31471, 31581, 31686,
		31786, 31881, 31972, 32058, 32138, 32214, 32286, 32352, 32413,
		32470, 32522, 32568, 32610, 32647, 32679, 32706, 32729, 32746,
		32758, 32766, 32767
	};
	Shortword	tx, ty;
	Shortword	sign;
	Shortword	index1, index2;
	Shortword	m;
	Shortword	temp;


	sign = 0;
	if (x < 0){
		tx = negate(x);
		sign = -1;
	} else {
		tx = x;
	}

	/* if angle > pi/2, sin(angle) = sin(pi-angle) */
	if (tx > X05_Q15){
		tx = sub(ONE_Q15, tx);
	}
	/* convert input to be within range 0-128 */
	index1 = shr(tx, 7);
	index2 = add(index1, 1);

	if (index1 == 128){
		if (sign != 0)
			return(negate(table[index1]));
		else
			return(table[index1]);
	}

	m = sub(tx, shl(index1, 7));
	/* convert decimal part to Q15 */
	m = shl(m, 8);

	/* interpolate */
	temp = sub(table[index2], table[index1]);
	temp = mult(m, temp);
	ty = add(table[index1], temp);

	if (sign != 0)
		return(negate(ty));
	else
		return(ty);
}


/***************************************************************************
 *
 *	 FUNCTION NAME: cos_fxp
 *
 *	 PURPOSE:
 *
 *	   Compute the cosine of x whose value is expressed in radians/PI.
 *
 *
 *	 INPUTS:
 *
 *	   x
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   ty
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *************************************************************************/
Shortword cos_fxp(Shortword x)
{
	static const Shortword	table[129] = {
		32767, 32766, 32758, 32746, 32729, 32706, 32679, 32647, 32610,
		32568, 32522, 32470, 32413, 32352, 32286, 32214, 32138, 32058,
		31972, 31881, 31786, 31686, 31581, 31471, 31357, 31238, 31114,
		30986, 30853, 30715, 30572, 30425, 30274, 30118, 29957, 29792,
		29622, 29448, 29269, 29086, 28899, 28707, 28511, 28311, 28106,
		27897, 27684, 27467, 27246, 27020, 26791, 26557, 26320, 26078,
		25833, 25583, 25330, 25073, 24812, 24548, 24279, 24008, 23732,
		23453, 23170, 22884, 22595, 22302, 22006, 21706, 21403, 21097,
		20788, 20475, 20160, 19841, 19520, 19195, 18868, 18538, 18205,
		17869, 17531, 17190, 16846, 16500, 16151, 15800, 15447, 15091,
		14733, 14373, 14010, 13646, 13279, 12910, 12540, 12167, 11793,
		11417, 11039, 10660, 10279,  9896,	9512,  9127,  8740,  8351,
		7962,	7571,  7180,  6787,  6393,	5998,  5602,  5205,  4808,
		4410,	4011,  3612,  3212,  2811,	2411,  2009,  1608,  1206,
		804,	 402,	  0
	};
	Shortword	tx, ty;
	Shortword	sign;
	Shortword	index1, index2;
	Shortword	m;
	Shortword	temp;


	sign = 0;
	if (x < 0){
		tx = negate(x);
	} else {
		tx = x;
	}

	/* if angle > pi/2, cos(angle) = -cos(pi-angle) */
	if (tx > X05_Q15){
		tx = sub(ONE_Q15, tx);
		sign = -1;
	}
	/* convert input to be within range 0-128 */
	index1 = shr(tx, 7);
	index2 = add(index1, 1);

	if (index1 == 128)
		return((Shortword) 0);

	m = sub(tx, shl(index1, 7));
	/* convert decimal part to Q15 */
	m = shl(m, 8);

	temp = sub(table[index2], table[index1]);
	temp = mult(m, temp);
	ty = add(table[index1], temp);

	if (sign != 0)
		return(negate(ty));
	else
		return(ty);
}

/***************************************************************************
 *
 *	 FUNCTION NAME: sqrt_Q15
 *
 *	 PURPOSE:
 *
 *	   Compute the Square Root of x Q15
 *		Based on 6-term Taylor-series
 *
 *	 INPUTS:
 *
 *	   x
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *	 OUTPUTS:
 *
 *	   none
 *
 *	 RETURN VALUE:
 *
 *	   A
 *					   16 bit short signed integer (Shortword) in Q15.
 *
 *************************************************************************/

/*************************************************************************/
/*  We calculate sqrt(y) = sqrt(x+1) :                                   */
/*  Approximated by:                                                     */
/*  1 + (x/2) - 0.5(x/2)^2 + 0.5(x/2)^3 - 0.625(x/2)^4 + 0.875(x/2)^5    */
/*  before this operation, y is normalized to [0.5; 1[                   */
/*************************************************************************/

Shortword sqrt_Q15(Shortword x)
{
	Shortword	shift, odd;
	Shortword	temp, x_2, x_24;
	Longword	L_temp, L_temp2;
	Longword	L_A;

	if (x==0) return 0;		/* return zero */
	L_A		= L_deposit_h(x);
	shift	= norm_l(L_A);
	L_A		= L_shl(L_A, shift); /* Normalize */
	odd		= 0;
	if (shift & 0x1) odd = 1; /* Odd exponent */
	shift	= shl(shift, -1);
	shift	= negate(shift);
	L_A		= L_shl(L_A, -1);
	L_A		= L_sub(L_A, L_deposit_h(0x4000));
	temp	= extract_h(L_A);
	x_2		= temp;
	L_A		= L_add(L_A, L_deposit_h(0x4000));
	L_A		= L_add(L_A, L_deposit_h(0x4000));
	L_temp	= L_mult(temp, temp);
 	L_temp	= -L_temp;
	L_temp2	= L_shl(L_temp, -1);
	L_A		= L_add(L_A, L_temp2);
	L_temp	= L_mult((Shortword) L_shl(L_temp, -16), (Shortword) L_shl(L_temp, -16));
	x_24	= extract_h(L_temp);
	L_temp	= L_mult(x_24, 0x5000);
	L_A		= L_sub(L_A, L_temp);
	temp	= mult(x_24, x_2);
	L_A		= L_add(L_A, L_mult(0x7000,temp));
	temp	= mult(x_2, x_2);
	L_temp2	= L_mult(temp, x_2);
	L_A		= L_add(L_A, L_shl(L_temp2, -1));
	L_A		= L_add(L_A, L_shl(0x80,8));
	if (odd){ /* Adjust, if odd exponent! */
		L_A		= L_mult((Shortword) L_shl(L_A, -16), 0x5A82); /* L_A*=1/sqrt(2) */
		L_A		= L_add(L_A, L_shl(0x80,8)); /* Round */
	}
	L_A		= L_shl(L_A,shift);
	temp	= extract_h(L_A);
	return	temp;
}


Shortword add_shr(Shortword Var1, Shortword Var2)
{
	Shortword	temp;
	Longword	L_1, L_2;


	L_1		= L_deposit_l(Var1);
	L_2		= L_deposit_l(Var2);
	temp	= (Shortword) L_shr(L_add(L_1, L_2),1);
	return temp;
}


Shortword sub_shr(Shortword Var1, Shortword Var2)
{
	Shortword	temp;
	Longword	L_1, L_2;


	L_1		= L_deposit_l(Var1);
	L_2		= L_deposit_l(Var2);
	temp	= (Shortword) L_shr(L_sub(L_1, L_2), 1);
	return temp;
}