n0Texture::n0Texture(u32 texID, u32 texWidth, u32 texHeight, u32 bpp, u32 colorspace)
:width(texWidth), height(texHeight), textureID(texID), bpp(bpp), colorspace(colorspace)
{
//glEnable(GL_TEXTURE_2D);
while( glGetError());
glGenTextures(1,&glID);
glBindTexture(GL_TEXTURE_2D, glID);
Pow2(texWidth);
Pow2(texHeight);
u32 val = 0xff00ff00;
u32 val2 = 0xff0000FF;
u32 * fakeData = (u32*)malloc(texWidth*texHeight * sizeof(u32));
for(u32 y = 0; y < texHeight ; y++)
{
if(y%16 == 0)
{
SWAP(val,val2);
}
for(u32 x = 0; x < texWidth ; x++)
{
if(x%16 == 0)
{
SWAP(val,val2);
}
fakeData[y*texWidth + x] = val;
}
}
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST); // Linear Filtering
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST); // Linear Filtering
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, fakeData);
//n0_FlushGLError();
u32 err;
while(err = glGetError())
{
printf("GL ERROR #%d - %s:%d",err,__FILE__,__LINE__);
};
// glDisable(GL_TEXTURE_2D);
}
double Track_RMSE(Wml::Vector2d &point_2d, Wml::Matrix4d &P,
Wml::Matrix3d &K,
Wml::Vector3d &v_3d)
{
Wml::Vector3d pt3d;
pt3d[0] = P(0,0)*v_3d[0]+P(0,1)*v_3d[1]+P(0,2)*v_3d[2]+P(0,3);
pt3d[1] = P(1,0)*v_3d[0]+P(1,1)*v_3d[1]+P(1,2)*v_3d[2]+P(1,3);
pt3d[2] = P(2,0)*v_3d[0]+P(2,1)*v_3d[1]+P(2,2)*v_3d[2]+P(2,3);
pt3d[0] /= pt3d[2];
pt3d[1] /= pt3d[2];
pt3d[2] = 1.0;
double RMSE = Pow2(K(0, 0)*pt3d[0] + K(0, 2) - point_2d.X()) + Pow2(K(1, 1) * pt3d[1] + K(1, 2) - point_2d.Y());
RMSE = sqrt(RMSE);
return RMSE;
}
ID opzMiniIdMgr::GetId()
{
ID i, count;
if(_binTree[1] >= Pow2(_level - 1))
{
DoubleGrow();
}
//본격적으로 빈 id를 찾습니다.
for(i = 1u, count = 0u; count < _level - 1; count++)
{
_binTree[i]++;
if(_binTree[2u * i] < Pow2(_level - count - 2U))
{ //왼쪽
i *= 2u;
}
else
{ //오른쪽
i = 2u * i + 1u;
}
}
_binTree[i] = 1u;
return i - Pow2(_level - 1u) + 1u;
}
int main () {
while(1){
gets(In);
if(In[0] == '0') break;
int len = strlen(In);
int Tmp = len;
Ans = 0;
for(int i = 0; i < len; i++){
int t = In[i] - '0';
Ans = Ans + (t * (Pow2(Tmp) - 1));
Tmp--;
}
printf("%d\n",Ans);
}
return 0;
}
void opzMiniIdMgr::SetId(ID id)
{
while(Pow2(_level - 1) < id)
{
//level이 충분하지 못하다.
DoubleGrow();
}
ID index = GetTreeIndex(id);
if(index == 0u) return;
if(_binTree[index] >= 1u)
{
// assert(L"이미 할당 된 ID를 또 Set했습니다.");
return;
}
else
{
while(index > 0u)
{
++_binTree[index];
index /= 2u;
}
}
}
//=============================================================================
//函数名称:Dec_gain
//函数功能:解码的音调和码书增益
//=============================================================================
void Dec_gain(
gc_predState *pred_state, /* i/o: MA predictor state */
enum Mode mode, /* i : AMR mode */
Word16 index, /* i : index of quantization. */
Word16 code[], /* i : Innovative vector. */
Word16 evenSubfr, /* i : Flag for even subframes */
Word16 * gain_pit, /* o : Pitch gain. */
Word16 * gain_cod, /* o : Code gain. */
Flag * pOverflow
)
{
const Word16 *p;
Word16 frac;
Word16 gcode0;
Word16 exp;
Word16 qua_ener;
Word16 qua_ener_MR122;
Word16 g_code;
Word32 L_tmp;
Word16 temp1;
Word16 temp2;
/* Read the quantized gains (table depends on mode) */
//阅读量化收益（表取决于模式）
index = shl(index, 2, pOverflow);
if (mode == MR102 || mode == MR74 || mode == MR67)
{
p = &table_gain_highrates[index];
*gain_pit = *p++;
g_code = *p++;
qua_ener_MR122 = *p++;
qua_ener = *p;
}
else
{
if (mode == MR475)
{
index += (1 ^ evenSubfr) << 1; /* evenSubfr is 0 or 1 */
if (index > (MR475_VQ_SIZE*4 - 2))
{
index = (MR475_VQ_SIZE * 4 - 2); /* avoid possible buffer overflow */
}
p = &table_gain_MR475[index];
*gain_pit = *p++;
g_code = *p++;
/*---------------------------------------------------------*
* calculate predictor update values (not stored in 4.75 *
* quantizer table to save space): *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* qua_ener = log2(g) *
* qua_ener_MR122 = 20*log10(g) *
*---------------------------------------------------------*/
//计算预测更新值（不存储在4.75量化表，以节省空间）
/* Log2(x Q12) = log2(x) + 12 */
temp1 = (Word16) L_deposit_l(g_code);
Log2(temp1, &exp, &frac, pOverflow);
exp = sub(exp, 12, pOverflow);
temp1 = shr_r(frac, 5, pOverflow);
temp2 = shl(exp, 10, pOverflow);
qua_ener_MR122 = add(temp1, temp2, pOverflow);
/* 24660 Q12 ~= 6.0206 = 20*log10(2) */
L_tmp = Mpy_32_16(exp, frac, 24660, pOverflow);
L_tmp = L_shl(L_tmp, 13, pOverflow);
qua_ener = pv_round(L_tmp, pOverflow);
/* Q12 * Q0 = Q13 -> Q10 */
}
else
{
p = &table_gain_lowrates[index];
*gain_pit = *p++;
g_code = *p++;
qua_ener_MR122 = *p++;
qua_ener = *p;
}
}
/*-------------------------------------------------------------------*
* predict codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = Pow2(int(d)+frac(d)) *
* = 2^exp + 2^frac *
* *
* gcode0 (Q14) = 2^14*2^frac = gc0 * 2^(14-exp) *
*-------------------------------------------------------------------*/
gc_pred(pred_state, mode, code, &exp, &frac, NULL, NULL, pOverflow);
gcode0 = (Word16) Pow2(14, frac, pOverflow);
/*------------------------------------------------------------------*
* read quantized gains, update table of past quantized energies *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* st->past_qua_en(Q10) = 20 * Log10(g_fac) / constant *
* = Log2(g_fac) *
* = qua_ener *
* constant = 20*Log10(2) *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0, pOverflow);
temp1 = sub(10, exp, pOverflow);
L_tmp = L_shr(L_tmp, temp1, pOverflow);
*gain_cod = extract_h(L_tmp);
/* update table of past quantized energies */
//过去的量化能量表更新
gc_pred_update(pred_state, qua_ener_MR122, qua_ener);
return;
}
void solve(void) {
Input();
GetPrime();
Work();
Pow2();
}
GrGeomExtentArray *GrGeomCreateExtentArray(
SubgridArray *subgrids,
int xl_ghost,
int xu_ghost,
int yl_ghost,
int yu_ghost,
int zl_ghost,
int zu_ghost)
{
Background *bg = GlobalsBackground;
GrGeomExtentArray *extent_array;
GrGeomExtents *extents;
int size;
Subgrid *subgrid;
int ref;
int bg_ix, bg_iy, bg_iz;
int bg_nx, bg_ny, bg_nz;
int is;
size = SubgridArraySize(subgrids);
extents = ctalloc(GrGeomExtents, size);
ForSubgridI(is, subgrids)
{
subgrid = SubgridArraySubgrid(subgrids, is);
/* compute background grid extents on MaxRefLevel index space */
ref = (int)pow(2.0, GlobalsMaxRefLevel);
bg_ix = BackgroundIX(bg) * ref;
bg_iy = BackgroundIY(bg) * ref;
bg_iz = BackgroundIZ(bg) * ref;
bg_nx = BackgroundNX(bg) * ref;
bg_ny = BackgroundNY(bg) * ref;
bg_nz = BackgroundNZ(bg) * ref;
ref = (int)Pow2(GlobalsMaxRefLevel);
/*------------------------------------------
* set the lower extent values
*------------------------------------------*/
if (xl_ghost > -1)
{
xl_ghost = pfmax(xl_ghost, 1);
GrGeomExtentsIXLower(extents[is]) =
(SubgridIX(subgrid) - xl_ghost) * ref;
}
else
{
GrGeomExtentsIXLower(extents[is]) = bg_ix;
}
if (yl_ghost > -1)
{
yl_ghost = pfmax(yl_ghost, 1);
GrGeomExtentsIYLower(extents[is]) =
(SubgridIY(subgrid) - yl_ghost) * ref;
}
else
{
GrGeomExtentsIYLower(extents[is]) = bg_iy;
}
if (zl_ghost > -1)
{
zl_ghost = pfmax(zl_ghost, 1);
GrGeomExtentsIZLower(extents[is]) =
(SubgridIZ(subgrid) - zl_ghost) * ref;
}
else
{
GrGeomExtentsIZLower(extents[is]) = bg_iz;
}
/*------------------------------------------
* set the upper extent values
*------------------------------------------*/
if (xu_ghost > -1)
{
xu_ghost = pfmax(xu_ghost, 1);
GrGeomExtentsIXUpper(extents[is]) =
(SubgridIX(subgrid) + SubgridNX(subgrid) + xu_ghost) * ref - 1;
}
else
{
GrGeomExtentsIXUpper(extents[is]) = bg_ix + bg_nx - 1;
}
if (yu_ghost > -1)
{
yu_ghost = pfmax(yu_ghost, 1);
GrGeomExtentsIYUpper(extents[is]) =
(SubgridIY(subgrid) + SubgridNY(subgrid) + yu_ghost) * ref - 1;
}
else
{
GrGeomExtentsIYUpper(extents[is]) = bg_iy + bg_ny - 1;
}
if (zu_ghost > -1)
{
zu_ghost = pfmax(zu_ghost, 1);
GrGeomExtentsIZUpper(extents[is]) =
(SubgridIZ(subgrid) + SubgridNZ(subgrid) + zu_ghost) * ref - 1;
}
else
{
GrGeomExtentsIZUpper(extents[is]) = bg_iz + bg_nz - 1;
}
/*------------------------------------------
* convert to "octree coordinates"
*------------------------------------------*/
/* Moved into the loop by SGS 7/8/98, was lying outside the is
* loop which was an error (accessing invalid array elements)
*/
GrGeomExtentsIXLower(extents[is]) -= bg_ix;
GrGeomExtentsIYLower(extents[is]) -= bg_iy;
GrGeomExtentsIZLower(extents[is]) -= bg_iz;
GrGeomExtentsIXUpper(extents[is]) -= bg_ix;
GrGeomExtentsIYUpper(extents[is]) -= bg_iy;
GrGeomExtentsIZUpper(extents[is]) -= bg_iz;
}
/*************************************************************************
*
* FUNCTION: Dec_gain()
*
* PURPOSE: Decode the pitch and codebook gains
*
************************************************************************/
void Dec_gain(
gc_predState *pred_state, /* i/o: MA predictor state */
enum Mode mode, /* i : AMR mode */
Word16 index, /* i : index of quantization. */
Word16 code[], /* i : Innovative vector. */
Word16 evenSubfr, /* i : Flag for even subframes */
Word16 * gain_pit, /* o : Pitch gain. */
Word16 * gain_cod /* o : Code gain. */
)
{
const Word16 *p;
Word16 frac, gcode0, exp, qua_ener, qua_ener_MR122;
Word16 g_code;
Word32 L_tmp;
/* Read the quantized gains (table depends on mode) */
index = shl (index, 2);
test(); test(); test();
if ( sub (mode, MR102) == 0
|| sub (mode, MR74) == 0
|| sub (mode, MR67) == 0)
{
p = &table_gain_highrates[index]; move16 ();
*gain_pit = *p++; move16 ();
g_code = *p++; move16 ();
qua_ener_MR122 = *p++; move16 ();
qua_ener = *p; move16 ();
}
else
{
test();
if (sub (mode, MR475) == 0)
{
index = add (index, shl(sub(1, evenSubfr), 1));
p = &table_gain_MR475[index]; move16 ();
*gain_pit = *p++; move16 ();
g_code = *p++; move16 ();
/*---------------------------------------------------------*
* calculate predictor update values (not stored in 4.75 *
* quantizer table to save space): *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* qua_ener = log2(g) *
* qua_ener_MR122 = 20*log10(g) *
*---------------------------------------------------------*/
/* Log2(x Q12) = log2(x) + 12 */
Log2 (L_deposit_l (g_code), &exp, &frac);
exp = sub(exp, 12);
qua_ener_MR122 = add (shr_r (frac, 5), shl (exp, 10));
/* 24660 Q12 ~= 6.0206 = 20*log10(2) */
L_tmp = Mpy_32_16(exp, frac, 24660);
qua_ener = round (L_shl (L_tmp, 13)); /* Q12 * Q0 = Q13 -> Q10 */
}
else
{
p = &table_gain_lowrates[index]; move16 ();
*gain_pit = *p++; move16 ();
g_code = *p++; move16 ();
qua_ener_MR122 = *p++; move16 ();
qua_ener = *p; move16 ();
}
}
/*-------------------------------------------------------------------*
* predict codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = Pow2(int(d)+frac(d)) *
* = 2^exp + 2^frac *
* *
* gcode0 (Q14) = 2^14*2^frac = gc0 * 2^(14-exp) *
*-------------------------------------------------------------------*/
gc_pred(pred_state, mode, code, &exp, &frac, NULL, NULL);
gcode0 = extract_l(Pow2(14, frac));
/*------------------------------------------------------------------*
* read quantized gains, update table of past quantized energies *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* st->past_qua_en(Q10) = 20 * Log10(g_fac) / constant *
* = Log2(g_fac) *
* = qua_ener *
* constant = 20*Log10(2) *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0);
L_tmp = L_shr(L_tmp, sub(10, exp));
*gain_cod = extract_h(L_tmp);
/* update table of past quantized energies */
gc_pred_update(pred_state, qua_ener_MR122, qua_ener);
return;
}
Word16
Qua_gain( /* o : index of quantization. */
enum Mode mode, /* i : AMR mode */
Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */
Word16 frac_gcode0, /* i : predicted CB gain (fraction), Q15 */
Word16 frac_coeff[], /* i : energy coeff. (5), fraction part, Q15 */
Word16 exp_coeff[], /* i : energy coeff. (5), exponent part, Q0 */
/* (frac_coeff and exp_coeff computed in */
/* calc_filt_energies()) */
Word16 gp_limit, /* i : pitch gain limit */
Word16 *gain_pit, /* o : Pitch gain, Q14 */
Word16 *gain_cod, /* o : Code gain, Q1 */
Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */
/* (for MR122 MA predictor update) */
Word16 *qua_ener, /* o : quantized energy error, Q10 */
/* (for other MA predictor update) */
CommonAmrTbls* common_amr_tbls, /* i : ptr to struct of tables ptrs */
Flag *pOverflow /* o : overflow indicator */
)
{
const Word16 *p;
Word16 i;
Word16 j;
Word16 index = 0;
Word16 gcode0;
Word16 e_max;
Word16 temp;
Word16 exp_code;
Word16 g_pitch;
Word16 g2_pitch;
Word16 g_code;
Word16 g2_code;
Word16 g_pit_cod;
Word16 coeff[5];
Word16 coeff_lo[5];
Word16 exp_max[5];
Word32 L_tmp;
Word32 L_tmp2;
Word32 dist_min;
const Word16 *table_gain;
Word16 table_len;
if (mode == MR102 || mode == MR74 || mode == MR67)
{
table_len = VQ_SIZE_HIGHRATES;
table_gain = common_amr_tbls->table_gain_highrates_ptr;
}
else
{
table_len = VQ_SIZE_LOWRATES;
table_gain = common_amr_tbls->table_gain_lowrates_ptr;
}
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = 2^exp_gcode0 + 2^frac_gcode0 *
* *
* gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0) *
*-------------------------------------------------------------------*/
gcode0 = (Word16)(Pow2(14, frac_gcode0, pOverflow));
/*-------------------------------------------------------------------*
* Scaling considerations: *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
*-------------------------------------------------------------------*/
/*
* The error energy (sum) to be minimized consists of five terms, t[0..4].
*
* t[0] = gp^2 * <y1 y1>
* t[1] = -2*gp * <xn y1>
* t[2] = gc^2 * <y2 y2>
* t[3] = -2*gc * <xn y2>
* t[4] = 2*gp*gc * <y1 y2>
*
*/
/* determine the scaling exponent for g_code: ec = ec0 - 11 */
exp_code = exp_gcode0 - 11;
/* calculate exp_max[i] = s[i]-1 */
exp_max[0] = exp_coeff[0] - 13;
exp_max[1] = exp_coeff[1] - 14;
temp = shl(exp_code, 1, pOverflow);
temp += 15;
exp_max[2] = add_16(exp_coeff[2], temp, pOverflow);
exp_max[3] = add_16(exp_coeff[3], exp_code, pOverflow);
temp = exp_code + 1;
exp_max[4] = add_16(exp_coeff[4], temp, pOverflow);
/*-------------------------------------------------------------------*
* Find maximum exponent: *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* *
* For the sum operation, all terms must have the same scaling; *
* that scaling should be low enough to prevent overflow. There- *
* fore, the maximum scale is determined and all coefficients are *
* re-scaled: *
* *
* e_max = max(exp_max[i]) + 1; *
* e = exp_max[i]-e_max; e <= 0! *
* c[i] = c[i]*2^e *
*-------------------------------------------------------------------*/
e_max = exp_max[0];
for (i = 1; i < 5; i++)
{
if (exp_max[i] > e_max)
{
e_max = exp_max[i];
}
}
e_max++;
for (i = 0; i < 5; i++)
{
j = e_max - exp_max[i];
L_tmp = ((Word32)frac_coeff[i] << 16);
L_tmp = L_shr(L_tmp, j, pOverflow);
L_Extract(L_tmp, &coeff[i], &coeff_lo[i], pOverflow);
}
/*-------------------------------------------------------------------*
* Codebook search: *
* ~~~~~~~~~~~~~~~~ *
* *
* For each pair (g_pitch, g_fac) in the table calculate the *
* terms t[0..4] and sum them up; the result is the mean squared *
* error for the quantized gains from the table. The index for the *
* minimum MSE is stored and finally used to retrieve the quantized *
* gains *
*-------------------------------------------------------------------*/
/* start with "infinite" MSE */
dist_min = MAX_32;
p = &table_gain[0];
for (i = 0; i < table_len; i++)
{
g_pitch = *p++;
g_code = *p++; /* this is g_fac */
p++; /* skip log2(g_fac) */
p++; /* skip 20*log10(g_fac) */
if (g_pitch <= gp_limit)
{
g_code = mult(g_code, gcode0, pOverflow);
g2_pitch = mult(g_pitch, g_pitch, pOverflow);
g2_code = mult(g_code, g_code, pOverflow);
g_pit_cod = mult(g_code, g_pitch, pOverflow);
L_tmp = Mpy_32_16(coeff[0], coeff_lo[0], g2_pitch, pOverflow);
L_tmp2 = Mpy_32_16(coeff[1], coeff_lo[1], g_pitch, pOverflow);
L_tmp = L_add(L_tmp, L_tmp2, pOverflow);
L_tmp2 = Mpy_32_16(coeff[2], coeff_lo[2], g2_code, pOverflow);
L_tmp = L_add(L_tmp, L_tmp2, pOverflow);
L_tmp2 = Mpy_32_16(coeff[3], coeff_lo[3], g_code, pOverflow);
L_tmp = L_add(L_tmp, L_tmp2, pOverflow);
L_tmp2 = Mpy_32_16(coeff[4], coeff_lo[4], g_pit_cod, pOverflow);
L_tmp = L_add(L_tmp, L_tmp2, pOverflow);
/* store table index if MSE for this index is lower
than the minimum MSE seen so far */
if (L_tmp < dist_min)
{
dist_min = L_tmp;
index = i;
}
}
}
/*------------------------------------------------------------------*
* read quantized gains and new values for MA predictor memories *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
*------------------------------------------------------------------*/
/* Read the quantized gains */
p = &table_gain[shl(index, 2, pOverflow)];
*gain_pit = *p++;
g_code = *p++;
*qua_ener_MR122 = *p++;
*qua_ener = *p;
/*------------------------------------------------------------------*
* calculate final fixed codebook gain: *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* gc = gc0 * g *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0, pOverflow);
temp = 10 - exp_gcode0;
L_tmp = L_shr(L_tmp, temp, pOverflow);
*gain_cod = (Word16)(L_tmp >> 16);
return index;
}
/*************************************************************************
*
* FUNCTION: Qua_gain()
*
* PURPOSE: Quantization of pitch and codebook gains.
* (using predicted codebook gain)
*
*************************************************************************/
Word16
Qua_gain( /* o : index of quantization. */
enum Mode mode, /* i : AMR mode */
Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */
Word16 frac_gcode0, /* i : predicted CB gain (fraction), Q15 */
Word16 frac_coeff[], /* i : energy coeff. (5), fraction part, Q15 */
Word16 exp_coeff[], /* i : energy coeff. (5), exponent part, Q0 */
/* (frac_coeff and exp_coeff computed in */
/* calc_filt_energies()) */
Word16 gp_limit, /* i : pitch gain limit */
Word16 *gain_pit, /* o : Pitch gain, Q14 */
Word16 *gain_cod, /* o : Code gain, Q1 */
Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */
/* (for MR122 MA predictor update) */
Word16 *qua_ener /* o : quantized energy error, Q10 */
/* (for other MA predictor update) */
)
{
const Word16 *p;
Word16 i, j, index = 0;
Word16 gcode0, e_max, exp_code;
Word16 g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
Word16 coeff[5], coeff_lo[5];
Word16 exp_max[5];
Word32 L_tmp, dist_min;
const Word16 *table_gain;
Word16 table_len;
if ( sub (mode, MR102) == 0 || sub (mode, MR74) == 0 || sub (mode, MR67) == 0)
{
table_len = VQ_SIZE_HIGHRATES;
table_gain = table_gain_highrates;
}
else
{
table_len = VQ_SIZE_LOWRATES;
table_gain = table_gain_lowrates;
}
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = 2^exp_gcode0 + 2^frac_gcode0 *
* *
* gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0) *
*-------------------------------------------------------------------*/
gcode0 = extract_l(Pow2(14, frac_gcode0));
/*-------------------------------------------------------------------*
* Scaling considerations: *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
*-------------------------------------------------------------------*/
/*
* The error energy (sum) to be minimized consists of five terms, t[0..4].
*
* t[0] = gp^2 * <y1 y1>
* t[1] = -2*gp * <xn y1>
* t[2] = gc^2 * <y2 y2>
* t[3] = -2*gc * <xn y2>
* t[4] = 2*gp*gc * <y1 y2>
*
*/
/* determine the scaling exponent for g_code: ec = ec0 - 11 */
exp_code = sub(exp_gcode0, 11);
/* calculate exp_max[i] = s[i]-1 */
exp_max[0] = sub(exp_coeff[0], 13);
exp_max[1] = sub(exp_coeff[1], 14);
exp_max[2] = add(exp_coeff[2], add(15, shl(exp_code, 1)));
exp_max[3] = add(exp_coeff[3], exp_code);
exp_max[4] = add(exp_coeff[4], add(1, exp_code));
/*-------------------------------------------------------------------*
* Find maximum exponent: *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* *
* For the sum operation, all terms must have the same scaling; *
* that scaling should be low enough to prevent overflow. There- *
* fore, the maximum scale is determined and all coefficients are *
* re-scaled: *
* *
* e_max = max(exp_max[i]) + 1; *
* e = exp_max[i]-e_max; e <= 0! *
* c[i] = c[i]*2^e *
*-------------------------------------------------------------------*/
e_max = exp_max[0];
for (i = 1; i < 5; i++)
{
if (sub(exp_max[i], e_max) > 0)
{
e_max = exp_max[i];
}
}
e_max = add(e_max, 1); /* To avoid overflow */
for (i = 0; i < 5; i++) {
j = sub(e_max, exp_max[i]);
L_tmp = L_deposit_h(frac_coeff[i]);
L_tmp = L_shr(L_tmp, j);
L_Extract(L_tmp, &coeff[i], &coeff_lo[i]);
}
/*-------------------------------------------------------------------*
* Codebook search: *
* ~~~~~~~~~~~~~~~~ *
* *
* For each pair (g_pitch, g_fac) in the table calculate the *
* terms t[0..4] and sum them up; the result is the mean squared *
* error for the quantized gains from the table. The index for the *
* minimum MSE is stored and finally used to retrieve the quantized *
* gains *
*-------------------------------------------------------------------*/
/* start with "infinite" MSE */
dist_min = MAX_32;
p = &table_gain[0];
for (i = 0; i < table_len; i++)
{
g_pitch = *p++;
g_code = *p++; /* this is g_fac */
p++; /* skip log2(g_fac) */
p++; /* skip 20*log10(g_fac) */
if (sub(g_pitch, gp_limit) <= 0)
{
g_code = mult(g_code, gcode0);
g2_pitch = mult(g_pitch, g_pitch);
g2_code = mult(g_code, g_code);
g_pit_cod = mult(g_code, g_pitch);
L_tmp = Mpy_32_16(coeff[0], coeff_lo[0], g2_pitch);
L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lo[1], g_pitch));
L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lo[2], g2_code));
L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lo[3], g_code));
L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lo[4], g_pit_cod));
/* store table index if MSE for this index is lower
than the minimum MSE seen so far */
if (L_sub(L_tmp, dist_min) < (Word32) 0)
{
dist_min = L_tmp;
index = i;
}
}
}
/*------------------------------------------------------------------*
* read quantized gains and new values for MA predictor memories *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
*------------------------------------------------------------------*/
/* Read the quantized gains */
p = &table_gain[shl (index, 2)];
*gain_pit = *p++;
g_code = *p++;
*qua_ener_MR122 = *p++;
*qua_ener = *p;
/*------------------------------------------------------------------*
* calculate final fixed codebook gain: *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* *
* gc = gc0 * g *
*------------------------------------------------------------------*/
L_tmp = L_mult(g_code, gcode0);
L_tmp = L_shr(L_tmp, sub(10, exp_gcode0));
*gain_cod = extract_h(L_tmp);
return index;
}
//! Returns the distance between the two given 2D points.
template <typename T> force_inline T getDistance(const point2d<T> &PosA, const point2d<T> &PosB)
{
return Sqrt(Pow2(PosB.X - PosA.X) + Pow2(PosB.Y - PosA.Y));
}
/*
**************************************************************************
*
* Function : dtx_dec
*
**************************************************************************
*/
int dtx_dec(
dtx_decState *st, /* i/o : State struct */
Word16 mem_syn[], /* i/o : AMR decoder state */
D_plsfState* lsfState, /* i/o : decoder lsf states */
gc_predState* predState, /* i/o : prediction states */
Cb_gain_averageState* averState, /* i/o : CB gain average states */
enum DTXStateType new_state, /* i : new DTX state */
enum Mode mode, /* i : AMR mode */
Word16 parm[], /* i : Vector of synthesis parameters */
Word16 synth[], /* o : synthesised speech */
Word16 A_t[] /* o : decoded LP filter in 4 subframes*/
)
{
Word16 log_en_index;
Word16 i, j;
Word16 int_fac;
Word32 L_log_en_int;
Word16 lsp_int[M];
Word16 log_en_int_e;
Word16 log_en_int_m;
Word16 level;
Word16 acoeff[M + 1];
Word16 refl[M];
Word16 pred_err;
Word16 ex[L_SUBFR];
Word16 ma_pred_init;
Word16 log_pg_e, log_pg_m;
Word16 log_pg;
Flag negative;
Word16 lsf_mean;
Word32 L_lsf_mean;
Word16 lsf_variab_index;
Word16 lsf_variab_factor;
Word16 lsf_int[M];
Word16 lsf_int_variab[M];
Word16 lsp_int_variab[M];
Word16 acoeff_variab[M + 1];
Word16 lsf[M];
Word32 L_lsf[M];
Word16 ptr;
Word16 tmp_int_length;
/* This function is called if synthesis state is not SPEECH
* the globally passed inputs to this function are
* st->sid_frame
* st->valid_data
* st->dtxHangoverAdded
* new_state (SPEECH, DTX, DTX_MUTE)
*/
test(); test();
if ((st->dtxHangoverAdded != 0) &&
(st->sid_frame != 0))
{
/* sid_first after dtx hangover period */
/* or sid_upd after dtxhangover */
/* set log_en_adjust to correct value */
st->log_en_adjust = dtx_log_en_adjust[mode];
ptr = add(st->lsf_hist_ptr, M); move16();
test();
if (sub(ptr, 80) == 0)
{
ptr = 0; move16();
}
Copy( &st->lsf_hist[st->lsf_hist_ptr],&st->lsf_hist[ptr],M);
ptr = add(st->log_en_hist_ptr,1); move16();
test();
if (sub(ptr, DTX_HIST_SIZE) == 0)
{
ptr = 0; move16();
}
move16();
st->log_en_hist[ptr] = st->log_en_hist[st->log_en_hist_ptr]; /* Q11 */
/* compute mean log energy and lsp *
* from decoded signal (SID_FIRST) */
st->log_en = 0; move16();
for (i = 0; i < M; i++)
{
L_lsf[i] = 0; move16();
}
/* average energy and lsp */
for (i = 0; i < DTX_HIST_SIZE; i++)
{
st->log_en = add(st->log_en,
shr(st->log_en_hist[i],3));
for (j = 0; j < M; j++)
{
L_lsf[j] = L_add(L_lsf[j],
L_deposit_l(st->lsf_hist[i * M + j]));
}
}
for (j = 0; j < M; j++)
{
lsf[j] = extract_l(L_shr(L_lsf[j],3)); /* divide by 8 */ move16();
}
Lsf_lsp(lsf, st->lsp, M);
/* make log_en speech coder mode independent */
/* added again later before synthesis */
st->log_en = sub(st->log_en, st->log_en_adjust);
/* compute lsf variability vector */
Copy(st->lsf_hist, st->lsf_hist_mean, 80);
for (i = 0; i < M; i++)
{
L_lsf_mean = 0; move32();
/* compute mean lsf */
for (j = 0; j < 8; j++)
{
L_lsf_mean = L_add(L_lsf_mean,
L_deposit_l(st->lsf_hist_mean[i+j*M]));
}
lsf_mean = extract_l(L_shr(L_lsf_mean, 3)); move16();
/* subtract mean and limit to within reasonable limits *
* moreover the upper lsf's are attenuated */
for (j = 0; j < 8; j++)
{
/* subtract mean */
st->lsf_hist_mean[i+j*M] =
sub(st->lsf_hist_mean[i+j*M], lsf_mean);
/* attenuate deviation from mean, especially for upper lsf's */
st->lsf_hist_mean[i+j*M] =
mult(st->lsf_hist_mean[i+j*M], lsf_hist_mean_scale[i]);
/* limit the deviation */
test();
if (st->lsf_hist_mean[i+j*M] < 0)
{
negative = 1; move16();
}
else
{
negative = 0; move16();
}
st->lsf_hist_mean[i+j*M] = abs_s(st->lsf_hist_mean[i+j*M]);
/* apply soft limit */
test();
if (sub(st->lsf_hist_mean[i+j*M], 655) > 0)
{
st->lsf_hist_mean[i+j*M] =
add(655, shr(sub(st->lsf_hist_mean[i+j*M], 655), 2));
}
/* apply hard limit */
test();
if (sub(st->lsf_hist_mean[i+j*M], 1310) > 0)
{
st->lsf_hist_mean[i+j*M] = 1310; move16();
}
test();
if (negative != 0)
{
st->lsf_hist_mean[i+j*M] = -st->lsf_hist_mean[i+j*M];move16();
}
}
}
}
test();
if (st->sid_frame != 0 )
{
/* Set old SID parameters, always shift */
/* even if there is no new valid_data */
Copy(st->lsp, st->lsp_old, M);
st->old_log_en = st->log_en; move16();
test();
if (st->valid_data != 0 ) /* new data available (no CRC) */
{
/* Compute interpolation factor, since the division only works *
* for values of since_last_sid < 32 we have to limit the *
* interpolation to 32 frames */
tmp_int_length = st->since_last_sid; move16();
st->since_last_sid = 0; move16();
test();
if (sub(tmp_int_length, 32) > 0)
{
tmp_int_length = 32; move16();
}
test();
if (sub(tmp_int_length, 2) >= 0)
{
move16();
st->true_sid_period_inv = div_s(1 << 10, shl(tmp_int_length, 10));
}
else
{
st->true_sid_period_inv = 1 << 14; /* 0.5 it Q15 */ move16();
}
Init_D_plsf_3(lsfState, parm[0]); /* temporay initialization */
D_plsf_3(lsfState, MRDTX, 0, &parm[1], st->lsp);
Set_zero(lsfState->past_r_q, M); /* reset for next speech frame */
log_en_index = parm[4]; move16();
/* Q11 and divide by 4 */
st->log_en = shl(log_en_index, (11 - 2)); move16();
/* Subtract 2.5 in Q11 */
st->log_en = sub(st->log_en, (2560 * 2));
/* Index 0 is reserved for silence */
test();
if (log_en_index == 0)
{
st->log_en = MIN_16; move16();
}
/* no interpolation at startup after coder reset */
/* or when SID_UPD has been received right after SPEECH */
test(); test();
if ((st->data_updated == 0) ||
(sub(st->dtxGlobalState, SPEECH) == 0)
)
{
Copy(st->lsp, st->lsp_old, M);
st->old_log_en = st->log_en; move16();
}
} /* endif valid_data */
/* initialize gain predictor memory of other modes */
ma_pred_init = sub(shr(st->log_en,1), 9000); move16();
test();
if (ma_pred_init > 0)
{
ma_pred_init = 0; move16();
}
test();
if (sub(ma_pred_init, -14436) < 0)
{
ma_pred_init = -14436; move16();
}
predState->past_qua_en[0] = ma_pred_init; move16();
predState->past_qua_en[1] = ma_pred_init; move16();
predState->past_qua_en[2] = ma_pred_init; move16();
predState->past_qua_en[3] = ma_pred_init; move16();
/* past_qua_en for other modes than MR122 */
ma_pred_init = mult(5443, ma_pred_init);
/* scale down by factor 20*log10(2) in Q15 */
predState->past_qua_en_MR122[0] = ma_pred_init; move16();
predState->past_qua_en_MR122[1] = ma_pred_init; move16();
predState->past_qua_en_MR122[2] = ma_pred_init; move16();
predState->past_qua_en_MR122[3] = ma_pred_init; move16();
} /* endif sid_frame */
/* CN generation */
/* recompute level adjustment factor Q11 *
* st->log_en_adjust = 0.9*st->log_en_adjust + *
* 0.1*dtx_log_en_adjust[mode]); */
move16();
st->log_en_adjust = add(mult(st->log_en_adjust, 29491),
shr(mult(shl(dtx_log_en_adjust[mode],5),3277),5));
/* Interpolate SID info */
int_fac = shl(add(1,st->since_last_sid), 10); /* Q10 */ move16();
int_fac = mult(int_fac, st->true_sid_period_inv); /* Q10 * Q15 -> Q10 */
/* Maximize to 1.0 in Q10 */
test();
if (sub(int_fac, 1024) > 0)
{
int_fac = 1024; move16();
}
int_fac = shl(int_fac, 4); /* Q10 -> Q14 */
L_log_en_int = L_mult(int_fac, st->log_en); /* Q14 * Q11->Q26 */ move32();
for(i = 0; i < M; i++)
{
lsp_int[i] = mult(int_fac, st->lsp[i]);/* Q14 * Q15 -> Q14 */ move16();
}
int_fac = sub(16384, int_fac); /* 1-k in Q14 */ move16();
/* (Q14 * Q11 -> Q26) + Q26 -> Q26 */
L_log_en_int = L_mac(L_log_en_int, int_fac, st->old_log_en);
for(i = 0; i < M; i++)
{
/* Q14 + (Q14 * Q15 -> Q14) -> Q14 */
lsp_int[i] = add(lsp_int[i], mult(int_fac, st->lsp_old[i])); move16();
lsp_int[i] = shl(lsp_int[i], 1); /* Q14 -> Q15 */ move16();
}
/* compute the amount of lsf variability */
lsf_variab_factor = sub(st->log_pg_mean,2457); /* -0.6 in Q12 */ move16();
/* *0.3 Q12*Q15 -> Q12 */
lsf_variab_factor = sub(4096, mult(lsf_variab_factor, 9830));
/* limit to values between 0..1 in Q12 */
test();
if (sub(lsf_variab_factor, 4096) > 0)
{
lsf_variab_factor = 4096; move16();
}
test();
if (lsf_variab_factor < 0)
{
lsf_variab_factor = 0; move16();
}
lsf_variab_factor = shl(lsf_variab_factor, 3); /* -> Q15 */ move16();
/* get index of vector to do variability with */
lsf_variab_index = pseudonoise(&st->L_pn_seed_rx, 3); move16();
/* convert to lsf */
Lsp_lsf(lsp_int, lsf_int, M);
/* apply lsf variability */
Copy(lsf_int, lsf_int_variab, M);
for(i = 0; i < M; i++)
{
move16();
lsf_int_variab[i] = add(lsf_int_variab[i],
mult(lsf_variab_factor,
st->lsf_hist_mean[i+lsf_variab_index*M]));
}
/* make sure that LSP's are ordered */
Reorder_lsf(lsf_int, LSF_GAP, M);
Reorder_lsf(lsf_int_variab, LSF_GAP, M);
/* copy lsf to speech decoders lsf state */
Copy(lsf_int, lsfState->past_lsf_q, M);
/* convert to lsp */
Lsf_lsp(lsf_int, lsp_int, M);
Lsf_lsp(lsf_int_variab, lsp_int_variab, M);
/* Compute acoeffs Q12 acoeff is used for level *
* normalization and postfilter, acoeff_variab is *
* used for synthesis filter *
* by doing this we make sure that the level *
* in high frequenncies does not jump up and down */
Lsp_Az(lsp_int, acoeff);
Lsp_Az(lsp_int_variab, acoeff_variab);
/* For use in postfilter */
Copy(acoeff, &A_t[0], M + 1);
Copy(acoeff, &A_t[M + 1], M + 1);
Copy(acoeff, &A_t[2 * (M + 1)], M + 1);
Copy(acoeff, &A_t[3 * (M + 1)], M + 1);
/* Compute reflection coefficients Q15 */
A_Refl(&acoeff[1], refl);
/* Compute prediction error in Q15 */
pred_err = MAX_16; /* 0.99997 in Q15 */ move16();
for (i = 0; i < M; i++)
{
pred_err = mult(pred_err, sub(MAX_16, mult(refl[i], refl[i])));
}
/* compute logarithm of prediction gain */
Log2(L_deposit_l(pred_err), &log_pg_e, &log_pg_m);
/* convert exponent and mantissa to Word16 Q12 */
log_pg = shl(sub(log_pg_e,15), 12); /* Q12 */ move16();
log_pg = shr(sub(0,add(log_pg, shr(log_pg_m, 15-12))), 1); move16();
st->log_pg_mean = add(mult(29491,st->log_pg_mean),
mult(3277, log_pg)); move16();
/* Compute interpolated log energy */
L_log_en_int = L_shr(L_log_en_int, 10); /* Q26 -> Q16 */ move32();
/* Add 4 in Q16 */
L_log_en_int = L_add(L_log_en_int, 4 * 65536L); move32();
/* subtract prediction gain */
L_log_en_int = L_sub(L_log_en_int, L_shl(L_deposit_l(log_pg), 4));move32();
/* adjust level to speech coder mode */
L_log_en_int = L_add(L_log_en_int,
L_shl(L_deposit_l(st->log_en_adjust), 5)); move32();
log_en_int_e = extract_h(L_log_en_int); move16();
move16();
log_en_int_m = extract_l(L_shr(L_sub(L_log_en_int,
L_deposit_h(log_en_int_e)), 1));
level = extract_l(Pow2(log_en_int_e, log_en_int_m)); /* Q4 */ move16();
for (i = 0; i < 4; i++)
{
/* Compute innovation vector */
build_CN_code(&st->L_pn_seed_rx, ex);
for (j = 0; j < L_SUBFR; j++)
{
ex[j] = mult(level, ex[j]); move16();
}
/* Synthesize */
Syn_filt(acoeff_variab, ex, &synth[i * L_SUBFR], L_SUBFR,
mem_syn, 1);
} /* next i */
/* reset codebook averaging variables */
averState->hangVar = 20; move16();
averState->hangCount = 0; move16();
test();
if (sub(new_state, DTX_MUTE) == 0)
{
/* mute comfort noise as it has been quite a long time since
* last SID update was performed */
tmp_int_length = st->since_last_sid; move16();
test();
if (sub(tmp_int_length, 32) > 0)
{
tmp_int_length = 32; move16();
}
/* safety guard against division by zero */
test();
if(tmp_int_length <= 0) {
tmp_int_length = 8; move16();
}
move16();
st->true_sid_period_inv = div_s(1 << 10, shl(tmp_int_length, 10));
st->since_last_sid = 0; move16();
Copy(st->lsp, st->lsp_old, M);
st->old_log_en = st->log_en; move16();
/* subtract 1/8 in Q11 i.e -6/8 dB */
st->log_en = sub(st->log_en, 256); move16();
}
/* reset interpolation length timer
* if data has been updated. */
test(); test(); test(); test();
if ((st->sid_frame != 0) &&
((st->valid_data != 0) ||
((st->valid_data == 0) && (st->dtxHangoverAdded) != 0)))
{
st->since_last_sid = 0; move16();
st->data_updated = 1; move16();
}
return 0;
}
void CIVLiveViewer::PaintArrowLine(
const HDC dc,
const RECT& rect,
WPG_TripwireEventDescription& Line,
DWORD dwShowCount1,
DWORD dwShowCount2 )
{
int x[2], y[2];
ViewHelper::TranslateWPGPoint(rect, Line.startPoint, x[0], y[0]);
ViewHelper::TranslateWPGPoint(rect, Line.endPoint, x[1], y[1]);
::MoveToEx(dc, x[0], y[0], NULL);
::LineTo(dc, x[1], y[1]);
// 利用垂直和两点的距离算出两个点的坐标
POINT MedPoint;
MedPoint.x = (x[0] + x[1])/2;
MedPoint.y = (y[0] + y[1])/2;
double m = sqrt(
Pow2(double(MedPoint.x-x[0])) + Pow2(double(MedPoint.y-y[0])) );
if ( m < 0.0001 )
{
return;
}
POINT A[2];
A[0].x = MedPoint.x + long(ArrowLineLen/m*(y[0]-MedPoint.y));
A[0].y = MedPoint.y - long(ArrowLineLen/m*(x[0]-MedPoint.x));
A[1].x = MedPoint.x - long(ArrowLineLen/m*(y[0]-MedPoint.y));
A[1].y = MedPoint.y + long(ArrowLineLen/m*(x[0]-MedPoint.x));
// dc.p
double o;
long nXoffset = A[0].x-A[1].x;
long nYoffset = A[0].y-A[1].y;
if (nXoffset == 0)
{
o = nYoffset > 0 ? M_PI_2 : (-M_PI_2);
}
else
{
double tanValue = 1.0*(nYoffset)/nXoffset;
o = atan(tanValue);
}
// 修正两种特殊情况，因为我把它的角度转换为[0, pi]区间的，
// atan取值范围为(-pi/2, pi/2)
if ( o < 0 )
{
o += M_PI;
}
else if ( o == 0 && nXoffset > 0 )
{
o = M_PI;
}
int nOldMode = ::SetBkMode(dc, TRANSPARENT);
COLORREF nOldCol = ::SetTextColor(dc, Font_Color);
//HGDIOBJ hOldFont = ::SelectObject(dc, m_hFont);
bool bUp = (A[0].y > A[1].y);
//bool bUp = (A[0].x > A[1].x) ^ (A[0].y < A[1].y);
//bool bUp = GetPointRLineState(BeginPoint, EndPoint, A[1]) > 0;
//TRACE("%d\n", bUp);
if ( Line.direction == ANY_DIRECTION ||
Line.direction == RIGHT_TO_LEFT )
{
::MoveToEx(dc, MedPoint.x, MedPoint.y, NULL);
::LineTo(dc, A[0].x, A[0].y);
DrawArrow(dc, A[0], ArrowHeadLen, o, bUp, dwShowCount1);
}
if ( Line.direction == ANY_DIRECTION ||
Line.direction == LEFT_TO_RIGHT ) // Line_Show_Left
{
::MoveToEx(dc, MedPoint.x, MedPoint.y, NULL);
::LineTo(dc, A[1].x, A[1].y);
DrawArrow(dc, A[1], ArrowHeadLen, o, !bUp, dwShowCount2);
}
//SelectObject(dc, hOldFont);
::SetTextColor(dc, nOldCol);
::SetBkMode(dc, nOldMode);
}
/*************************************************************************
*
* FUNCTION: MR795_gain_quant
*
* PURPOSE: pitch and codebook quantization for MR795
*
*************************************************************************/
void
MR795_gain_quant(
GainAdaptState *adapt_st, /* i/o: gain adapter state structure */
Word16 res[], /* i : LP residual, Q0 */
Word16 exc[], /* i : LTP excitation (unfiltered), Q0 */
Word16 code[], /* i : CB innovation (unfiltered), Q13 */
Word16 frac_coeff[], /* i : coefficients (5), Q15 */
Word16 exp_coeff[], /* i : energy coefficients (5), Q0 */
/* coefficients from calc_filt_ener() */
Word16 exp_code_en, /* i : innovation energy (exponent), Q0 */
Word16 frac_code_en, /* i : innovation energy (fraction), Q15 */
Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */
Word16 frac_gcode0, /* i : predicted CB gain (fraction), Q15 */
Word16 L_subfr, /* i : Subframe length */
Word16 cod_gain_frac, /* i : opt. codebook gain (fraction),Q15 */
Word16 cod_gain_exp, /* i : opt. codebook gain (exponent), Q0 */
Word16 gp_limit, /* i : pitch gain limit */
Word16 *gain_pit, /* i/o: Pitch gain, Q14 */
Word16 *gain_cod, /* o : Code gain, Q1 */
Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */
/* (for MR122 MA predictor update) */
Word16 *qua_ener, /* o : quantized energy error, Q10 */
/* (for other MA predictor update) */
Word16 **anap /* o : Index of quantization */
/* (first gain pitch, then code pitch)*/
)
{
Word16 frac_en[4];
Word16 exp_en[4];
Word16 ltpg, alpha, gcode0;
Word16 g_pitch_cand[3]; /* pitch gain candidates Q14 */
Word16 g_pitch_cind[3]; /* pitch gain indices Q0 */
Word16 gain_pit_index;
Word16 gain_cod_index;
Word16 exp;
Word16 gain_cod_unq; /* code gain (unq.) Q(10-exp_gcode0) */
/* get list of candidate quantized pitch gain values
* and corresponding quantization indices
*/
gain_pit_index = q_gain_pitch (MR795, gp_limit, gain_pit,
g_pitch_cand, g_pitch_cind);
/* function result */
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = 2^exp_gcode0 + 2^frac_gcode0 *
* *
* gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0) *
*-------------------------------------------------------------------*/
gcode0 = extract_l(Pow2(14, frac_gcode0)); /* Q14 */
/* pre-quantization of codebook gain
* (using three pitch gain candidates);
* result: best guess of pitch gain and code gain
*/
MR795_gain_code_quant3(
exp_gcode0, gcode0, g_pitch_cand, g_pitch_cind,
frac_coeff, exp_coeff,
gain_pit, &gain_pit_index, gain_cod, &gain_cod_index,
qua_ener_MR122, qua_ener);
/* calculation of energy coefficients and LTP coding gain */
calc_unfilt_energies(res, exc, code, *gain_pit, L_subfr,
frac_en, exp_en, <pg);
/* run gain adaptor, calculate alpha factor to balance LTP/CB gain
* (this includes the gain adaptor update)
* Note: ltpg = 0 if frac_en[0] == 0, so the update is OK in that case
*/
gain_adapt(adapt_st, ltpg, *gain_cod, &alpha);
/* if this is a very low energy signal (threshold: see
* calc_unfilt_energies) or alpha <= 0 then don't run the modified quantizer
*/
if (frac_en[0] != 0 && alpha > 0)
{
/* innovation energy <cod cod> was already computed in gc_pred() */
/* (this overwrites the LtpResEn which is no longer needed) */
frac_en[3] = frac_code_en;
exp_en[3] = exp_code_en;
/* store optimum codebook gain in Q(10-exp_gcode0) */
exp = add (sub (cod_gain_exp, exp_gcode0), 10);
gain_cod_unq = shl (cod_gain_frac, exp);
/* run quantization with modified criterion */
gain_cod_index = MR795_gain_code_quant_mod(
*gain_pit, exp_gcode0, gcode0,
frac_en, exp_en, alpha, gain_cod_unq,
gain_cod, qua_ener_MR122, qua_ener); /* function result */
}
*(*anap)++ = gain_pit_index;
*(*anap)++ = gain_cod_index;
}
/*----------------------------------------------------------------------------
; FUNCTION CODE
----------------------------------------------------------------------------*/
void d_gain_code(
gc_predState *pred_state, /* i/o : MA predictor state */
enum Mode mode, /* i : AMR mode (MR795 or MR122) */
Word16 index, /* i : received quantization index */
Word16 code[], /* i : innovation codevector */
const Word16* qua_gain_code_ptr, /* i : Pointer to read-only table */
Word16 *gain_code, /* o : decoded innovation gain */
Flag *pOverflow
)
{
Word16 gcode0, exp, frac;
const Word16 *p;
Word16 qua_ener_MR122, qua_ener;
Word16 exp_inn_en;
Word16 frac_inn_en;
Word32 L_tmp;
Word16 tbl_tmp;
Word16 temp;
/*-------------- Decode codebook gain ---------------*/
/*-------------------------------------------------------------------*
* predict codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~ *
* gc0 = Pow2(int(d)+frac(d)) *
* = 2^exp + 2^frac *
* *
*-------------------------------------------------------------------*/
gc_pred(pred_state, mode, code, &exp, &frac,
&exp_inn_en, &frac_inn_en, pOverflow);
index &= 31; /* index < 32, to avoid buffer overflow */
tbl_tmp = index + (index << 1);
p = &qua_gain_code_ptr[tbl_tmp];
/* Different scalings between MR122 and the other modes */
temp = sub((Word16)mode, (Word16)MR122, pOverflow);
if (temp == 0)
{
gcode0 = (Word16)(Pow2(exp, frac, pOverflow)); /* predicted gain */
gcode0 = shl(gcode0, 4, pOverflow);
*gain_code = shl(mult(gcode0, *p++, pOverflow), 1, pOverflow);
}
else
{
gcode0 = (Word16)(Pow2(14, frac, pOverflow));
L_tmp = L_mult(*p++, gcode0, pOverflow);
L_tmp = L_shr(L_tmp, sub(9, exp, pOverflow), pOverflow);
*gain_code = (Word16)(L_tmp >> 16); /* Q1 */
}
/*-------------------------------------------------------------------*
* update table of past quantized energies *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
*-------------------------------------------------------------------*/
qua_ener_MR122 = *p++;
qua_ener = *p++;
gc_pred_update(pred_state, qua_ener_MR122, qua_ener);
return;
}
Word16 vad2 (Word16 * farray_ptr, vadState2 * st)
{
/*
* The channel table is defined below. In this table, the
* lower and higher frequency coefficients for each of the 16
* channels are specified. The table excludes the coefficients
* with numbers 0 (DC), 1, and 64 (Foldover frequency).
*/
const static Word16 ch_tbl[NUM_CHAN][2] =
{
{2, 3},
{4, 5},
{6, 7},
{8, 9},
{10, 11},
{12, 13},
{14, 16},
{17, 19},
{20, 22},
{23, 26},
{27, 30},
{31, 35},
{36, 41},
{42, 48},
{49, 55},
{56, 63}
};
/* channel energy scaling table - allows efficient division by number
* of DFT bins in the channel: 1/2, 1/3, 1/4, etc.
*/
const static Word16 ch_tbl_sh[NUM_CHAN] =
{
16384, 16384, 16384, 16384, 16384, 16384, 10923, 10923,
10923, 8192, 8192, 6554, 5461, 4681, 4681, 4096
};
/*
* The voice metric table is defined below. It is a non-
* linear table with a deadband near zero. It maps the SNR
* index (quantized SNR value) to a number that is a measure
* of voice quality.
*/
const static Word16 vm_tbl[90] =
{
2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
3, 3, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 7, 7, 7,
8, 8, 9, 9, 10, 10, 11, 12, 12, 13, 13, 14, 15,
15, 16, 17, 17, 18, 19, 20, 20, 21, 22, 23, 24,
24, 25, 26, 27, 28, 28, 29, 30, 31, 32, 33, 34,
35, 36, 37, 37, 38, 39, 40, 41, 42, 43, 44, 45,
46, 47, 48, 49, 50, 50, 50, 50, 50, 50, 50, 50,
50, 50
};
/* hangover as a function of peak SNR (3 dB steps) */
const static Word16 hangover_table[20] =
{
30, 30, 30, 30, 30, 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 8, 8, 8
};
/* burst sensitivity as a function of peak SNR (3 dB steps) */
const static Word16 burstcount_table[20] =
{
8, 8, 8, 8, 8, 8, 8, 8, 7, 6, 5, 4, 4, 4, 4, 4, 4, 4, 4, 4
};
/* voice metric sensitivity as a function of peak SNR (3 dB steps) */
const static Word16 vm_threshold_table[20] =
{
34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 40, 51, 71, 100, 139, 191, 257, 337, 432
};
/* State tables that use 22,9 or 27,4 scaling for ch_enrg[] */
const static Word16 noise_floor_chan[2] = {NOISE_FLOOR_CHAN_0, NOISE_FLOOR_CHAN_1};
const static Word16 min_chan_enrg[2] = {MIN_CHAN_ENRG_0, MIN_CHAN_ENRG_1};
const static Word16 ine_noise[2] = {INE_NOISE_0, INE_NOISE_1};
const static Word16 fbits[2] = {FRACTIONAL_BITS_0, FRACTIONAL_BITS_1};
const static Word16 state_change_shift_r[2] = {STATE_1_TO_0_SHIFT_R, STATE_0_TO_1_SHIFT_R};
/* Energy scale table given 30,1 input scaling (also account for -6 dB shift on input) */
const static Word16 enrg_norm_shift[2] = {(FRACTIONAL_BITS_0-1+2), (FRACTIONAL_BITS_1-1+2)};
/* Automatic variables */
Word32 Lenrg; /* scaled as 30,1 */
Word32 Ltne; /* scaled as 22,9 */
Word32 Ltce; /* scaled as 22,9 or 27,4 */
Word16 tne_db; /* scaled as 7,8 */
Word16 tce_db; /* scaled as 7,8 */
Word16 input_buffer[FRM_LEN]; /* used for block normalising input data */
Word16 data_buffer[FFT_LEN]; /* used for in-place FFT */
Word16 ch_snr[NUM_CHAN]; /* scaled as 7,8 */
Word16 ch_snrq; /* scaled as 15,0 (in 0.375 dB steps) */
Word16 vm_sum; /* scaled as 15,0 */
Word16 ch_enrg_dev; /* scaled as 7,8 */
Word32 Lpeak; /* maximum channel energy */
Word16 p2a_flag; /* flag to indicate spectral peak-to-average ratio > 10 dB */
Word16 ch_enrg_db[NUM_CHAN]; /* scaled as 7,8 */
Word16 ch_noise_db; /* scaled as 7,8 */
Word16 alpha; /* scaled as 0,15 */
Word16 one_m_alpha; /* scaled as 0,15 */
Word16 update_flag; /* set to indicate a background noise estimate update */
Word16 i, j, j1, j2; /* Scratch variables */
Word16 hi1, lo1;
Word32 Ltmp, Ltmp1, Ltmp2;
Word16 tmp;
Word16 normb_shift; /* block norm shift count */
Word16 ivad; /* intermediate VAD decision (return value) */
Word16 tsnrq; /* total signal-to-noise ratio (quantized 3 dB steps) scaled as 15,0 */
Word16 xt; /* instantaneous frame SNR in dB, scaled as 7,8 */
Word16 state_change;
/* Increment frame counter */
st->Lframe_cnt = L_add(st->Lframe_cnt, 1);
/* Block normalize the input */
normb_shift = block_norm(farray_ptr, input_buffer, FRM_LEN, FFT_HEADROOM);
/* Pre-emphasize the input data and store in the data buffer with the appropriate offset */
for (i = 0; i < DELAY; i++)
{
data_buffer[i] = 0; move16();
}
st->pre_emp_mem = shr_r(st->pre_emp_mem, sub(st->last_normb_shift, normb_shift));
st->last_normb_shift = normb_shift; move16();
data_buffer[DELAY] = add(input_buffer[0], mult(PRE_EMP_FAC, st->pre_emp_mem)); move16();
for (i = DELAY + 1, j = 1; i < DELAY + FRM_LEN; i++, j++)
{
data_buffer[i] = add(input_buffer[j], mult(PRE_EMP_FAC, input_buffer[j-1])); move16();
}
st->pre_emp_mem = input_buffer[FRM_LEN-1]; move16();
for (i = DELAY + FRM_LEN; i < FFT_LEN; i++)
{
data_buffer[i] = 0; move16();
}
/* Perform FFT on the data buffer */
r_fft(data_buffer);
/* Use normb_shift factor to determine the scaling of the energy estimates */
state_change = 0; move16();
test();
if (st->shift_state == 0)
{ test();
if (sub(normb_shift, -FFT_HEADROOM+2) <= 0)
{
state_change = 1; move16();
st->shift_state = 1; move16();
}
}
else
{ test();
if (sub(normb_shift, -FFT_HEADROOM+5) >= 0)
{
state_change = 1; move16();
st->shift_state = 0; move16();
}
}
/* Scale channel energy estimate */ test();
if (state_change)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->Lch_enrg[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[st->shift_state]); move32();
}
}
/* Estimate the energy in each channel */
test();
if (L_sub(st->Lframe_cnt, 1) == 0)
{
alpha = 32767; move16();
one_m_alpha = 0; move16();
}
else
{
alpha = CEE_SM_FAC; move16();
one_m_alpha = ONE_MINUS_CEE_SM_FAC; move16();
}
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Lenrg = 0; move16();
j1 = ch_tbl[i][0]; move16();
j2 = ch_tbl[i][1]; move16();
for (j = j1; j <= j2; j++)
{
Lenrg = L_mac(Lenrg, data_buffer[2 * j], data_buffer[2 * j]);
Lenrg = L_mac(Lenrg, data_buffer[2 * j + 1], data_buffer[2 * j + 1]);
}
/* Denorm energy & scale 30,1 according to the state */
Lenrg = L_shr_r(Lenrg, sub(shl(normb_shift, 1), enrg_norm_shift[st->shift_state]));
/* integrate over time: e[i] = (1-alpha)*e[i] + alpha*enrg/num_bins_in_chan */
tmp = mult(alpha, ch_tbl_sh[i]);
L_Extract (Lenrg, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, tmp);
L_Extract (st->Lch_enrg[i], &hi1, &lo1);
st->Lch_enrg[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, one_m_alpha)); move32();
test();
if (L_sub(st->Lch_enrg[i], min_chan_enrg[st->shift_state]) < 0)
{
st->Lch_enrg[i] = min_chan_enrg[st->shift_state]; move32();
}
}
/* Compute the total channel energy estimate (Ltce) */
Ltce = 0; move16();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltce = L_add(Ltce, st->Lch_enrg[i]);
}
/* Calculate spectral peak-to-average ratio, set flag if p2a > 10 dB */
Lpeak = 0; move32();
for (i = LO_CHAN+2; i <= HI_CHAN; i++) /* Sine waves not valid for low frequencies */
{ test();
if (L_sub(st->Lch_enrg [i], Lpeak) > 0)
{
Lpeak = st->Lch_enrg [i]; move32();
}
}
/* Set p2a_flag if peak (dB) > average channel energy (dB) + 10 dB */
/* Lpeak > Ltce/num_channels * 10^(10/10) */
/* Lpeak > (10/16)*Ltce */
L_Extract (Ltce, &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, 20480);
test();
if (L_sub(Lpeak, Ltmp) > 0)
{
p2a_flag = TRUE; move16();
}
else
{
p2a_flag = FALSE; move16();
}
/* Initialize channel noise estimate to either the channel energy or fixed level */
/* Scale the energy appropriately to yield state 0 (22,9) scaling for noise */
test();
if (L_sub(st->Lframe_cnt, 4) <= 0)
{ test();
if (p2a_flag == TRUE)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->Lch_noise[i] = INE_NOISE_0; move32();
}
}
else
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{ test();
if (L_sub(st->Lch_enrg[i], ine_noise[st->shift_state]) < 0)
{
st->Lch_noise[i] = INE_NOISE_0; move32();
}
else
{ test();
if (st->shift_state == 1)
{
st->Lch_noise[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[0]);
move32();
}
else
{
st->Lch_noise[i] = st->Lch_enrg[i]; move32();
}
}
}
}
}
/* Compute the channel energy (in dB), the channel SNRs, and the sum of voice metrics */
vm_sum = 0; move16();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
ch_enrg_db[i] = fn10Log10(st->Lch_enrg[i], fbits[st->shift_state]); move16();
ch_noise_db = fn10Log10(st->Lch_noise[i], FRACTIONAL_BITS_0);
ch_snr[i] = sub(ch_enrg_db[i], ch_noise_db); move16();
/* quantize channel SNR in 3/8 dB steps (scaled 7,8 => 15,0) */
/* ch_snr = round((snr/(3/8))>>8) */
/* = round(((0.6667*snr)<<2)>>8) */
/* = round((0.6667*snr)>>6) */
ch_snrq = shr_r(mult(21845, ch_snr[i]), 6);
/* Accumulate the sum of voice metrics */ test();
if (sub(ch_snrq, 89) < 0)
{ test();
if (ch_snrq > 0)
{
j = ch_snrq; move16();
}
else
{
j = 0; move16();
}
}
else
{
j = 89; move16();
}
vm_sum = add(vm_sum, vm_tbl[j]);
}
/* Initialize NOMINAL peak voice energy and average noise energy, calculate instantaneous SNR */
test(),test(),logic16();
if (L_sub(st->Lframe_cnt, 4) <= 0 || st->fupdate_flag == TRUE)
{
/* tce_db = (96 - 22 - 10*log10(64) (due to FFT)) scaled as 7,8 */
tce_db = 14320; move16();
st->negSNRvar = 0; move16();
st->negSNRbias = 0; move16();
/* Compute the total noise estimate (Ltne) */
Ltne = 0; move32();
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltne = L_add(Ltne, st->Lch_noise[i]);
}
/* Get total noise in dB */
tne_db = fn10Log10(Ltne, FRACTIONAL_BITS_0);
/* Initialise instantaneous and long-term peak signal-to-noise ratios */
xt = sub(tce_db, tne_db);
st->tsnr = xt; move16();
}
else
{
/* Calculate instantaneous frame signal-to-noise ratio */
/* xt = 10*log10( sum(2.^(ch_snr*0.1*log2(10)))/length(ch_snr) ) */
Ltmp1 = 0; move32();
for (i=LO_CHAN; i<=HI_CHAN; i++) {
/* Ltmp2 = ch_snr[i] * 0.1 * log2(10); (ch_snr scaled as 7,8) */
Ltmp2 = L_shr(L_mult(ch_snr[i], 10885), 8);
L_Extract(Ltmp2, &hi1, &lo1);
hi1 = add(hi1, 3); /* 2^3 to compensate for negative SNR */
Ltmp1 = L_add(Ltmp1, Pow2(hi1, lo1));
}
xt = fn10Log10(Ltmp1, 4+3); /* average by 16, inverse compensation 2^3 */
/* Estimate long-term "peak" SNR */ test(),test();
if (sub(xt, st->tsnr) > 0)
{
/* tsnr = 0.9*tsnr + 0.1*xt; */
st->tsnr = round(L_add(L_mult(29491, st->tsnr), L_mult(3277, xt)));
}
/* else if (xt > 0.625*tsnr) */
else if (sub(xt, mult(20480, st->tsnr)) > 0)
{
/* tsnr = 0.998*tsnr + 0.002*xt; */
st->tsnr = round(L_add(L_mult(32702, st->tsnr), L_mult(66, xt)));
}
}
/* Quantize the long-term SNR in 3 dB steps, limit to 0 <= tsnrq <= 19 */
tsnrq = shr(mult(st->tsnr, 10923), 8);
/* tsnrq = min(19, max(0, tsnrq)); */ test(),test();
if (sub(tsnrq, 19) > 0)
{
tsnrq = 19; move16();
}
else if (tsnrq < 0)
{
tsnrq = 0; move16();
}
/* Calculate the negative SNR sensitivity bias */
test();
if (xt < 0)
{
/* negSNRvar = 0.99*negSNRvar + 0.01*xt*xt; */
/* xt scaled as 7,8 => xt*xt scaled as 14,17, shift to 7,8 and round */
tmp = round(L_shl(L_mult(xt, xt), 7));
st->negSNRvar = round(L_add(L_mult(32440, st->negSNRvar), L_mult(328, tmp)));
/* if (negSNRvar > 4.0) negSNRvar = 4.0; */ test();
if (sub(st->negSNRvar, 1024) > 0)
{
st->negSNRvar = 1024; move16();
}
/* negSNRbias = max(12.0*(negSNRvar - 0.65), 0.0); */
tmp = mult_r(shl(sub(st->negSNRvar, 166), 4), 24576); test();
if (tmp < 0)
{
st->negSNRbias = 0; move16();
}
else
{
st->negSNRbias = shr(tmp, 8);
}
}
/* Determine VAD as a function of the voice metric sum and quantized SNR */
tmp = add(vm_threshold_table[tsnrq], st->negSNRbias); test();
if (sub(vm_sum, tmp) > 0)
{
ivad = 1; move16();
st->burstcount = add(st->burstcount, 1); test();
if (sub(st->burstcount, burstcount_table[tsnrq]) > 0)
{
st->hangover = hangover_table[tsnrq]; move16();
}
}
else
{
st->burstcount = 0; move16();
st->hangover = sub(st->hangover, 1); test();
if (st->hangover <= 0)
{
ivad = 0; move16();
st->hangover = 0; move16();
}
else
{
ivad = 1; move16();
}
}
/* Calculate log spectral deviation */
ch_enrg_dev = 0; move16();
test();
if (L_sub(st->Lframe_cnt, 1) == 0)
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
st->ch_enrg_long_db[i] = ch_enrg_db[i]; move16();
}
}
else
{
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
tmp = abs_s(sub(st->ch_enrg_long_db[i], ch_enrg_db[i]));
ch_enrg_dev = add(ch_enrg_dev, tmp);
}
}
/*
* Calculate long term integration constant as a function of instantaneous SNR
* (i.e., high SNR (tsnr dB) -> slower integration (alpha = HIGH_ALPHA),
* low SNR (0 dB) -> faster integration (alpha = LOW_ALPHA)
*/
/* alpha = HIGH_ALPHA - ALPHA_RANGE * (tsnr - xt) / tsnr, low <= alpha <= high */
tmp = sub(st->tsnr, xt); test(),logic16(),test(),test();
if (tmp <= 0 || st->tsnr <= 0)
{
alpha = HIGH_ALPHA; move16();
one_m_alpha = 32768L-HIGH_ALPHA; move16();
}
else if (sub(tmp, st->tsnr) > 0)
{
alpha = LOW_ALPHA; move16();
one_m_alpha = 32768L-LOW_ALPHA; move16();
}
else
{
tmp = div_s(tmp, st->tsnr);
alpha = sub(HIGH_ALPHA, mult(ALPHA_RANGE, tmp));
one_m_alpha = sub(32767, alpha);
}
/* Calc long term log spectral energy */
for (i = LO_CHAN; i <= HI_CHAN; i++)
{
Ltmp1 = L_mult(one_m_alpha, ch_enrg_db[i]);
Ltmp2 = L_mult(alpha, st->ch_enrg_long_db[i]);
st->ch_enrg_long_db[i] = round(L_add(Ltmp1, Ltmp2));
}
/* Set or clear the noise update flags */
update_flag = FALSE; move16();
st->fupdate_flag = FALSE; move16();
test(),test();
if (sub(vm_sum, UPDATE_THLD) <= 0)
{ test();
if (st->burstcount == 0)
{
update_flag = TRUE; move16();
st->update_cnt = 0; move16();
}
}
else if (L_sub(Ltce, noise_floor_chan[st->shift_state]) > 0)
{ test();
if (sub(ch_enrg_dev, DEV_THLD) < 0)
{ test();
if (p2a_flag == FALSE)
{ test();
if (st->LTP_flag == FALSE)
{
st->update_cnt = add(st->update_cnt, 1); test();
if (sub(st->update_cnt, UPDATE_CNT_THLD) >= 0)
{
update_flag = TRUE; move16();
st->fupdate_flag = TRUE; move16();
}
}
}
}
}
test();
if (sub(st->update_cnt, st->last_update_cnt) == 0)
{
st->hyster_cnt = add(st->hyster_cnt, 1);
}
else
{
st->hyster_cnt = 0; move16();
}
st->last_update_cnt = st->update_cnt; move16();
test();
if (sub(st->hyster_cnt, HYSTER_CNT_THLD) > 0)
{
st->update_cnt = 0; move16();
}
/* Conditionally update the channel noise estimates */
test();
if (update_flag == TRUE)
{
/* Check shift state */ test();
if (st->shift_state == 1)
{
/* get factor to shift ch_enrg[] from state 1 to 0 (noise always state 0) */
tmp = state_change_shift_r[0]; move16();
}
else
{
/* No shift if already state 0 */
tmp = 0; move16();
}
/* Update noise energy estimate */
for (i = LO_CHAN; i <= HI_CHAN; i++)
{ test();
/* integrate over time: en[i] = (1-alpha)*en[i] + alpha*e[n] */
/* (extract with shift compensation for state 1) */
L_Extract (L_shr(st->Lch_enrg[i], tmp), &hi1, &lo1);
Ltmp = Mpy_32_16(hi1, lo1, CNE_SM_FAC);
L_Extract (st->Lch_noise[i], &hi1, &lo1);
st->Lch_noise[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, ONE_MINUS_CNE_SM_FAC)); move32();
/* Limit low level noise */ test();
if (L_sub(st->Lch_noise[i], MIN_NOISE_ENRG_0) < 0)
{
st->Lch_noise[i] = MIN_NOISE_ENRG_0; move32();
}
}
}
return(ivad);
} /* end of vad2 () */
/*---------------------------------------------------------------------------*
* Function Gain_predict *
* ~~~~~~~~~~~~~~~~~~~~~~ *
* MA prediction is performed on the innovation energy (in dB with mean *
* removed). *
*---------------------------------------------------------------------------*/
void Gain_predict(
Word16 past_qua_en[], /* (i) Q10 :Past quantized energies */
Word16 code[], /* (i) Q13 :Innovative vector. */
Word16 L_subfr, /* (i) :Subframe length. */
Word16 *gcode0, /* (o) Qxx :Predicted codebook gain */
Word16 *exp_gcode0 /* (o) :Q-Format(gcode0) */
)
{
Word16 i, exp, frac;
Word32 L_tmp;
/*-------------------------------*
* Energy coming from code *
*-------------------------------*/
L_tmp = 0;
for(i=0; i<L_subfr; i++)
L_tmp = L_mac(L_tmp, code[i], code[i]);
/*-----------------------------------------------------------------*
* Compute: means_ener - 10log10(ener_code/ L_sufr) *
* Note: mean_ener change from 36 dB to 30 dB because input/2 *
* *
* = 30.0 - 10 log10( ener_code / lcode) + 10log10(2^27) *
* !!ener_code in Q27!! *
* = 30.0 - 3.0103 * log2(ener_code) + 10log10(40) + 10log10(2^27) *
* = 30.0 - 3.0103 * log2(ener_code) + 16.02 + 81.278 *
* = 127.298 - 3.0103 * log2(ener_code) *
*-----------------------------------------------------------------*/
Log2(L_tmp, &exp, &frac); /* Q27->Q0 ^Q0 ^Q15 */
L_tmp = Mpy_32_16(exp, frac, -24660); /* Q0 Q15 Q13 -> ^Q14 */
/* hi:Q0+Q13+1 */
/* lo:Q15+Q13-15+1 */
/* -24660[Q13]=-3.0103 */
L_tmp = L_mac(L_tmp, 32588, 32); /* 32588*32[Q14]=127.298 */
/*-----------------------------------------------------------------*
* Compute gcode0. *
* = Sum(i=0,3) pred[i]*past_qua_en[i] - ener_code + mean_ener *
*-----------------------------------------------------------------*/
L_tmp = L_shl(L_tmp, 10); /* From Q14 to Q24 */
for(i=0; i<4; i++)
L_tmp = L_mac(L_tmp, pred[i], past_qua_en[i]); /* Q13*Q10 ->Q24 */
*gcode0 = extract_h(L_tmp); /* From Q24 to Q8 */
/*-----------------------------------------------------------------*
* gcode0 = pow(10.0, gcode0/20) *
* = pow(2, 3.3219*gcode0/20) *
* = pow(2, 0.166*gcode0) *
*-----------------------------------------------------------------*/
L_tmp = L_mult(*gcode0, 5439); /* *0.166 in Q15, result in Q24*/
L_tmp = L_shr(L_tmp, 8); /* From Q24 to Q16 */
L_Extract(L_tmp, &exp, &frac); /* Extract exponent of gcode0 */
*gcode0 = extract_l(Pow2(14, frac)); /* Put 14 as exponent so that */
/* output of Pow2() will be: */
/* 16768 < Pow2() <= 32767 */
*exp_gcode0 = sub(14,exp);
}
INT16 q_gain_code ( /* Return quantization index */
INT16 code[], /* (i) : fixed codebook excitation */
INT16 lcode, /* (i) : codevector size */
INT16 *gain, /* (i/o) : quantized fixed codebook gain */
INT16 txdtx_ctrl,
INT16 i_subfr
)
{
INT16 i, index=0;
INT16 gcode0, err, err_min, exp, frac;
INT32 ener, ener_code;
register INT32 ener_code_hi=0;
register UINT32 ener_code_lo=0;
INT16 aver_gain;
static INT16 gcode0_CN;
VPP_EFR_PROFILE_FUNCTION_ENTER(q_gain_code);
if ((txdtx_ctrl & TX_SP_FLAG) != 0)
{
/*-------------------------------------------------------------------*
* energy of code: *
* ~~~~~~~~~~~~~~~ *
* ener_code(Q17) = 10 * Log10(energy/lcode) / constant *
* = 1/2 * Log2(energy/lcode) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
/* ener_code = log10(ener_code/lcode) / (20*log10(2)) */
//ener_code = 0;
ener_code_lo = 0;
for (i = 0; i < lcode; i++)
{
//ener_code = L_mac (ener_code, code[i], code[i]);
//ener_code = L_MAC(ener_code, code[i], code[i]);
VPP_MLA16(ener_code_hi,ener_code_lo,code[i], code[i]);
}
/* ener_code = ener_code / lcode */
ener_code = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);
//ener_code = L_mult (round(ener_code), 26214);
ener_code = L_MULT(ROUND(ener_code), 26214);
/* ener_code = 1/2 * Log2(ener_code) */
Log2 (ener_code, &exp, &frac);
//ener_code = L_Comp (sub (exp, 30), frac);
ener_code = L_Comp (SUB (exp, 30), frac);
/* predicted energy */
//ener = MEAN_ENER;
ener_code_lo = MEAN_ENER>>1;
ener_code_hi =0;
for (i = 0; i < 4; i++)
{
//ener = L_mac (ener, past_qua_en[i], pred[i]);
//ener = L_MAC(ener, past_qua_en[i], pred[i]);
VPP_MLA16(ener_code_hi,ener_code_lo, past_qua_en[i], pred[i]);
}
ener = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);
/*-------------------------------------------------------------------*
* predicted codebook gain *
* ~~~~~~~~~~~~~~~~~~~~~~~ *
* gcode0(Qx) = Pow10( (ener*constant - ener_code*constant) / 20 ) *
* = Pow2(ener-ener_code) *
* constant = 20*Log10(2) *
*-------------------------------------------------------------------*/
//ener = L_shr (L_SUB(ener, ener_code), 1);
ener = L_SHR_D(L_SUB(ener, ener_code), 1);
L_Extract (ener, &exp, &frac);
//gcode0 = extract_l (Pow2 (exp, frac)); /* predicted gain */
gcode0 = EXTRACT_L(Pow2 (exp, frac));
//gcode0 = shl (gcode0, 4);
gcode0 = SHL(gcode0, 4);
/*-------------------------------------------------------------------*
* Search for best quantizer *
*-------------------------------------------------------------------*/
//err_min = abs_s (sub (*gain, mult (gcode0, qua_gain_code[0])));
err_min = ABS_S(SUB (*gain, MULT(gcode0, qua_gain_code[0])));
index = 0;
for (i = 1; i < NB_QUA_CODE; i++)
{
//err = abs_s (sub (*gain, mult (gcode0, qua_gain_code[i])));
err = ABS_S(SUB (*gain, MULT (gcode0, qua_gain_code[i])));
//if (sub (err, err_min) < 0)
if (SUB (err, err_min) < 0)
{
err_min = err;
index = i;
}
}
//*gain = mult (gcode0, qua_gain_code[index]);
*gain = MULT(gcode0, qua_gain_code[index]);
/*------------------------------------------------------------------*
* update table of past quantized energies *
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ *
* past_qua_en(Q12) = 20 * Log10(qua_gain_code) / constant *
* = Log2(qua_gain_code) *
* constant = 20*Log10(2) *
*------------------------------------------------------------------*/
for (i = 3; i > 0; i--)
{
past_qua_en[i] = past_qua_en[i - 1];
}
//Log2 (L_deposit_l (qua_gain_code[index]), &exp, &frac);
Log2 (L_DEPOSIT_L(qua_gain_code[index]), &exp, &frac);
//past_qua_en[0] = shr (frac, 5);
past_qua_en[0] = SHR_D(frac, 5);
//past_qua_en[0] = add (past_qua_en[0], shl (sub (exp, 11), 10));
past_qua_en[0] = ADD (past_qua_en[0], SHL(SUB (exp, 11), 10));
update_gain_code_history_tx (*gain, gain_code_old_tx);
}
