/* ***** BEGIN LICENSE BLOCK *****  
 * Source last modified: $Id: sbrhfadj.c,v 1.3 2005/05/24 16:01:55 albertofloyd Exp $ 
 *   
 * Portions Copyright (c) 1995-2005 RealNetworks, Inc. All Rights Reserved.  
 *       
 * The contents of this file, and the files included with this file, 
 * are subject to the current version of the RealNetworks Public 
 * Source License (the "RPSL") available at 
 * http://www.helixcommunity.org/content/rpsl unless you have licensed 
 * the file under the current version of the RealNetworks Community 
 * Source License (the "RCSL") available at 
 * http://www.helixcommunity.org/content/rcsl, in which case the RCSL 
 * will apply. You may also obtain the license terms directly from 
 * RealNetworks.  You may not use this file except in compliance with 
 * the RPSL or, if you have a valid RCSL with RealNetworks applicable 
 * to this file, the RCSL.  Please see the applicable RPSL or RCSL for 
 * the rights, obligations and limitations governing use of the 
 * contents of the file. 
 *   
 * This file is part of the Helix DNA Technology. RealNetworks is the 
 * developer of the Original Code and owns the copyrights in the 
 * portions it created. 
 *   
 * This file, and the files included with this file, is distributed 
 * and made available on an 'AS IS' basis, WITHOUT WARRANTY OF ANY 
 * KIND, EITHER EXPRESS OR IMPLIED, AND REALNETWORKS HEREBY DISCLAIMS 
 * ALL SUCH WARRANTIES, INCLUDING WITHOUT LIMITATION, ANY WARRANTIES 
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, QUIET 
 * ENJOYMENT OR NON-INFRINGEMENT. 
 *  
 * Technology Compatibility Kit Test Suite(s) Location:  
 *    http://www.helixcommunity.org/content/tck  
 *  
 * Contributor(s):  
 *   
 * ***** END LICENSE BLOCK ***** */  

/**************************************************************************************
 * Fixed-point HE-AAC decoder
 * Jon Recker (jrecker@real.com)
 * February 2005
 *
 * sbrhfadj.c - high frequency adjustment for SBR
 **************************************************************************************/

#include "sbr.h"
#include "assembly.h"

/* invBandTab[i] = 1.0 / (i + 1), Q31 */
static const int invBandTab[64] PROGMEM = {
	0x7fffffff, 0x40000000, 0x2aaaaaab, 0x20000000, 0x1999999a, 0x15555555, 0x12492492, 0x10000000, 
	0x0e38e38e, 0x0ccccccd, 0x0ba2e8ba, 0x0aaaaaab, 0x09d89d8a, 0x09249249, 0x08888889, 0x08000000, 
	0x07878788, 0x071c71c7, 0x06bca1af, 0x06666666, 0x06186186, 0x05d1745d, 0x0590b216, 0x05555555, 
	0x051eb852, 0x04ec4ec5, 0x04bda12f, 0x04924925, 0x0469ee58, 0x04444444, 0x04210842, 0x04000000, 
	0x03e0f83e, 0x03c3c3c4, 0x03a83a84, 0x038e38e4, 0x03759f23, 0x035e50d8, 0x03483483, 0x03333333, 
	0x031f3832, 0x030c30c3, 0x02fa0be8, 0x02e8ba2f, 0x02d82d83, 0x02c8590b, 0x02b93105, 0x02aaaaab, 
	0x029cbc15, 0x028f5c29, 0x02828283, 0x02762762, 0x026a439f, 0x025ed098, 0x0253c825, 0x02492492, 
	0x023ee090, 0x0234f72c, 0x022b63cc, 0x02222222, 0x02192e2a, 0x02108421, 0x02082082, 0x02000000, 
};

/**************************************************************************************
 * Function:    EstimateEnvelope
 *
 * Description: estimate power of generated HF QMF bands in one time-domain envelope
 *                (4.6.18.7.3)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              index of current envelope
 *
 * Outputs:     power of each QMF subband, stored as integer (Q0) * 2^N, N >= 0
 *
 * Return:      none
 **************************************************************************************/
static void EstimateEnvelope(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int env)
{
	int i, m, iStart, iEnd, xre, xim, nScale, expMax;
	int p, n, mStart, mEnd, invFact, t;
	int *XBuf;
	U64 eCurr;
	unsigned char *freqBandTab;

	/* estimate current envelope */
	iStart = sbrGrid->envTimeBorder[env] + HF_ADJ;
	iEnd =   sbrGrid->envTimeBorder[env+1] + HF_ADJ;
	if (sbrGrid->freqRes[env]) {
		n = sbrFreq->nHigh;
		freqBandTab = sbrFreq->freqHigh;
	} else {
		n = sbrFreq->nLow;
		freqBandTab = sbrFreq->freqLow;
	}

	/* ADS should inline MADD64 (smlal) properly, but check to make sure */
	expMax = 0;
	if (sbrHdr->interpFreq) {
		for (m = 0; m < sbrFreq->numQMFBands; m++) {
			eCurr.w64 = 0;
			XBuf = psi->XBuf[iStart][sbrFreq->kStart + m];
			for (i = iStart; i < iEnd; i++) {
				/* scale to int before calculating power (precision not critical, and avoids overflow) */
				xre = (*XBuf) >> FBITS_OUT_QMFA;	XBuf += 1;
				xim = (*XBuf) >> FBITS_OUT_QMFA;	XBuf += (2*64 - 1);
				eCurr.w64 = MADD64(eCurr.w64, xre, xre);
				eCurr.w64 = MADD64(eCurr.w64, xim, xim);
			}

			/* eCurr.w64 is now Q(64 - 2*FBITS_OUT_QMFA) (64-bit word)
			 * if energy is too big to fit in 32-bit word (> 2^31) scale down by power of 2
			 */
			nScale = 0;
			if (eCurr.r.hi32) {
				nScale = (32 - CLZ(eCurr.r.hi32)) + 1;
				t  = (int)(eCurr.r.lo32 >> nScale);		/* logical (unsigned) >> */
				t |= eCurr.r.hi32 << (32 - nScale);
			} else if (eCurr.r.lo32 >> 31) {
				nScale = 1;
				t  = (int)(eCurr.r.lo32 >> nScale);		/* logical (unsigned) >> */
			} else {
				t  = (int)eCurr.r.lo32;
			}

			invFact = invBandTab[(iEnd - iStart)-1];
			psi->eCurr[m] = MULSHIFT32(t, invFact);
			psi->eCurrExp[m] = nScale + 1;	/* +1 for invFact = Q31 */
			if (psi->eCurrExp[m] > expMax)
				expMax = psi->eCurrExp[m];
		}
	} else {
		for (p = 0; p < n; p++) {
			mStart = freqBandTab[p];
			mEnd =   freqBandTab[p+1];
			eCurr.w64 = 0;
			for (i = iStart; i < iEnd; i++) {
				XBuf = psi->XBuf[i][mStart];
				for (m = mStart; m < mEnd; m++) {
					xre = (*XBuf++) >> FBITS_OUT_QMFA;
					xim = (*XBuf++) >> FBITS_OUT_QMFA;
					eCurr.w64 = MADD64(eCurr.w64, xre, xre);
					eCurr.w64 = MADD64(eCurr.w64, xim, xim);
				}
			}

			nScale = 0;
			if (eCurr.r.hi32) {
				nScale = (32 - CLZ(eCurr.r.hi32)) + 1;
				t  = (int)(eCurr.r.lo32 >> nScale);		/* logical (unsigned) >> */
				t |= eCurr.r.hi32 << (32 - nScale);
			} else if (eCurr.r.lo32 >> 31) {
				nScale = 1;
				t  = (int)(eCurr.r.lo32 >> nScale);		/* logical (unsigned) >> */
			} else {
				t  = (int)eCurr.r.lo32;
			}

			invFact = invBandTab[(iEnd - iStart)-1];
			invFact = MULSHIFT32(invBandTab[(mEnd - mStart)-1], invFact) << 1;
			t = MULSHIFT32(t, invFact);

			for (m = mStart; m < mEnd; m++) {
				psi->eCurr[m - sbrFreq->kStart] = t;
				psi->eCurrExp[m - sbrFreq->kStart] = nScale + 1;	/* +1 for invFact = Q31 */
			}
			if (psi->eCurrExp[mStart - sbrFreq->kStart] > expMax)
				expMax = psi->eCurrExp[mStart - sbrFreq->kStart];
		}
	}
	psi->eCurrExpMax = expMax;
}

/**************************************************************************************
 * Function:    GetSMapped
 *
 * Description: calculate SMapped (4.6.18.7.2)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current envelope
 *              index of current QMF band
 *              la flag for this envelope
 *
 * Outputs:     none
 *
 * Return:      1 if a sinusoid is present in this band, 0 if not
 **************************************************************************************/
static int GetSMapped(SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int band, int la)
{
	int bandStart, bandEnd, oddFlag, r;

	if (sbrGrid->freqRes[env]) {
		/* high resolution */
		bandStart = band;
		bandEnd = band+1;
	} else {
		/* low resolution (see CalcFreqLow() for mapping) */
		oddFlag = sbrFreq->nHigh & 0x01;
		bandStart = (band > 0 ? 2*band - oddFlag : 0);		/* starting index for freqLow[band] */
		bandEnd = 2*(band+1) - oddFlag;						/* ending index for freqLow[band+1] */
	}

	/* sMapped = 1 if sIndexMapped == 1 for any frequency in this band */
	for (band = bandStart; band < bandEnd; band++) {
		if (sbrChan->addHarmonic[1][band]) {
			r = ((sbrFreq->freqHigh[band+1] + sbrFreq->freqHigh[band]) >> 1);
			if (env >= la || sbrChan->addHarmonic[0][r] == 1)
				return 1;
		}
	}
	return 0;
}

#define GBOOST_MAX	0x2830afd3	/* Q28, 1.584893192 squared */
#define	ACC_SCALE	6

/* squared version of table in 4.6.18.7.5 */
static const int limGainTab[4] PROGMEM = {0x20138ca7, 0x40000000, 0x7fb27dce, 0x80000000};	/* Q30 (0x80000000 = sentinel for GMAX) */

/**************************************************************************************
 * Function:    CalcMaxGain
 *
 * Description: calculate max gain in one limiter band (4.6.18.7.5)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *              index of current envelope
 *              index of current limiter band
 *              number of fraction bits in dequantized envelope 
 *                (max = Q(FBITS_OUT_DQ_ENV - 6) = Q23, can go negative)
 *
 * Outputs:     updated gainMax, gainMaxFBits, and sumEOrigMapped in PSInfoSBR struct
 *
 * Return:      none
 **************************************************************************************/
static void CalcMaxGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int ch, int env, int lim, int fbitsDQ)
{
	int m, mStart, mEnd, q, z, r;
	int sumEOrigMapped, sumECurr, gainMax, eOMGainMax, envBand;
	unsigned char eCurrExpMax;
	unsigned char *freqBandTab;

	mStart = sbrFreq->freqLimiter[lim];   /* these are offsets from kStart */
	mEnd =   sbrFreq->freqLimiter[lim + 1];
	freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow);

	/* calculate max gain to apply to signal in this limiter band */
	sumECurr = 0;
	sumEOrigMapped = 0;
	eCurrExpMax = psi->eCurrExpMax;
	eOMGainMax = psi->eOMGainMax;
	envBand = psi->envBand;
	for (m = mStart; m < mEnd; m++) {
		/* map current QMF band to appropriate envelope band */
		if (m == freqBandTab[envBand + 1] - sbrFreq->kStart) {
			envBand++;
			eOMGainMax = psi->envDataDequant[ch][env][envBand] >> ACC_SCALE;	/* summing max 48 bands */
		}
		sumEOrigMapped += eOMGainMax;

		/* easy test for overflow on ARM */
		sumECurr += (psi->eCurr[m] >> (eCurrExpMax - psi->eCurrExp[m]));
		if (sumECurr >> 30) {
			sumECurr >>= 1;
			eCurrExpMax++;
		}
	}
	psi->eOMGainMax = eOMGainMax;
	psi->envBand = envBand;

	psi->gainMaxFBits = 30;	/* Q30 tables */
	if (sumECurr == 0) {
		/* any non-zero numerator * 1/EPS_0 is > G_MAX */
		gainMax = (sumEOrigMapped == 0 ? (int)limGainTab[sbrHdr->limiterGains] : (int)0x80000000);
	} else if (sumEOrigMapped == 0) {
		/* 1/(any non-zero denominator) * EPS_0 * limGainTab[x] is appx. 0 */
		gainMax = 0;
	} else {
		/* sumEOrigMapped = Q(fbitsDQ - ACC_SCALE), sumECurr = Q(-eCurrExpMax) */
		gainMax = limGainTab[sbrHdr->limiterGains];
		if (sbrHdr->limiterGains != 3) {
			q = MULSHIFT32(sumEOrigMapped, gainMax);	/* Q(fbitsDQ - ACC_SCALE - 2), gainMax = Q30  */
			z = CLZ(sumECurr) - 1;
			r = InvRNormalized(sumECurr << z);	/* in =  Q(z - eCurrExpMax), out = Q(29 + 31 - z + eCurrExpMax) */
			gainMax = MULSHIFT32(q, r);			/* Q(29 + 31 - z + eCurrExpMax + fbitsDQ - ACC_SCALE - 2 - 32) */
			psi->gainMaxFBits = 26 - z + eCurrExpMax + fbitsDQ - ACC_SCALE;
		}
	}
	psi->sumEOrigMapped = sumEOrigMapped;
	psi->gainMax = gainMax;
}

/**************************************************************************************
 * Function:    CalcNoiseDivFactors
 *
 * Description: calculate 1/(1+Q) and Q/(1+Q) (4.6.18.7.4; 4.6.18.7.5)
 *
 * Inputs:      dequantized noise floor scalefactor
 *
 * Outputs:     1/(1+Q) and Q/(1+Q), format = Q31
 *
 * Return:      none
 **************************************************************************************/
static void CalcNoiseDivFactors(int q, int *qp1Inv, int *qqp1Inv)
{
	int z, qp1, t, s;

	/* 1 + Q_orig */
	qp1  = (q >> 1);
	qp1 += (1 << (FBITS_OUT_DQ_NOISE - 1));		/* >> 1 to avoid overflow when adding 1.0 */
	z = CLZ(qp1) - 1;							/* z <= 31 - FBITS_OUT_DQ_NOISE */
	qp1 <<= z;									/* Q(FBITS_OUT_DQ_NOISE + z) = Q31 * 2^-(31 - (FBITS_OUT_DQ_NOISE + z)) */
	t = InvRNormalized(qp1) << 1;				/* Q30 * 2^(31 - (FBITS_OUT_DQ_NOISE + z)), guaranteed not to overflow */

	/* normalize to Q31 */
	s = (31 - (FBITS_OUT_DQ_NOISE - 1) - z - 1);	/* clearly z >= 0, z <= (30 - (FBITS_OUT_DQ_NOISE - 1)) */
	*qp1Inv = (t >> s);								/* s = [0, 31 - FBITS_OUT_DQ_NOISE] */
	*qqp1Inv = MULSHIFT32(t, q) << (32 - FBITS_OUT_DQ_NOISE - s);
}

/**************************************************************************************
 * Function:    CalcComponentGains
 *
 * Description: calculate gain of envelope, sinusoids, and noise in one limiter band
 *                (4.6.18.7.5)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *              index of current envelope
 *              index of current limiter band
 *              number of fraction bits in dequantized envelope
 *
 * Outputs:     gains for envelope, sinusoids and noise
 *              number of fraction bits for envelope gain
 *              sum of the total gain for each component in this band
 *              other updated state variables
 *
 * Return:      none
 **************************************************************************************/
static void CalcComponentGains(PSInfoSBR *psi, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env, int lim, int fbitsDQ)
{
	int d, m, mStart, mEnd, q, qm, noiseFloor, sIndexMapped;
	int shift, eCurr, maxFlag, gainMax, gainMaxFBits;
	int gain, sm, z, r, fbitsGain, gainScale;
	unsigned char *freqBandTab;

	mStart = sbrFreq->freqLimiter[lim];   /* these are offsets from kStart */
	mEnd =   sbrFreq->freqLimiter[lim + 1];

	gainMax = psi->gainMax;
	gainMaxFBits = psi->gainMaxFBits;

	d = (env == psi->la || env == sbrChan->laPrev ? 0 : 1);
	freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow);

	/* figure out which noise floor this envelope is in (only 1 or 2 noise floors allowed) */
	noiseFloor = 0;
	if (sbrGrid->numNoiseFloors == 2 && sbrGrid->noiseTimeBorder[1] <= sbrGrid->envTimeBorder[env])
		noiseFloor++;

	psi->sumECurrGLim = 0;
	psi->sumSM = 0;
	psi->sumQM = 0;
	/* calculate energy of noise to add in this limiter band */
	for (m = mStart; m < mEnd; m++) {
		if (m == sbrFreq->freqNoise[psi->noiseFloorBand + 1] - sbrFreq->kStart) {
			/* map current QMF band to appropriate noise floor band (NOTE: freqLimiter[0] == freqLow[0] = freqHigh[0]) */
			psi->noiseFloorBand++;
			CalcNoiseDivFactors(psi->noiseDataDequant[ch][noiseFloor][psi->noiseFloorBand], &(psi->qp1Inv), &(psi->qqp1Inv));
		}
		if (m == sbrFreq->freqHigh[psi->highBand + 1] - sbrFreq->kStart)
			psi->highBand++;
		if (m == freqBandTab[psi->sBand + 1] - sbrFreq->kStart) {
			psi->sBand++;
			psi->sMapped = GetSMapped(sbrGrid, sbrFreq, sbrChan, env, psi->sBand, psi->la);
		}

		/* get sIndexMapped for this QMF subband */
		sIndexMapped = 0;
		r = ((sbrFreq->freqHigh[psi->highBand+1] + sbrFreq->freqHigh[psi->highBand]) >> 1);
		if (m + sbrFreq->kStart == r) {
			/* r = center frequency, deltaStep = (env >= la || sIndexMapped'(r, numEnv'-1) == 1) */
			if (env >= psi->la || sbrChan->addHarmonic[0][r] == 1)
				sIndexMapped = sbrChan->addHarmonic[1][psi->highBand];
		}

		/* save sine flags from last envelope in this frame:
		 *   addHarmonic[0][0...63] = saved sine present flag from previous frame, for each QMF subband
		 *   addHarmonic[1][0...nHigh-1] = addHarmonic bit from current frame, for each high-res frequency band
		 * from MPEG reference code - slightly different from spec
		 *   (sIndexMapped'(m,LE'-1) can still be 0 when numEnv == psi->la)
		 */
		if (env == sbrGrid->numEnv - 1) {
			if (m + sbrFreq->kStart == r)
				sbrChan->addHarmonic[0][m + sbrFreq->kStart] = sbrChan->addHarmonic[1][psi->highBand];
			else
				sbrChan->addHarmonic[0][m + sbrFreq->kStart] = 0;
		}

		gain = psi->envDataDequant[ch][env][psi->sBand];
		qm = MULSHIFT32(gain, psi->qqp1Inv) << 1;
		sm = (sIndexMapped ? MULSHIFT32(gain, psi->qp1Inv) << 1 : 0);

		/* three cases: (sMapped == 0 && delta == 1), (sMapped == 0 && delta == 0), (sMapped == 1) */
		if (d == 1 && psi->sMapped == 0)
			gain = MULSHIFT32(psi->qp1Inv, gain) << 1;
		else if (psi->sMapped != 0)
			gain = MULSHIFT32(psi->qqp1Inv, gain) << 1;

		/* gain, qm, sm = Q(fbitsDQ), gainMax = Q(fbitsGainMax) */
		eCurr = psi->eCurr[m];
		if (eCurr) {
			z = CLZ(eCurr) - 1;
			r = InvRNormalized(eCurr << z);		/* in = Q(z - eCurrExp), out = Q(29 + 31 - z + eCurrExp) */
			gainScale = MULSHIFT32(gain, r);	/* out = Q(29 + 31 - z + eCurrExp + fbitsDQ - 32) */
			fbitsGain = 29 + 31 - z + psi->eCurrExp[m] + fbitsDQ - 32;
		} else {
			/* if eCurr == 0, then gain is unchanged (divide by EPS = 1) */
			gainScale = gain;
			fbitsGain = fbitsDQ;
		}

		/* see if gain for this band exceeds max gain */
		maxFlag = 0;
		if (gainMax != (int)0x80000000) {
			if (fbitsGain >= gainMaxFBits) {
				shift = MIN(fbitsGain - gainMaxFBits, 31);
				maxFlag = ((gainScale >> shift) > gainMax ? 1 : 0);
			} else {
				shift = MIN(gainMaxFBits - fbitsGain, 31);
				maxFlag = (gainScale > (gainMax >> shift) ? 1 : 0);
			}
		}

		if (maxFlag) {
			/* gainScale > gainMax, calculate ratio with 32/16 division */
			q = 0;
			r = gainScale;	/* guaranteed > 0, else maxFlag could not have been set */
			z = CLZ(r);
			if (z < 16) {
				q = 16 - z;
				r >>= q;	/* out = Q(fbitsGain - q) */
			}

			z = CLZ(gainMax) - 1;
			r = (gainMax << z) / r;		/* out = Q((fbitsGainMax + z) - (fbitsGain - q)) */
			q = (gainMaxFBits + z) - (fbitsGain - q);	/* r = Q(q) */
			if (q > 30) {
				r >>= MIN(q - 30, 31);
			} else {
				z = MIN(30 - q, 30);
				CLIP_2N_SHIFT30(r, z);	/* let r = Q30 since range = [0.0, 1.0) (clip to 0x3fffffff = 0.99999) */
			}

			qm = MULSHIFT32(qm, r) << 2;
			gain = MULSHIFT32(gain, r) << 2;
			psi->gLimBuf[m] = gainMax;
			psi->gLimFbits[m] = gainMaxFBits;
		} else {
			psi->gLimBuf[m] = gainScale;
			psi->gLimFbits[m] = fbitsGain;
		}

		/* sumSM, sumQM, sumECurrGLim = Q(fbitsDQ - ACC_SCALE) */
		psi->smBuf[m] = sm;
		psi->sumSM += (sm >> ACC_SCALE);

		psi->qmLimBuf[m] = qm;
		if (env != psi->la && env != sbrChan->laPrev && sm == 0)
			psi->sumQM += (qm >> ACC_SCALE);

		/* eCurr * gain^2 same as gain^2, before division by eCurr 
		 * (but note that gain != 0 even if eCurr == 0, since it's divided by eps)
		 */
		if (eCurr)
			psi->sumECurrGLim += (gain >> ACC_SCALE);
	}
}

/**************************************************************************************
 * Function:    ApplyBoost
 *
 * Description: calculate and apply boost factor for envelope, sinusoids, and noise 
 *                in this limiter band (4.6.18.7.5)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRFreq struct for this SCE/CPE block
 *              index of current limiter band
 *              number of fraction bits in dequantized envelope
 *
 * Outputs:     envelope gain, sinusoids and noise after scaling by gBoost
 *              format = Q(FBITS_GLIM_BOOST) for envelope gain,
 *                     = Q(FBITS_QLIM_BOOST) for noise
 *                     = Q(FBITS_OUT_QMFA) for sinusoids
 *
 * Return:      none
 *
 * Notes:       after scaling, each component has at least 1 GB
 **************************************************************************************/
static void ApplyBoost(PSInfoSBR *psi, SBRFreq *sbrFreq, int lim, int fbitsDQ)
{
	int m, mStart, mEnd, q, z, r;
	int sumEOrigMapped, gBoost;

	mStart = sbrFreq->freqLimiter[lim];   /* these are offsets from kStart */
	mEnd =   sbrFreq->freqLimiter[lim + 1];

	sumEOrigMapped = psi->sumEOrigMapped >> 1;
	r = (psi->sumECurrGLim >> 1) + (psi->sumSM >> 1) + (psi->sumQM >> 1);	/* 1 GB fine (sm and qm are mutually exclusive in acc) */
	if (r < (1 << (31-28))) {
		/* any non-zero numerator * 1/EPS_0 is > GBOOST_MAX 
		 * round very small r to zero to avoid scaling problems
		 */
		gBoost = (sumEOrigMapped == 0 ? (1 << 28) : GBOOST_MAX);
		z = 0;
	} else if (sumEOrigMapped == 0) {
		/* 1/(any non-zero denominator) * EPS_0 is appx. 0 */
		gBoost = 0;
		z = 0;
	} else {
		/* numerator (sumEOrigMapped) and denominator (r) have same Q format (before << z) */
		z = CLZ(r) - 1;	/* z = [0, 27] */
		r = InvRNormalized(r << z);
		gBoost = MULSHIFT32(sumEOrigMapped, r);
	}

	/* gBoost = Q(28 - z) */
	if (gBoost > (GBOOST_MAX >> z)) {
		gBoost = GBOOST_MAX;
		z = 0;
	}
	gBoost <<= z;	/* gBoost = Q28, minimum 1 GB */

	/* convert gain, noise, sinusoids to fixed Q format, clipping if necessary
	 *   (rare, usually only happens at very low bitrates, introduces slight
	 *    distortion into final HF mapping, but should be inaudible)
	 */
	for (m = mStart; m < mEnd; m++) {
		/* let gLimBoost = Q24, since in practice the max values are usually 16 to 20
		 *   unless limiterGains == 3 (limiter off) and eCurr ~= 0 (i.e. huge gain, but only
		 *   because the envelope has 0 power anyway)
		 */
		q = MULSHIFT32(psi->gLimBuf[m], gBoost) << 2;	/* Q(gLimFbits) * Q(28) --> Q(gLimFbits[m]-2) */
		r = SqrtFix(q, psi->gLimFbits[m] - 2, &z);
		z -= FBITS_GLIM_BOOST;
		if (z >= 0) {
			psi->gLimBoost[m] = r >> MIN(z, 31);
		} else {
			z = MIN(30, -z);
			CLIP_2N_SHIFT30(r, z);
			psi->gLimBoost[m] = r;
		}

		q = MULSHIFT32(psi->qmLimBuf[m], gBoost) << 2;	/* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */
		r = SqrtFix(q, fbitsDQ - 2, &z);
		z -= FBITS_QLIM_BOOST;		/* << by 14, since integer sqrt of x < 2^16, and we want to leave 1 GB */
		if (z >= 0) {
			psi->qmLimBoost[m] = r >> MIN(31, z);
		} else {
			z = MIN(30, -z);
			CLIP_2N_SHIFT30(r, z);
			psi->qmLimBoost[m] = r;
		}

		q = MULSHIFT32(psi->smBuf[m], gBoost) << 2;		/* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */
		r = SqrtFix(q, fbitsDQ - 2, &z);
		z -= FBITS_OUT_QMFA;		/* justify for adding to signal (xBuf) later */
		if (z >= 0) {
			psi->smBoost[m] = r >> MIN(31, z);
		} else {
			z = MIN(30, -z);
			CLIP_2N_SHIFT30(r, z);
			psi->smBoost[m] = r;
		}
	}
}

/**************************************************************************************
 * Function:    CalcGain
 *
 * Description: calculate and apply proper gain to HF components in one envelope 
 *                (4.6.18.7.5)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *              index of current envelope
 *
 * Outputs:     envelope gain, sinusoids and noise after scaling
 *
 * Return:      none
 **************************************************************************************/
static void CalcGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env)
{
	int lim, fbitsDQ;

	/* initialize to -1 so that mapping limiter bands to env/noise bands works right on first pass */
	psi->envBand        = -1;
	psi->noiseFloorBand = -1;
	psi->sBand          = -1;
	psi->highBand       = -1;

	fbitsDQ = (FBITS_OUT_DQ_ENV - psi->envDataDequantScale[ch][env]);	/* Q(29 - optional scalefactor) */
	for (lim = 0; lim < sbrFreq->nLimiter; lim++) {
		/* the QMF bands are divided into lim regions (consecutive, non-overlapping) */
		CalcMaxGain(psi, sbrHdr, sbrGrid, sbrFreq, ch, env, lim, fbitsDQ);
		CalcComponentGains(psi, sbrGrid, sbrFreq, sbrChan, ch, env, lim, fbitsDQ);
		ApplyBoost(psi, sbrFreq, lim, fbitsDQ);
	}
}

/* hSmooth table from 4.7.18.7.6, format = Q31 */
static const int hSmoothCoef[MAX_NUM_SMOOTH_COEFS] PROGMEM = {
	0x2aaaaaab, 0x2697a512, 0x1becfa68, 0x0ebdb043, 0x04130598, 
};

/**************************************************************************************
 * Function:    MapHF
 *
 * Description: map HF components to proper QMF bands, with optional gain smoothing
 *                filter (4.6.18.7.6)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current envelope
 *              reset flag (can be non-zero for first envelope only)
 *
 * Outputs:     complete reconstructed subband QMF samples for this envelope
 *
 * Return:      none
 * 
 * Notes:       ensures that output has >= MIN_GBITS_IN_QMFS guard bits,
 *                so it's not necessary to check anything in the synth QMF
 **************************************************************************************/
static void MapHF(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int hfReset)
{
	int noiseTabIndex, sinIndex, gainNoiseIndex, hSL;
	int i, iStart, iEnd, m, idx, j, s, n, smre, smim;
	int gFilt, qFilt, xre, xim, gbMask, gbIdx;
	int *XBuf;

	noiseTabIndex =   sbrChan->noiseTabIndex;
	sinIndex =        sbrChan->sinIndex;
	gainNoiseIndex =  sbrChan->gainNoiseIndex;	/* oldest entries in filter delay buffer */

	if (hfReset)
		noiseTabIndex = 2;	/* starts at 1, double since complex */
	hSL = (sbrHdr->smoothMode ? 0 : 4); 

	if (hfReset) {
		for (i = 0; i < hSL; i++) {
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m];
				sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m];
			}
			gainNoiseIndex++;
			if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS)
				gainNoiseIndex = 0;
		}
		ASSERT(env == 0);	/* should only be reset when env == 0 */
	}

	iStart = sbrGrid->envTimeBorder[env];
	iEnd =   sbrGrid->envTimeBorder[env+1];
	for (i = iStart; i < iEnd; i++) {
		/* save new values in temp buffers (delay)
		 * we only store MAX_NUM_SMOOTH_COEFS most recent values, 
		 *   so don't keep storing the same value over and over
		 */
		if (i - iStart < MAX_NUM_SMOOTH_COEFS) {
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m];
				sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m];
			}
		}

		/* see 4.6.18.7.6 */
		XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart];
		gbMask = 0;
		for (m = 0; m < sbrFreq->numQMFBands; m++) {
			if (env == psi->la || env == sbrChan->laPrev) {
				/* no smoothing filter for gain, and qFilt = 0 (only need to do once) */
				if (i == iStart) {
					psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m];
					psi->qFiltLast[m] = 0;
				}
			} else if (hSL == 0) {
				/* no smoothing filter for gain, (only need to do once) */
				if (i == iStart) {
					psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m];
					psi->qFiltLast[m] = sbrChan->qTemp[gainNoiseIndex][m];
				}
			} else {
				/* apply smoothing filter to gain and noise (after MAX_NUM_SMOOTH_COEFS, it's always the same) */
				if (i - iStart < MAX_NUM_SMOOTH_COEFS) {
					gFilt = 0;
					qFilt = 0;
					idx = gainNoiseIndex;
					for (j = 0; j < MAX_NUM_SMOOTH_COEFS; j++) {
						/* sum(abs(hSmoothCoef[j])) for all j < 1.0 */
						gFilt += MULSHIFT32(sbrChan->gTemp[idx][m], hSmoothCoef[j]);
						qFilt += MULSHIFT32(sbrChan->qTemp[idx][m], hSmoothCoef[j]);
						idx--;
						if (idx < 0)
							idx += MAX_NUM_SMOOTH_COEFS;
					}
					psi->gFiltLast[m] = gFilt << 1;	/* restore to Q(FBITS_GLIM_BOOST) (gain of filter < 1.0, so no overflow) */
					psi->qFiltLast[m] = qFilt << 1;	/* restore to Q(FBITS_QLIM_BOOST) */
				}
			}

			if (psi->smBoost[m] != 0) {
				/* add scaled signal and sinusoid, don't add noise (qFilt = 0) */
				smre = psi->smBoost[m];
				smim = smre;

				/* sinIndex:  [0] xre += sm   [1] xim += sm*s   [2] xre -= sm   [3] xim -= sm*s  */
				s = (sinIndex >> 1);	/* if 2 or 3, flip sign to subtract sm */
				s <<= 31;
				smre ^= (s >> 31);
				smre -= (s >> 31);
				s ^= ((m + sbrFreq->kStart) << 31);
				smim ^= (s >> 31);
				smim -= (s >> 31);

				/* if sinIndex == 0 or 2, smim = 0; if sinIndex == 1 or 3, smre = 0 */
				s = sinIndex << 31;
				smim &= (s >> 31);
				s ^= 0x80000000;
				smre &= (s >> 31);

				noiseTabIndex += 2;		/* noise filtered by 0, but still need to bump index */
			} else {
				/* add scaled signal and scaled noise */
				qFilt = psi->qFiltLast[m];	
				n = noiseTab[noiseTabIndex++];
				smre = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA);

				n = noiseTab[noiseTabIndex++];
				smim = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA);
			}
			noiseTabIndex &= 1023;	/* 512 complex numbers */

			gFilt = psi->gFiltLast[m];
			xre = MULSHIFT32(gFilt, XBuf[0]);
			xim = MULSHIFT32(gFilt, XBuf[1]);
			CLIP_2N_SHIFT30(xre, 32 - FBITS_GLIM_BOOST);
			CLIP_2N_SHIFT30(xim, 32 - FBITS_GLIM_BOOST);

			xre += smre;	*XBuf++ = xre;
			xim += smim;	*XBuf++ = xim;

			gbMask |= FASTABS(xre);
			gbMask |= FASTABS(xim);
		}
		/* update circular buffer index */
		gainNoiseIndex++;
		if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS)
			gainNoiseIndex = 0;

		sinIndex++;
		sinIndex &= 3;

		/* ensure MIN_GBITS_IN_QMFS guard bits in output
		 * almost never occurs in practice, but checking here makes synth QMF logic very simple
		 */
		if (gbMask >> (31 - MIN_GBITS_IN_QMFS)) {
			XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart];
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				xre = XBuf[0];	xim = XBuf[1];	
				CLIP_2N(xre, (31 - MIN_GBITS_IN_QMFS));	
				CLIP_2N(xim, (31 - MIN_GBITS_IN_QMFS));	
				*XBuf++ = xre;	*XBuf++ = xim;
			}	
			CLIP_2N(gbMask, (31 - MIN_GBITS_IN_QMFS));	
		}
		gbIdx = ((i + HF_ADJ) >> 5) & 0x01;
		sbrChan->gbMask[gbIdx] |= gbMask;
	}
	sbrChan->noiseTabIndex =  noiseTabIndex;
	sbrChan->sinIndex =       sinIndex;
	sbrChan->gainNoiseIndex = gainNoiseIndex;
}

/**************************************************************************************
 * Function:    AdjustHighFreq
 *
 * Description: adjust high frequencies and add noise and sinusoids (4.6.18.7)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *
 * Outputs:     complete reconstructed subband QMF samples for this channel
 *
 * Return:      none
 **************************************************************************************/
void AdjustHighFreq(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch)
{
	int i, env, hfReset;
	unsigned char frameClass, pointer;

	frameClass = sbrGrid->frameClass;
	pointer  = sbrGrid->pointer;

	/* derive la from table 4.159 */
	if ((frameClass == SBR_GRID_FIXVAR || frameClass == SBR_GRID_VARVAR) && pointer > 0)
		psi->la = sbrGrid->numEnv + 1 - pointer;
	else if (frameClass == SBR_GRID_VARFIX && pointer > 1)
		psi->la = pointer - 1;
	else
		psi->la = -1;

	/* for each envelope, estimate gain and adjust SBR QMF bands */
	hfReset = sbrChan->reset;
	for (env = 0; env < sbrGrid->numEnv; env++) {
		EstimateEnvelope(psi, sbrHdr, sbrGrid, sbrFreq, env);
		CalcGain(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, ch, env);
		MapHF(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, env, hfReset);
		hfReset = 0;	/* only set for first envelope after header reset */
	}

	/* set saved sine flags to 0 for QMF bands outside of current frequency range */
	for (i = 0; i < sbrFreq->freqLimiter[0] + sbrFreq->kStart; i++)
		sbrChan->addHarmonic[0][i] = 0;
	for (i = sbrFreq->freqLimiter[sbrFreq->nLimiter] + sbrFreq->kStart; i < 64; i++)
		sbrChan->addHarmonic[0][i] = 0;
	sbrChan->addHarmonicFlag[0] = sbrChan->addHarmonicFlag[1];

	/* save la for next frame */
	if (psi->la == sbrGrid->numEnv)
		sbrChan->laPrev = 0;
	else
		sbrChan->laPrev = -1;
}