/* ***** BEGIN LICENSE BLOCK *****  
 * Source last modified: $Id: sbrfreq.c,v 1.2 2005/05/20 18:05:41 jrecker 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
 *
 * sbrfreq.c - frequency band table calculation for SBR
 **************************************************************************************/

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

/**************************************************************************************
 * Function:    BubbleSort
 *
 * Description: in-place sort of unsigned chars
 *
 * Inputs:      buffer of elements to sort
 *              number of elements to sort
 *
 * Outputs:     sorted buffer
 *
 * Return:      none
 **************************************************************************************/
static void BubbleSort(unsigned char *v, int nItems)
{
	int i;
	unsigned char t;

	while (nItems >= 2) {
		for (i = 0; i < nItems-1; i++) {
			if (v[i+1] < v[i]) {
				t = v[i+1];	
				v[i+1] = v[i];	
				v[i] = t;
			}
		}
		nItems--;
	}
}

/**************************************************************************************
 * Function:    VMin
 *
 * Description: find smallest element in a buffer of unsigned chars
 *
 * Inputs:      buffer of elements to search
 *              number of elements to search
 *
 * Outputs:     none
 *
 * Return:      smallest element in buffer
 **************************************************************************************/
static unsigned char VMin(unsigned char *v, int nItems)
{
	int i;
	unsigned char vMin;

	vMin = v[0];
	for (i = 1; i < nItems; i++) {
		if (v[i] < vMin)
			vMin = v[i];
	}
	return vMin;
}

/**************************************************************************************
 * Function:    VMax
 *
 * Description: find largest element in a buffer of unsigned chars
 *
 * Inputs:      buffer of elements to search
 *              number of elements to search
 *
 * Outputs:     none
 *
 * Return:      largest element in buffer
 **************************************************************************************/
static unsigned char VMax(unsigned char *v, int nItems)
{
	int i;
	unsigned char vMax;

	vMax = v[0];
	for (i = 1; i < nItems; i++) {
		if (v[i] > vMax)
			vMax = v[i];
	}
	return vMax;
}

/**************************************************************************************
 * Function:    CalcFreqMasterScaleZero
 *
 * Description: calculate master frequency table when freqScale == 0 
 *                (4.6.18.3.2.1, figure 4.39)
 *
 * Inputs:      alterScale flag
 *              index of first QMF subband in master freq table (k0)
 *              index of last QMF subband (k2)
 *
 * Outputs:     master frequency table
 *
 * Return:      number of bands in master frequency table
 *
 * Notes:       assumes k2 - k0 <= 48 and k2 >= k0 (4.6.18.3.6)
 **************************************************************************************/
static int CalcFreqMasterScaleZero(unsigned char *freqMaster, int alterScale, int k0, int k2)
{
	int nMaster, k, nBands, k2Achieved, dk, vDk[64], k2Diff;

	if (alterScale) {
		dk = 2;
		nBands = 2 * ((k2 - k0 + 2) >> 2);
	} else {
		dk = 1;
		nBands = 2 * ((k2 - k0) >> 1);
	}

	if (nBands <= 0)
		return 0;

	k2Achieved = k0 + nBands * dk;
	k2Diff = k2 - k2Achieved;
	for (k = 0; k < nBands; k++)
		vDk[k] = dk;

	if (k2Diff > 0) {
		k = nBands - 1;
		while (k2Diff) {
			vDk[k]++;
			k--;
			k2Diff--;
		}
	} else if (k2Diff < 0) {
		k = 0;
		while (k2Diff) {
			vDk[k]--;
			k++;
			k2Diff++;
		}
	}

	nMaster = nBands;
	freqMaster[0] = k0;
	for (k = 1; k <= nBands; k++)
		freqMaster[k] = freqMaster[k-1] + vDk[k-1];

	return nMaster;
}

/* mBandTab[i] = temp1[i] / 2 */
static const int mBandTab[3] PROGMEM = {6, 5, 4};

/* invWarpTab[i] = 1.0 / temp2[i], Q30 (see 4.6.18.3.2.1) */
static const int invWarpTab[2] PROGMEM = {0x40000000, 0x313b13b1};

/**************************************************************************************
 * Function:    CalcFreqMasterScale
 *
 * Description: calculate master frequency table when freqScale > 0 
 *                (4.6.18.3.2.1, figure 4.39)
 *
 * Inputs:      alterScale flag
 *              freqScale flag
 *              index of first QMF subband in master freq table (k0)
 *              index of last QMF subband (k2)
 *
 * Outputs:     master frequency table
 *
 * Return:      number of bands in master frequency table
 *
 * Notes:       assumes k2 - k0 <= 48 and k2 >= k0 (4.6.18.3.6)
 **************************************************************************************/
static int CalcFreqMaster(unsigned char *freqMaster, int freqScale, int alterScale, int k0, int k2)
{
	int bands, twoRegions, k, k1, t, vLast, vCurr, pCurr;
	int invWarp, nBands0, nBands1, change;
	unsigned char vDk1Min, vDk0Max;
	unsigned char *vDelta;

	if (freqScale < 1 || freqScale > 3)
		return -1;

	bands = mBandTab[freqScale - 1];
	invWarp = invWarpTab[alterScale];

	/* tested for all k0 = [5, 64], k2 = [k0, 64] */
	if (k2*10000 > 22449*k0) {
		twoRegions = 1;
		k1 = 2*k0;
	} else {
		twoRegions = 0;
		k1 = k2;
	}
	
	/* tested for all k0 = [5, 64], k1 = [k0, 64], freqScale = [1,3] */
	t = (log2Tab[k1] - log2Tab[k0]) >> 3;				/* log2(k1/k0), Q28 to Q25 */
	nBands0 = 2 * (((bands * t) + (1 << 24)) >> 25);	/* multiply by bands/2, round to nearest int (mBandTab has factor of 1/2 rolled in) */

	/* tested for all valid combinations of k0, k1, nBands (from sampRate, freqScale, alterScale) 
	 * roundoff error can be a problem with fixpt (e.g. pCurr = 12.499999 instead of 12.50003)
	 *   because successive multiplication always undershoots a little bit, but this
	 *   doesn't occur in any of the ratios we encounter from the valid k0/k1 bands in the spec
	 */
	t = RatioPowInv(k1, k0, nBands0);
	pCurr = k0 << 24;
	vLast = k0;
	vDelta = freqMaster + 1;	/* operate in-place */
	for (k = 0; k < nBands0; k++) {
		pCurr = MULSHIFT32(pCurr, t) << 8;	/* keep in Q24 */
		vCurr = (pCurr + (1 << 23)) >> 24;
		vDelta[k] = (vCurr - vLast);
		vLast = vCurr;
	}

	/* sort the deltas and find max delta for first region */
	BubbleSort(vDelta, nBands0);
	vDk0Max = VMax(vDelta, nBands0);

	/* fill master frequency table with bands from first region */
	freqMaster[0] = k0;
	for (k = 1; k <= nBands0; k++)
		freqMaster[k] += freqMaster[k-1];

	/* if only one region, then the table is complete */
	if (!twoRegions)
		return nBands0;

	/* tested for all k1 = [10, 64], k2 = [k0, 64], freqScale = [1,3] */
	t = (log2Tab[k2] - log2Tab[k1]) >> 3;		/* log2(k1/k0), Q28 to Q25 */
	t = MULSHIFT32(bands * t, invWarp) << 2;	/* multiply by bands/2, divide by warp factor, keep Q25 */
	nBands1 = 2 * ((t + (1 << 24)) >> 25);		/* round to nearest int */
				
	/* see comments above for calculations in first region */
	t = RatioPowInv(k2, k1, nBands1);
	pCurr = k1 << 24;
	vLast = k1;
	vDelta = freqMaster + nBands0 + 1;	/* operate in-place */
	for (k = 0; k < nBands1; k++) {
		pCurr = MULSHIFT32(pCurr, t) << 8;	/* keep in Q24 */
		vCurr = (pCurr + (1 << 23)) >> 24;
		vDelta[k] = (vCurr - vLast);
		vLast = vCurr;
	}

	/* sort the deltas, adjusting first and last if the second region has smaller deltas than the first */
	vDk1Min = VMin(vDelta, nBands1);
	if (vDk1Min < vDk0Max) {
		BubbleSort(vDelta, nBands1);
		change = vDk0Max - vDelta[0];
		if (change > ((vDelta[nBands1 - 1] - vDelta[0]) >> 1))
			 change = ((vDelta[nBands1 - 1] - vDelta[0]) >> 1);
		vDelta[0] += change;
		vDelta[nBands1-1] -= change;
	}
	BubbleSort(vDelta, nBands1);

	/* fill master frequency table with bands from second region 
	 * Note: freqMaster[nBands0] = k1
	 */
	for (k = 1; k <= nBands1; k++)
		freqMaster[k + nBands0] += freqMaster[k + nBands0 - 1];

	return (nBands0 + nBands1);
}

/**************************************************************************************
 * Function:    CalcFreqHigh
 *
 * Description: calculate high resolution frequency table (4.6.18.3.2.2)
 *
 * Inputs:      master frequency table
 *              number of bands in master frequency table 
 *              crossover band from header
 *
 * Outputs:     high resolution frequency table
 *
 * Return:      number of bands in high resolution frequency table
 **************************************************************************************/
static int CalcFreqHigh(unsigned char *freqHigh, unsigned char *freqMaster, int nMaster, int crossOverBand)
{
	int k, nHigh;

	nHigh = nMaster - crossOverBand;

	for (k = 0; k <= nHigh; k++)
		freqHigh[k] = freqMaster[k + crossOverBand];
	
	return nHigh;
}

/**************************************************************************************
 * Function:    CalcFreqLow
 *
 * Description: calculate low resolution frequency table (4.6.18.3.2.2)
 *
 * Inputs:      high resolution frequency table
 *              number of bands in high resolution frequency table 
 *
 * Outputs:     low resolution frequency table
 *
 * Return:      number of bands in low resolution frequency table
 **************************************************************************************/
static int CalcFreqLow(unsigned char *freqLow, unsigned char *freqHigh, int nHigh)
{
	int k, nLow, oddFlag;

	nLow = nHigh - (nHigh >> 1);
	freqLow[0] = freqHigh[0];
	oddFlag = nHigh & 0x01;

	for (k = 1; k <= nLow; k++)
		freqLow[k] = freqHigh[2*k - oddFlag];

	return nLow;
}

/**************************************************************************************
 * Function:    CalcFreqNoise
 *
 * Description: calculate noise floor frequency table (4.6.18.3.2.2)
 *
 * Inputs:      low resolution frequency table
 *              number of bands in low resolution frequency table
 *              index of starting QMF subband for SBR (kStart)
 *              index of last QMF subband (k2)
 *              number of noise bands
 *
 * Outputs:     noise floor frequency table
 *
 * Return:      number of bands in noise floor frequency table
 **************************************************************************************/
static int CalcFreqNoise(unsigned char *freqNoise, unsigned char *freqLow, int nLow, int kStart, int k2, int noiseBands)
{
	int i, iLast, k, nQ, lTop, lBottom;

	lTop = log2Tab[k2];
	lBottom = log2Tab[kStart];
	nQ = noiseBands*((lTop - lBottom) >> 2);	/* Q28 to Q26, noiseBands = [0,3] */
	nQ = (nQ + (1 << 25)) >> 26;
	if (nQ < 1)
		nQ = 1;

	ASSERT(nQ <= MAX_NUM_NOISE_FLOOR_BANDS);	/* required from 4.6.18.3.6 */

	iLast = 0;
	freqNoise[0] = freqLow[0];
	for (k = 1; k <= nQ; k++) {
		i = iLast + (nLow - iLast) / (nQ + 1 - k);	/* truncating division */
		freqNoise[k] = freqLow[i];
		iLast = i;
	}

	return nQ;
}

/**************************************************************************************
 * Function:    BuildPatches
 *
 * Description: build high frequency patches (4.6.18.6.3)
 *
 * Inputs:      master frequency table
 *              number of bands in low resolution frequency table
 *              index of first QMF subband in master freq table (k0)
 *              index of starting QMF subband for SBR (kStart)
 *              number of QMF bands in high resolution frequency table
 *              sample rate index
 *
 * Outputs:     starting subband for each patch
 *              number of subbands in each patch
 *
 * Return:      number of patches
 **************************************************************************************/
static int BuildPatches(unsigned char *patchNumSubbands, unsigned char *patchStartSubband, unsigned char *freqMaster, 
						int nMaster, int k0, int kStart, int numQMFBands, int sampRateIdx)
{
	int i, j, k;
	int msb, sb, usb, numPatches, goalSB, oddFlag;

	msb = k0;
	usb = kStart;
	numPatches = 0;
	goalSB = goalSBTab[sampRateIdx];

	if (nMaster == 0) {
		patchNumSubbands[0] = 0;
		patchStartSubband[0] = 0;
		return 0;
	}

	if (goalSB < kStart + numQMFBands) {
		k = 0;
		for (i = 0; freqMaster[i] < goalSB; i++)
			k = i+1;
	} else {
		k = nMaster;
	}

	do {
		j = k+1;
		do {
			j--;
			sb = freqMaster[j];
			oddFlag = (sb - 2 + k0) & 0x01;
		} while (sb > k0 - 1 + msb - oddFlag);

		patchNumSubbands[numPatches] = MAX(sb - usb, 0);
		patchStartSubband[numPatches] = k0 - oddFlag - patchNumSubbands[numPatches];

		/* from MPEG reference code - slightly different from spec */
		if ((patchNumSubbands[numPatches] < 3) && (numPatches > 0))
			break;

		if (patchNumSubbands[numPatches] > 0) {
			usb = sb;
			msb = sb;
			numPatches++;
		} else {
			msb = kStart;
		}

		if (freqMaster[k] - sb < 3)
			k = nMaster;

	} while (sb != (kStart + numQMFBands) && numPatches <= MAX_NUM_PATCHES);

	return numPatches;
}

/**************************************************************************************
 * Function:    FindFreq
 *
 * Description: search buffer of unsigned chars for a specific value
 *
 * Inputs:      buffer of elements to search
 *              number of elements to search
 *              value to search for
 *
 * Outputs:     none
 *
 * Return:      non-zero if the value is found anywhere in the buffer, zero otherwise
 **************************************************************************************/
static int FindFreq(unsigned char *freq, int nFreq, unsigned char val)
{
	int k;

	for (k = 0; k < nFreq; k++) {
		if (freq[k] == val)
			return 1;
	}

	return 0;
}

/**************************************************************************************
 * Function:    RemoveFreq
 *
 * Description: remove one element from a buffer of unsigned chars
 *
 * Inputs:      buffer of elements
 *              number of elements
 *              index of element to remove
 *
 * Outputs:     new buffer of length nFreq-1
 *
 * Return:      none
 **************************************************************************************/
static void RemoveFreq(unsigned char *freq, int nFreq, int removeIdx)
{
	int k;

	if (removeIdx >= nFreq)
		return;

	for (k = removeIdx; k < nFreq - 1; k++)
		freq[k] = freq[k+1];
}

/**************************************************************************************
 * Function:    CalcFreqLimiter
 *
 * Description: calculate limiter frequency table (4.6.18.3.2.3)
 *
 * Inputs:      number of subbands in each patch
 *              low resolution frequency table
 *              number of bands in low resolution frequency table
 *              index of starting QMF subband for SBR (kStart)
 *              number of limiter bands
 *              number of patches
 *
 * Outputs:     limiter frequency table
 *
 * Return:      number of bands in limiter frequency table
 **************************************************************************************/
static int CalcFreqLimiter(unsigned char *freqLimiter, unsigned char *patchNumSubbands, unsigned char *freqLow, 
						   int nLow, int kStart, int limiterBands, int numPatches)
{
	int k, bands, nLimiter, nOctaves;
	int limBandsPerOctave[3] = {120, 200, 300};		/* [1.2, 2.0, 3.0] * 100 */
	unsigned char patchBorders[MAX_NUM_PATCHES + 1];

	/* simple case */
	if (limiterBands == 0) {
		freqLimiter[0] = freqLow[0] - kStart;
		freqLimiter[1] = freqLow[nLow] - kStart;
		return 1;
	}

	bands = limBandsPerOctave[limiterBands - 1];
	patchBorders[0] = kStart;

	/* from MPEG reference code - slightly different from spec (top border) */
	for (k = 1; k < numPatches; k++)
		patchBorders[k] = patchBorders[k-1] + patchNumSubbands[k-1];
	patchBorders[k] = freqLow[nLow];

	for (k = 0; k <= nLow; k++)
		freqLimiter[k] = freqLow[k];

	for (k = 1; k < numPatches; k++)
		freqLimiter[k+nLow] = patchBorders[k];

	k = 1;
	nLimiter = nLow + numPatches - 1;
	BubbleSort(freqLimiter, nLimiter + 1);

	while (k <= nLimiter) {
		nOctaves = log2Tab[freqLimiter[k]] - log2Tab[freqLimiter[k-1]];	/* Q28 */
		nOctaves = (nOctaves >> 9) * bands;	/* Q19, max bands = 300 < 2^9 */
		if (nOctaves < (49 << 19)) {		/* compare with 0.49*100, in Q19 */
			if (freqLimiter[k] == freqLimiter[k-1] || FindFreq(patchBorders, numPatches + 1, freqLimiter[k]) == 0) {
				RemoveFreq(freqLimiter, nLimiter + 1, k);
				nLimiter--;
			} else if (FindFreq(patchBorders, numPatches + 1, freqLimiter[k-1]) == 0) {
				RemoveFreq(freqLimiter, nLimiter + 1, k-1);
				nLimiter--;
			} else {
				k++;
			}
		} else {
			k++;
		}
	}

	/* store limiter boundaries as offsets from kStart */
	for (k = 0; k <= nLimiter; k++)
		freqLimiter[k] -= kStart;

	return nLimiter;
}

/**************************************************************************************
 * Function:    CalcFreqTables
 *
 * Description: calulate master and derived frequency tables, and patches
 *
 * Inputs:      initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRFreq struct for this SCE/CPE block
 *              sample rate index of output sample rate (after SBR)
 *
 * Outputs:     master and derived frequency tables, and patches
 *
 * Return:      non-zero if error, zero otherwise
 **************************************************************************************/
int CalcFreqTables(SBRHeader *sbrHdr, SBRFreq *sbrFreq, int sampRateIdx)
{
	int k0, k2;

	k0 = k0Tab[sampRateIdx][sbrHdr->startFreq];

	if (sbrHdr->stopFreq == 14)
		k2 = 2*k0;
	else if (sbrHdr->stopFreq == 15)
		k2 = 3*k0;
	else
		k2 = k2Tab[sampRateIdx][sbrHdr->stopFreq];
	if (k2 > 64)
		k2 = 64;

	/* calculate master frequency table */
	if (sbrHdr->freqScale == 0)
		sbrFreq->nMaster = CalcFreqMasterScaleZero(sbrFreq->freqMaster, sbrHdr->alterScale, k0, k2);
	else
		sbrFreq->nMaster = CalcFreqMaster(sbrFreq->freqMaster, sbrHdr->freqScale, sbrHdr->alterScale, k0, k2);

	/* calculate high frequency table and related parameters */
	sbrFreq->nHigh = CalcFreqHigh(sbrFreq->freqHigh, sbrFreq->freqMaster, sbrFreq->nMaster, sbrHdr->crossOverBand);
	sbrFreq->numQMFBands = sbrFreq->freqHigh[sbrFreq->nHigh] - sbrFreq->freqHigh[0];
	sbrFreq->kStart = sbrFreq->freqHigh[0];

	/* calculate low frequency table */
	sbrFreq->nLow = CalcFreqLow(sbrFreq->freqLow, sbrFreq->freqHigh, sbrFreq->nHigh);

	/* calculate noise floor frequency table */
	sbrFreq->numNoiseFloorBands = CalcFreqNoise(sbrFreq->freqNoise, sbrFreq->freqLow, sbrFreq->nLow, sbrFreq->kStart, k2, sbrHdr->noiseBands);

	/* calculate limiter table */
	sbrFreq->numPatches = BuildPatches(sbrFreq->patchNumSubbands, sbrFreq->patchStartSubband, sbrFreq->freqMaster, 
		sbrFreq->nMaster, k0, sbrFreq->kStart, sbrFreq->numQMFBands, sampRateIdx);
	sbrFreq->nLimiter = CalcFreqLimiter(sbrFreq->freqLimiter, sbrFreq->patchNumSubbands, sbrFreq->freqLow, sbrFreq->nLow, sbrFreq->kStart,
		sbrHdr->limiterBands, sbrFreq->numPatches);

	return 0;
}