/*
 ** Copyright 2003-2010, VisualOn, Inc.
 **
 ** Licensed under the Apache License, Version 2.0 (the "License");
 ** you may not use this file except in compliance with the License.
 ** You may obtain a copy of the License at
 **
 **     http://www.apache.org/licenses/LICENSE-2.0
 **
 ** Unless required by applicable law or agreed to in writing, software
 ** distributed under the License is distributed on an "AS IS" BASIS,
 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 ** See the License for the specific language governing permissions and
 ** limitations under the License.
 */

/***********************************************************************
*       File: apisf_2s.c                                               *
*                                                                      *
*       Description: Coding/Decodeing of ISF parameters with predication
*       The ISF vector is quantized using two-stage VQ with split-by-2 *
*       in 1st stage and split-by-5(or 3) in the second stage          *
*                                                                      *
************************************************************************/

#include "typedef.h"
#include "basic_op.h"
#include "cnst.h"
#include "acelp.h"
#include "qpisf_2s.tab"                    /* Codebooks of isfs */

#define MU         10923                   /* Prediction factor   (1.0/3.0) in Q15 */
#define N_SURV_MAX 4                       /* 4 survivors max */
#define ALPHA      29491                   /* 0. 9 in Q15     */
#define ONE_ALPHA (32768-ALPHA)            /* (1.0 - ALPHA) in Q15 */

/* private functions */
static void VQ_stage1(
		Word16 * x,                           /* input : ISF residual vector           */
		Word16 * dico,                        /* input : quantization codebook         */
		Word16 dim,                           /* input : dimention of vector           */
		Word16 dico_size,                     /* input : size of quantization codebook */
		Word16 * index,                       /* output: indices of survivors          */
		Word16 surv                           /* input : number of survivor            */
		);

/**************************************************************************
* Function:   Qpisf_2s_46B()                                              *
*                                                                         *
* Description: Quantization of isf parameters with prediction. (46 bits)  *
*                                                                         *
* The isf vector is quantized using two-stage VQ with split-by-2 in       *
*  1st stage and split-by-5 in the second stage.                          *
***************************************************************************/

void Qpisf_2s_46b(
		Word16 * isf1,                        /* (i) Q15 : ISF in the frequency domain (0..0.5) */
		Word16 * isf_q,                       /* (o) Q15 : quantized ISF               (0..0.5) */
		Word16 * past_isfq,                   /* (io)Q15 : past ISF quantizer                   */
		Word16 * indice,                      /* (o)     : quantization indices                 */
		Word16 nb_surv                        /* (i)     : number of survivor (1, 2, 3 or 4)    */
		)
{
	Word16 tmp_ind[5];
	Word16 surv1[N_SURV_MAX];              /* indices of survivors from 1st stage */
	Word32 i, k, temp, min_err, distance;
	Word16 isf[ORDER];
	Word16 isf_stage2[ORDER];

	for (i = 0; i < ORDER; i++)
	{
		isf[i] = vo_sub(isf1[i], mean_isf[i]);
		isf[i] = vo_sub(isf[i], vo_mult(MU, past_isfq[i])); 
	}

	VQ_stage1(&isf[0], dico1_isf, 9, SIZE_BK1, surv1, nb_surv);

	distance = MAX_32;          

	for (k = 0; k < nb_surv; k++)
	{
		for (i = 0; i < 9; i++)
		{
			isf_stage2[i] = vo_sub(isf[i], dico1_isf[i + surv1[k] * 9]); 
		}
		tmp_ind[0] = Sub_VQ(&isf_stage2[0], dico21_isf, 3, SIZE_BK21, &min_err); 
		temp = min_err;
		tmp_ind[1] = Sub_VQ(&isf_stage2[3], dico22_isf, 3, SIZE_BK22, &min_err); 
		temp = vo_L_add(temp, min_err);
		tmp_ind[2] = Sub_VQ(&isf_stage2[6], dico23_isf, 3, SIZE_BK23, &min_err);  
		temp = vo_L_add(temp, min_err);

		if(temp < distance)
		{
			distance = temp;               
			indice[0] = surv1[k];          
			for (i = 0; i < 3; i++)
			{
				indice[i + 2] = tmp_ind[i];
			}
		}
	}


	VQ_stage1(&isf[9], dico2_isf, 7, SIZE_BK2, surv1, nb_surv);

	distance = MAX_32;                   

	for (k = 0; k < nb_surv; k++)
	{
		for (i = 0; i < 7; i++)
		{
			isf_stage2[i] = vo_sub(isf[9 + i], dico2_isf[i + surv1[k] * 7]);       
		}

		tmp_ind[0] = Sub_VQ(&isf_stage2[0], dico24_isf, 3, SIZE_BK24, &min_err);
		temp = min_err; 
		tmp_ind[1] = Sub_VQ(&isf_stage2[3], dico25_isf, 4, SIZE_BK25, &min_err);
		temp = vo_L_add(temp, min_err);

		if(temp < distance)
		{
			distance = temp;               
			indice[1] = surv1[k];          
			for (i = 0; i < 2; i++)
			{
				indice[i + 5] = tmp_ind[i];
			}
		}
	}

	Dpisf_2s_46b(indice, isf_q, past_isfq, isf_q, isf_q, 0, 0);

	return;
}

/*****************************************************************************
* Function:   Qpisf_2s_36B()                                                 *
*                                                                            *
* Description: Quantization of isf parameters with prediction. (36 bits)     *
*                                                                            *
* The isf vector is quantized using two-stage VQ with split-by-2 in          *
*  1st stage and split-by-3 in the second stage.                             *
******************************************************************************/

void Qpisf_2s_36b(
		Word16 * isf1,                        /* (i) Q15 : ISF in the frequency domain (0..0.5) */
		Word16 * isf_q,                       /* (o) Q15 : quantized ISF               (0..0.5) */
		Word16 * past_isfq,                   /* (io)Q15 : past ISF quantizer                   */
		Word16 * indice,                      /* (o)     : quantization indices                 */
		Word16 nb_surv                        /* (i)     : number of survivor (1, 2, 3 or 4)    */
		)
{
	Word16 i, k, tmp_ind[5];
	Word16 surv1[N_SURV_MAX];              /* indices of survivors from 1st stage */
	Word32 temp, min_err, distance;
	Word16 isf[ORDER];
	Word16 isf_stage2[ORDER];

	for (i = 0; i < ORDER; i++)
	{
		isf[i] = vo_sub(isf1[i], mean_isf[i]);
		isf[i] = vo_sub(isf[i], vo_mult(MU, past_isfq[i]));
	}

	VQ_stage1(&isf[0], dico1_isf, 9, SIZE_BK1, surv1, nb_surv);

	distance = MAX_32;                  

	for (k = 0; k < nb_surv; k++)
	{
		for (i = 0; i < 9; i++)
		{
			isf_stage2[i] = vo_sub(isf[i], dico1_isf[i + surv1[k] * 9]); 
		}

		tmp_ind[0] = Sub_VQ(&isf_stage2[0], dico21_isf_36b, 5, SIZE_BK21_36b, &min_err);        
		temp = min_err;                  
		tmp_ind[1] = Sub_VQ(&isf_stage2[5], dico22_isf_36b, 4, SIZE_BK22_36b, &min_err);        
		temp = vo_L_add(temp, min_err);

		if(temp < distance)
		{
			distance = temp;               
			indice[0] = surv1[k];          
			for (i = 0; i < 2; i++)
			{
				indice[i + 2] = tmp_ind[i];
			}
		}
	}

	VQ_stage1(&isf[9], dico2_isf, 7, SIZE_BK2, surv1, nb_surv);
	distance = MAX_32;                    

	for (k = 0; k < nb_surv; k++)
	{
		for (i = 0; i < 7; i++)
		{
			isf_stage2[i] = vo_sub(isf[9 + i], dico2_isf[i + surv1[k] * 7]);     
		}

		tmp_ind[0] = Sub_VQ(&isf_stage2[0], dico23_isf_36b, 7, SIZE_BK23_36b, &min_err);  
		temp = min_err;                  

		if(temp < distance)
		{
			distance = temp;               
			indice[1] = surv1[k];          
			indice[4] = tmp_ind[0];        
		}
	}

	Dpisf_2s_36b(indice, isf_q, past_isfq, isf_q, isf_q, 0, 0);

	return;
}

/*********************************************************************
* Function: Dpisf_2s_46b()                                           *
*                                                                    *
* Description: Decoding of ISF parameters                            *
**********************************************************************/

void Dpisf_2s_46b(
		Word16 * indice,                      /* input:  quantization indices                       */
		Word16 * isf_q,                       /* output: quantized ISF in frequency domain (0..0.5) */
		Word16 * past_isfq,                   /* i/0   : past ISF quantizer                    */
		Word16 * isfold,                      /* input : past quantized ISF                    */
		Word16 * isf_buf,                     /* input : isf buffer                                                        */
		Word16 bfi,                           /* input : Bad frame indicator                   */
		Word16 enc_dec
		)
{
	Word16 ref_isf[M], tmp;
	Word32 i, j, L_tmp;

	if (bfi == 0)                          /* Good frame */
	{
		for (i = 0; i < 9; i++)
		{
			isf_q[i] = dico1_isf[indice[0] * 9 + i];    
		}
		for (i = 0; i < 7; i++)
		{
			isf_q[i + 9] = dico2_isf[indice[1] * 7 + i];       
		}

		for (i = 0; i < 3; i++)
		{
			isf_q[i] = add1(isf_q[i], dico21_isf[indice[2] * 3 + i]);   
			isf_q[i + 3] = add1(isf_q[i + 3], dico22_isf[indice[3] * 3 + i]);  
			isf_q[i + 6] = add1(isf_q[i + 6], dico23_isf[indice[4] * 3 + i]); 
			isf_q[i + 9] = add1(isf_q[i + 9], dico24_isf[indice[5] * 3 + i]); 
		}

		for (i = 0; i < 4; i++)
		{
			isf_q[i + 12] = add1(isf_q[i + 12], dico25_isf[indice[6] * 4 + i]);  
		}

		for (i = 0; i < ORDER; i++)
		{
			tmp = isf_q[i];               
			isf_q[i] = add1(tmp, mean_isf[i]);  
			isf_q[i] = add1(isf_q[i], vo_mult(MU, past_isfq[i]));
			past_isfq[i] = tmp;  
		}

		if (enc_dec)
		{
			for (i = 0; i < M; i++)
			{
				for (j = (L_MEANBUF - 1); j > 0; j--)
				{
					isf_buf[j * M + i] = isf_buf[(j - 1) * M + i]; 
				}
				isf_buf[i] = isf_q[i]; 
			}
		}
	} else
	{                                      /* bad frame */
		for (i = 0; i < M; i++)
		{
			L_tmp = mean_isf[i] << 14;
			for (j = 0; j < L_MEANBUF; j++)
			{
				L_tmp += (isf_buf[j * M + i] << 14);
			}
			ref_isf[i] = vo_round(L_tmp);
		}

		/* use the past ISFs slightly shifted towards their mean */
		for (i = 0; i < ORDER; i++)
		{
			isf_q[i] = add1(vo_mult(ALPHA, isfold[i]), vo_mult(ONE_ALPHA, ref_isf[i])); 
		}

		/* estimate past quantized residual to be used in next frame */
		for (i = 0; i < ORDER; i++)
		{
			tmp = add1(ref_isf[i], vo_mult(past_isfq[i], MU));      /* predicted ISF */
			past_isfq[i] = vo_sub(isf_q[i], tmp); 
			past_isfq[i] = (past_isfq[i] >> 1);        /* past_isfq[i] *= 0.5 */
		}
	}

	Reorder_isf(isf_q, ISF_GAP, ORDER);
	return;
}

/*********************************************************************
* Function:   Disf_2s_36b()                                          *
*                                                                    *
* Description: Decoding of ISF parameters                            *
*********************************************************************/

void Dpisf_2s_36b(
		Word16 * indice,                      /* input:  quantization indices                       */
		Word16 * isf_q,                       /* output: quantized ISF in frequency domain (0..0.5) */
		Word16 * past_isfq,                   /* i/0   : past ISF quantizer                    */
		Word16 * isfold,                      /* input : past quantized ISF                    */
		Word16 * isf_buf,                     /* input : isf buffer                                                        */
		Word16 bfi,                           /* input : Bad frame indicator                   */
		Word16 enc_dec
		)
{
	Word16 ref_isf[M], tmp;
	Word32 i, j, L_tmp;

	if (bfi == 0)                          /* Good frame */
	{
		for (i = 0; i < 9; i++)
		{
			isf_q[i] = dico1_isf[indice[0] * 9 + i];    
		}
		for (i = 0; i < 7; i++)
		{
			isf_q[i + 9] = dico2_isf[indice[1] * 7 + i];       
		}

		for (i = 0; i < 5; i++)
		{
			isf_q[i] = add1(isf_q[i], dico21_isf_36b[indice[2] * 5 + i]);       
		}
		for (i = 0; i < 4; i++)
		{
			isf_q[i + 5] = add1(isf_q[i + 5], dico22_isf_36b[indice[3] * 4 + i]);        
		}
		for (i = 0; i < 7; i++)
		{
			isf_q[i + 9] = add1(isf_q[i + 9], dico23_isf_36b[indice[4] * 7 + i]);       
		}

		for (i = 0; i < ORDER; i++)
		{
			tmp = isf_q[i];
			isf_q[i] = add1(tmp, mean_isf[i]);   
			isf_q[i] = add1(isf_q[i], vo_mult(MU, past_isfq[i]));   
			past_isfq[i] = tmp;           
		}


		if (enc_dec)
		{
			for (i = 0; i < M; i++)
			{
				for (j = (L_MEANBUF - 1); j > 0; j--)
				{
					isf_buf[j * M + i] = isf_buf[(j - 1) * M + i];      
				}
				isf_buf[i] = isf_q[i];    
			}
		}
	} else
	{                                      /* bad frame */
		for (i = 0; i < M; i++)
		{
			L_tmp = (mean_isf[i] << 14);
			for (j = 0; j < L_MEANBUF; j++)
			{
				L_tmp += (isf_buf[j * M + i] << 14);
			}
			ref_isf[i] = vo_round(L_tmp);    
		}

		/* use the past ISFs slightly shifted towards their mean */
		for (i = 0; i < ORDER; i++)
		{
			isf_q[i] = add1(vo_mult(ALPHA, isfold[i]), vo_mult(ONE_ALPHA, ref_isf[i]));        
		}

		/* estimate past quantized residual to be used in next frame */
		for (i = 0; i < ORDER; i++)
		{
			tmp = add1(ref_isf[i], vo_mult(past_isfq[i], MU));      /* predicted ISF */
			past_isfq[i] = vo_sub(isf_q[i], tmp);  
			past_isfq[i] = past_isfq[i] >> 1;         /* past_isfq[i] *= 0.5 */
		}
	}

	Reorder_isf(isf_q, ISF_GAP, ORDER);

	return;
}


/***************************************************************************
* Function:  Reorder_isf()                                                 *
*                                                                          *
* Description: To make sure that the  isfs are properly order and to       *
*              keep a certain minimum distance between consecutive isfs.   *
*--------------------------------------------------------------------------*
*    Argument         description                     in/out               *
*                                                                          *
*     isf[]           vector of isfs                    i/o                *
*     min_dist        minimum required distance         i                  *
*     n               LPC order                         i                  *
****************************************************************************/

void Reorder_isf(
		Word16 * isf,                         /* (i/o) Q15: ISF in the frequency domain (0..0.5) */
		Word16 min_dist,                      /* (i) Q15  : minimum distance to keep             */
		Word16 n                              /* (i)      : number of ISF                        */
		)
{
	Word32 i; 
	Word16 isf_min;

	isf_min = min_dist;                    
	for (i = 0; i < n - 1; i++)
	{
		if(isf[i] < isf_min)
		{
			isf[i] = isf_min;              
		}
		isf_min = (isf[i] + min_dist);
	}
	return;
}


Word16 Sub_VQ(                             /* output: return quantization index     */
		Word16 * x,                           /* input : ISF residual vector           */
		Word16 * dico,                        /* input : quantization codebook         */
		Word16 dim,                           /* input : dimention of vector           */
		Word16 dico_size,                     /* input : size of quantization codebook */
		Word32 * distance                     /* output: error of quantization         */
	     )
{
	Word16 temp, *p_dico;
	Word32 i, j, index;
	Word32 dist_min, dist;

	dist_min = MAX_32;                     
	p_dico = dico;                         

	index = 0;                             
	for (i = 0; i < dico_size; i++)
	{
		dist = 0;  

		for (j = 0; j < dim; j++)
		{
			temp = x[j] - (*p_dico++);
			dist += (temp * temp)<<1;
		}

		if(dist < dist_min)
		{
			dist_min = dist;               
			index = i;                     
		}
	}

	*distance = dist_min;                  

	/* Reading the selected vector */
	p_dico = &dico[index * dim];           
	for (j = 0; j < dim; j++)
	{
		x[j] = *p_dico++;                  
	}

	return index;
}


static void VQ_stage1(
		Word16 * x,                           /* input : ISF residual vector           */
		Word16 * dico,                        /* input : quantization codebook         */
		Word16 dim,                           /* input : dimention of vector           */
		Word16 dico_size,                     /* input : size of quantization codebook */
		Word16 * index,                       /* output: indices of survivors          */
		Word16 surv                           /* input : number of survivor            */
		)
{
	Word16 temp, *p_dico;
	Word32 i, j, k, l;
	Word32 dist_min[N_SURV_MAX], dist;

	dist_min[0] = MAX_32;
	dist_min[1] = MAX_32;
	dist_min[2] = MAX_32;
	dist_min[3] = MAX_32;
	index[0] = 0;
	index[1] = 1;
	index[2] = 2;
	index[3] = 3;

	p_dico = dico;                         

	for (i = 0; i < dico_size; i++)
	{
		dist = 0;                          
		for (j = 0; j < dim; j++)
		{
			temp = x[j] -  (*p_dico++);
			dist += (temp * temp)<<1;
		}

		for (k = 0; k < surv; k++)
		{
			if(dist < dist_min[k])
			{
				for (l = surv - 1; l > k; l--)
				{
					dist_min[l] = dist_min[l - 1];      
					index[l] = index[l - 1];    
				}
				dist_min[k] = dist;        
				index[k] = i;              
				break;
			}
		}
	}
	return;
}