/*
 * Copyright (c) 2011-2019 Belledonne Communications SARL.
 *
 * This file is part of bcg729.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#ifndef G729FIXEDPOINTMATH_H
#define G729FIXEDPOINTMATH_H

/*****************************************************************************/
/*                                                                           */
/*  This library provides the following functions                            */
/*                                                                           */
/*       g729Log2_Q0Q16  : Logarithm base 2                                  */
/*       g729Exp2_Q11Q16 : Exponentiel base 2                                */
/*       g729Sqrt_Q0Q7   : Square Root                                       */
/*       g729Cos_Q13Q15  : Cosine                                            */
/*       g729Atan_Q15Q13 : Arc Tangent                                       */
/*       g729Asin_Q15Q13 : Arc Sine                                          */
/*       g729Acos_Q15Q13 : Arc Cosine                                        */
/*                                                                           */
/*       Extention QxxQyy stands for input in Qxx output in Qyy              */
/*                                                                           */
/*****************************************************************************/

#include "typedef.h"
#include "basicOperationsMacros.h"
#include "utils.h"

/* constants defined in Q16: actual values:
 KL0 = -2.059978
 KL1 = 5.770780
 KL2 = -3.847187 
 KL3 = 1.139907    
*/
#define KL0 -135003
#define KL1 378194
#define KL2 -252129
#define KL3 74705
/*****************************************************************************/
/* g729Log2_Q0Q16 : logarithm base 2, frac part computed from Taylor serie   */
/*    paremeters:                                                            */
/*      -(i) x : 32 bits integer in Q0, expected to be>0(not checked here)   */
/*    return value:                                                          */
/*      - the log2(x) in Q16 on 32 bits                                      */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word32_t g729Log2_Q0Q16(word32_t x)
{
	/* first get the integer part and put it in the 16 MSB of return value (in Q16) */
	uint16_t leadingZeros = countLeadingZeros(x); /* note: MSB is excluded as considered as sign bit */
	word32_t retValue = SHL32(SUB16(30,leadingZeros), 16);

	/* now shift the number to have it on this form 01XX XXXX XXXX XXXX, and keep only 16 bits -> consider it as a number in range [0.5, 1[ in Q0.15 */
	word16_t acc = (word16_t)VSHR32(x, 16-leadingZeros); 
	/* So calling int the integer part of the log2, we have:   */
	/* int = 30 - leadingZeros                                 */
	/* acc = x*2^(leadingZeros - 16)                           */
	/* acc = x*2^(14 - int)                                    */
	/* considering the content of acc as a Q15 number eq *2^-15*/
	/* acc = x*2^(14 -int)*2^-15                               */
	/* acc = x*2^(-1 -int)                                     */
	/* log2(acc) = log2(x) -1 - int                            */
	/* log2(x) ~= -3.059978 + 5.770780*x - 3.847187*x^2 + 1.139907*x^3 (for .5 < x < 1) Taylor Serie log2(x) at x near 0.75 */
	/* log2(x) + 1 = -2.059978 + x*(5.770780 +x(-3.847187 + 1.139907*x)) */
	/* with coeff in Q16 : */
	/* log2(acc) +1 = log2(x) - int =                           */
	/* log2(acc) +1 = -135003 +acc*(378194 +acc*(-252129 + acc*74705)) acc in Q15 and constants in Q16 -> final result will be log2(x) -int in Q16(on 32 bits) */
	word32_t acc32 = ADD32(KL0, MULT16_32_Q15(acc, ADD32(KL1, MULT16_32_Q15(acc, ADD32(KL2, MULT16_32_Q15(acc, KL3))))));
	
	return ADD32(retValue,acc32);
}

/* constants defined in Q15: actual values:
 E0 = 1
 E1 = log(2)
 E2 = 3-4*log(2)
 E3 = 3*log(2) - 2
*/
#define E0 16384
#define E1 11356
#define E2 3726
#define E3 1301
/*****************************************************************************/
/* g729Exp2_Q11Q16 : Exponentielle base 2                                    */
/*    paremeters:                                                            */
/*      -(i) x : 16 bits integer in Q11                                      */
/*    return value:                                                          */
/*      - exp2(x) in Q16 on 32 bits                                          */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word32_t g729Exp2_Q11Q16(word16_t x)
{
	int integer;
	word16_t frac;
	integer = SHR16(x,11);
	if (integer>14) {
		return 0x7fffffff;
	} else {
		if (integer < -15) {
			return 0;
		}
	}
	frac = SHL16(x-SHL16(integer,11),3);
	frac = ADD16(E0, MULT16_16_Q14(frac, ADD16(E1, MULT16_16_Q14(frac, ADD16(E2 , MULT16_16_Q14(E3,frac))))));
	return VSHR32(EXTEND32(frac), -integer-2);
}

/* constants in Q14 */
#define C0 3634
#define C1 21173
#define C2 -12627
#define C3 4204
/*****************************************************************************/
/* g729Sqrt_Q0Q7 : Square root                                               */
/*    paremeters:                                                            */
/*      -(i) x : 32 bits unsigned integer in Q0                              */
/*    return value:                                                          */
/*      - sqrt(x) in Q7 on 32 bits signed integer                            */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word32_t g729Sqrt_Q0Q7(uword32_t x)
{
	int k;
	word32_t rt;

	if (x==0) return 0;
	/* set x in Q14 in range [0.25,1[ */
	k = (19-unsignedCountLeadingZeros(x))>>1;
	x = VSHR32(x, k*2); /* x = x.2^-2k */

	/* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */
	/* consider x as in Q14: y = x.2^(-2k-14) -> and give sqrt(y).2^14 = sqrt(x).2^(-k-7).2^14 */
	rt = ADD16(C0, MULT16_16_Q14(x, ADD16(C1, MULT16_16_Q14(x, ADD16(C2, MULT16_16_Q14(x, (C3))))))); /* rt = sqrt(x).2^(7-k)*/ 
	rt = VSHR32(rt,-k); /* rt = sqrt(x).2^7 */
	return rt;
}

/*****************************************************************************/
/* g729InvSqrt_Q0Q31 : Inverse Square root(1/Sqrt(x)                         */
/*      x is not tested to be >=1, shall be done by caller function          */
/*    paremeters:                                                            */
/*      -(i) x : 32 bits integer in Q0 in range [1, MAXINT32]                */
/*    return value:                                                          */
/*      - 1/sqrt(x) in Q31 on 32 bits in range [43341/2^31, MAXINT32]        */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word32_t g729InvSqrt_Q0Q31(word32_t x)
{
	if (x==1) return MAXINT32;
	return (word32_t)(DIV32_32_Q24(g729Sqrt_Q0Q7(x),x)); /* sqrt(x) in Q7 + Q24 -> Q31 */
}

/* constants Q0.15 */
#define Kcos1 32768 
#define Kcos2 -16384
#define Kcos3 1365 
#define Kcos4 -46

#define Ksin1 32768 
#define Ksin2 -5461
#define Ksin3 273 
#define Ksin4 -7

/*****************************************************************************/
/* g729Cos_Q13Q15 : Cosine fonction in [0, Pi]                               */
/*      x is not tested to be in correct range                               */
/*    paremeters:                                                            */
/*      -(i) x : 16 bits integer in Q13 in range [0, Pi(25736)]              */
/*    return value:                                                          */
/*      - cos(x) in Q0.15 on 16 bits in range [-1, 1[                        */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word16_t g729Cos_Q13Q15(word16_t x)
{
	/* input var x in Q2.13 and in ]0, Pi[ */
	word16_t x2,xScaled; 
	if (x<12868) {
		if (x<6434) { /* x in ]0, Pi/4[ */
	   		x2 = MULT16_16_P11(x,x); /* in Q0.15 */
			return SATURATE(ADD32(Kcos1, MULT16_16_P15(x2, ADD32(Kcos2, MULT16_16_P15(x2, ADD32(Kcos3, MULT16_16_P15(Kcos4, x2)))))), MAXINT16); /* return cos x, must saturate if return value is +1 */
		} else {/* x in [Pi/4, Pi/2[ */ 
			x = SUB16(12868,x); /* x = pi/2 -x, x in [0, Pi/4] in Q0.13 */ 
   			x2 = MULT16_16_P11(x,x); /* in Q0.15 */
			return (MULT16_16_P13(x,ADD32(Ksin1, MULT16_16_P15(x2, ADD32(Ksin2, MULT16_16_P15(x2, ADD32(Ksin3, MULT16_16_P15(Ksin4, x2)))))))); /* return cos x as sin(pi/2 -x) */
		}
	} else { /* x in [Pi/2, Pi[ */
		xScaled = SUB16(25736,x); /* xScaled = Pi - x -> in [0,Pi/2] with cos(Pi-x) = -cos(x) and sin(Pi-x) =  */
		if (x<19302) { /* x in [Pi/2, 3Pi/4], xScaled in [Pi/4, Pi/2] */
			xScaled = SUB16(12868,xScaled); /* xScaled = pi/2 - xScaled = x - Pi/2, xScaled in [0, Pi/4] in Q0.13 */
			x2 = MULT16_16_P11(xScaled,xScaled); /* in Q0.15 */
			return (MULT16_16_P13(-xScaled,ADD32(Ksin1, MULT16_16_P15(x2, ADD32(Ksin2, MULT16_16_P15(x2, ADD32(Ksin3, MULT16_16_P15(Ksin4, x2)))))))); /* return cos x as -sin(x - Pi/2) */
		} else { /* x in [3Pi/4, Pi[ -> xScaled in [0, Pi/4], cos(xScaled) = -cos(x) */
			x2 = MULT16_16_P11(xScaled,xScaled); /* in Q0.15 */
			return (SUB32(-Kcos1, MULT16_16_P15(x2, ADD32(Kcos2, MULT16_16_P15(x2, ADD32(Kcos3, MULT16_16_P15(Kcos4, x2))))))); /* return cos x as -cos(Pi -x) */
		}

	}
}
/* KPI6 = pi/6 in Q15 */
#define KPI6 17157 
/* KtanPI6 = tan(pi/6) in Q15 */
#define KtanPI6 18919 
/* KtanPI12 = tan(pi/12) in Q15 */
#define KtanPI12 8780

/* B = 0.257977658811405 in Q15 */
#define atanB 8453
/* C = 0.59120450521312 in Q15 */
#define atanC 19373 

/*****************************************************************************/
/* g729Atan_Q15Q13: ArcTangent fonction in [-2^16, 2^16[                     */
/*    paremeters:                                                            */
/*      -(i) x : 32 bits integer in Q15 in range [-2^16, 2^16[               */
/*    return value:                                                          */
/*      - atan(x) in Q2.13 on 16 bits in range ]-Pi/2(12868), Pi/2(12868)[   */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word16_t g729Atan_Q15Q13(word32_t x)
{
	/* constants for rational polynomial */
	word32_t angle;
	word16_t x2;
	int highSegment = 0;
	int sign = 0;
	int complement = 0;

	/* make argument positive */
	if (x < 0) {
		x = NEG16(x);
		sign = 1;
	}
	
	/* limit argument to 0..1 */
	if(x > ONE_IN_Q15){
		complement = 1;
		x = DIV32(ONE_IN_Q30, x); /* 1/x in Q15 */
	}

	/* determine segmentation */
	if(x > KtanPI12){
		highSegment = 1;
		/* x = (x - k)/(1 + k*x); */
		x = DIV32(SSHL(SUB32(x, KtanPI6), 15), ADD32(MULT16_16_Q15(KtanPI6, x), ONE_IN_Q15));
	}

	/* argument is now < tan(15 degrees) */
	/* approximate the function */
	x2 = MULT16_16_Q15(x,x);
	angle = DIV32(MULT16_16(x, ADD32(ONE_IN_Q15, MULT16_16_Q15(atanB, x2))), ADD32(ONE_IN_Q15, MULT16_16_Q15(atanC, x2)));  /* ang = x*(1 + B*x2)/(1 + C*x2) */

	/* now restore offset if needed */
	if(highSegment) {
		angle += KPI6;
	}

	/* restore complement if needed */
	if(complement) {
		angle = SUB32(HALF_PI_Q15, angle);
	}

	/* set result in Q13 */
	angle = PSHR(angle, 2);

	/* restore sign if needed */
	if(sign) {
		return NEG16(angle);
	} else {
		return angle;
	}
}



/*****************************************************************************/
/* g729Asin_Q15Q13: ArcSine fonction                                         */
/*    paremeters:                                                            */
/*      -(i) x : 16 bits integer in Q15 in range ]-1, 1[                     */
/*    return value:                                                          */
/*      - asin(x) in Q2.13 on 16 bits in range ]-Pi/2(12868), Pi/2(12868)[   */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word16_t g729Asin_Q15Q13(word16_t x)
{
	return g729Atan_Q15Q13(DIV32(SSHL(x,15), PSHR(g729Sqrt_Q0Q7(SUB32(ONE_IN_Q30, MULT16_16(x,x))),7))); /*  atan(x/sqrt(1.0 - x*x)) */
}

/*****************************************************************************/
/* g729Acos_Q15Q13: ArcCosine fonction                                       */
/*    paremeters:                                                            */
/*      -(i) x : 16 bits integer in Q15 in range ]-1, 1[                     */
/*    return value:                                                          */
/*      - acos(x) in Q2.13 on 16 bits in range ]0, Pi(25736)[                */
/*                                                                           */
/*****************************************************************************/
static BCG729_INLINE word16_t g729Acos_Q15Q13(word16_t x)
{
	return(HALF_PI_Q13 - g729Asin_Q15Q13(x));
}

#endif /* ifndef G729FIXEDPOINTMATH_H */
