/*
**
** File:    util_lbc.c
**
** Description: utility functions for the lbc codec
**
** Functions:
**
**  I/O functions:
**
**      Read_lbc()
**      Write_lbc()
**
**  High-pass filtering:
**
**      Rem_Dc()
**
**  Miscellaneous signal processing functions:
**
**      Vec_Norm()
**      Mem_Shift()
**      Comp_En()
**      Scale()
**
**  Bit stream packing/unpacking:
**
**      Line_Pack()
**      Line_Unpk()
**
**  Mathematical functions:
**
**      Sqrt_lbc()
**      Rand_lbc()
*/

/*
    ITU-T G.723 Speech Coder   ANSI-C Source Code     Version 5.00
    copyright (c) 1995, AudioCodes, DSP Group, France Telecom,
    Universite de Sherbrooke.  All rights reserved.
*/


#include <stdlib.h>
#include <stdio.h>

#include "typedef.h"
#include "basop.h"
#include "cst_lbc.h"
#include "tab_lbc.h"
#include "lbccodec.h"
#include "coder.h"
#include "decod.h"
#include "util_lbc.h"

/*
**
** Function:        Read_lbc()
**
** Description:     Read in a file
**
** Links to text:   Sections 2.2 & 4
**
** Arguments:
**
**  Word16 *Dpnt
**  int     Len
**  FILE *Fp
**
** Outputs:
**
**  Word16 *Dpnt
**
** Return value:    None
**
*/
void  Read_lbc( Word16 *Dpnt, int Len, FILE *Fp )
{
    int   i  ;

    for ( i = 0 ; i < Len ; i ++ )
        Dpnt[i] = (Word16) 0 ;

    fread ( (char *)Dpnt, sizeof(Word16), Len, Fp ) ;

    return;
}

/*
**
** Function:        Write_lbc()
**
** Description:     Write a file
**
** Links to text:   Section
**
** Arguments:
**
**  Word16 *Dpnt
**  int     Len
**  FILE *Fp
**
** Outputs:         None
**
** Return value:    None
**
*/
void    Write_lbc( Word16 *Dpnt, int Len, FILE *Fp )
{
	fwrite( (char *)Dpnt, sizeof(Word16), Len, Fp ) ;
}

void    Line_Wr( char *Line, FILE *Fp )
{
    Word16  Info ;
    int     Size   ;
	int		i;

    Info = Line[0] & (Word16)0x0003 ;

    /* Check frame type and rate informations */
    switch (Info) {

        case 0x0002 : {   /* SID frame */
            Size  = 4;
            break;
        }

        case 0x0003 : {  /* untransmitted silence frame */
            Size  = 1;
            break;
        }

        case 0x0001 : {   /* active frame, low rate */
            Size  = 20;
            break;
        }

        default : {  /* active frame, high rate */
            Size  = 24;
        }
    }
    fwrite( Line, Size , 1, Fp ) ;
}

int Line_Rd( char *Line, FILE *Fp )
{
    Word16  Info ;
    int     Size   ;

    if(fread( Line, 1,1, Fp ) != 1) return(-1);

    Info = Line[0] & (Word16)0x0003 ;
	/* Check frame type and rate informations */
    switch(Info) {

        /* Active frame, high rate */
        case 0 : {
            Size  = 23;
            break;
        }

        /* Active frame, low rate */
        case 1 : {
            Size  = 19;
            break;
        }

        /* Sid Frame */
        case 2 : {
            Size  = 3;
            break;
        }

        /* untransmitted */
        default : {
            return(0);
        }
    }
	fread( &Line[1], Size , 1, Fp ) ;
	
    return(0);
}

/*
**
** Function:        Rem_Dc()
**
** Description:     High-pass filtering
**
** Links to text:   Section 2.3
**
** Arguments:
**
**  Word16 *Dpnt
**
** Inputs:
**
**  CodStat.HpfZdl  FIR filter memory from previous frame (1 word)
**  CodStat.HpfPdl  IIR filter memory from previous frame (1 word)
**
** Outputs:
**
**  Word16 *Dpnt
**
** Return value:    None
**
*/
void  Rem_Dc( Word16 *Dpnt )
{
    int   i  ;


    Word32   Acc0,Acc1 ;

    if ( UseHp ) {
		for ( i = 0 ; i < Frame ; i ++ ) {

            /* Do the Fir and scale by 2 */
            Acc0 = L_mult( Dpnt[i], (Word16) 0x4000 ) ;
            Acc0 = L_mac ( Acc0, CodStat.HpfZdl, (Word16) 0xc000 ) ;
            CodStat.HpfZdl = Dpnt[i] ;

            /* Do the Iir part */
            Acc1 = L_mls( CodStat.HpfPdl, (Word16) 0x7f00 ) ;
            Acc0 = L_add( Acc0, Acc1 ) ;
            CodStat.HpfPdl = Acc0 ;
            Dpnt[i] = round(Acc0) ;
        }
    }
    else {
        for ( i = 0 ; i < Frame ; i ++ )
            Dpnt[i] = shr( Dpnt[i], (Word16) 1 ) ;
    }

    return;
}

/*
**
** Function:        Vec_Norm()
**
** Description:     Vector normalization
**
** Links to text:
**
** Arguments:
**
**  Word16 *Vect
**  Word16 Len
**
** Outputs:
**
**  Word16 *Vect
**
** Return value:  The power of 2 by which the data vector multiplyed.
**
*/
Word16  Vec_Norm( Word16 *Vect, Word16 Len )
{
    int   i  ;

    Word16  Acc0,Acc1   ;
    Word16  Exp   ;
    Word16  Rez ;
    Word32  Temp  ;

    static   short ShiftTable[16] = {
      0x0001 ,
      0x0002 ,
      0x0004 ,
      0x0008 ,
      0x0010 ,
      0x0020 ,
      0x0040 ,
      0x0080 ,
      0x0100 ,
      0x0200 ,
      0x0400 ,
      0x0800 ,
      0x1000 ,
      0x2000 ,
      0x4000 ,
      0x7fff
   } ;

    /* Find absolute maximum */
    Acc1 = (Word16) 0 ;
    for ( i = 0 ; i < Len ; i ++ ) {
        Acc0 = abs_s( Vect[i] ) ;
        if ( Acc0 > Acc1 )
            Acc1 = Acc0 ;
    }

    /* Get the shift count */
    Rez = norm_s( Acc1 ) ;
    Exp = ShiftTable[Rez] ;

    /* Normalize all the vector */
    for ( i = 0 ; i < Len ; i ++ ) {
        Temp = L_mult( Exp, Vect[i] ) ;
        Temp = L_shr( Temp, 4 ) ;
        Vect[i] = extract_l( Temp ) ;
    }

    Rez = sub( Rez, (Word16) 3) ;
    return Rez ;
}

/*
**
** Function:        Mem_Shift()
**
** Description:     Memory shift, update of the high-passed input speech signal
**
** Links to text:
**
** Arguments:
**
**  Word16 *PrevDat
**  Word16 *DataBuff
**
** Outputs:
**
**  Word16 *PrevDat
**  Word16 *DataBuff
**
** Return value:    None
**
*/
void  Mem_Shift( Word16 *PrevDat, Word16 *DataBuff )
{
    int   i  ;

    Word16   Dpnt[Frame+LpcFrame-SubFrLen] ;

    /*  Form Buffer  */
    for ( i = 0 ; i < LpcFrame-SubFrLen ; i ++ )
        Dpnt[i] = PrevDat[i] ;
    for ( i = 0 ; i < Frame ; i ++ )
        Dpnt[i+LpcFrame-SubFrLen] = DataBuff[i] ;

    /* Update PrevDat */
    for ( i = 0 ; i < LpcFrame-SubFrLen ; i ++ )
        PrevDat[i] = Dpnt[Frame+i] ;

    /* Update DataBuff */
    for ( i = 0 ; i < Frame ; i ++ )
        DataBuff[i] = Dpnt[(LpcFrame-SubFrLen)/2+i] ;

    return;
}

/*
**
** Function:        Line_Pack()
**
** Description:     Packing coded parameters in bitstream of 16-bit words
**
** Links to text:   Section 4
**
** Arguments:
**
**  LINEDEF *Line     Coded parameters for a frame
**  char    *Vout     bitstream chars
**  Word16   VadBit   Voice Activity Indicator
**
** Outputs:
**
**  Word16 *Vout
**
** Return value:    None
**
*/
void    Line_Pack( LINEDEF *Line, char *Vout, Word16 Ftyp )
{
    int     i ;
    int     BitCount ;

    Word16  BitStream[192] ;
    Word16 *Bsp = BitStream ;
    Word32  Temp ;

    /* Clear the output vector */
    for ( i = 0 ; i < 24 ; i ++ )
        Vout[i] = 0 ;

 /*
  * Add the coder rate info and frame type info to the 2 msb
  * of the first word of the frame.
  * The signaling is as follows:
  *     Ftyp  WrkRate => X1X0
  *       1     Rate63     00  :   High Rate
  *       1     Rate53     01  :   Low  Rate
  *       2       x        10  :   Silence Insertion Descriptor frame
  *       0       x        11  :   Used only for simulation of
  *                                 untransmitted silence frames
  */
    switch (Ftyp) {

        case 0 : {
            Temp = 0x00000003L;
            break;
        }

        case 2 : {
            Temp = 0x00000002L;
            break;
        }

        default : {
            if ( WrkRate == Rate63 )
                Temp = 0x00000000L ;
            else
                Temp = 0x00000001L ;
            break;
        }
    }

    /* Serialize Control info */
    Bsp = Par2Ser( Temp, Bsp, 2 ) ;


    /* Check for Speech/NonSpeech case */
    if ( Ftyp == 1 ) {

    /* 24 bit LspId */
    Temp = (*Line).LspId ;
    Bsp = Par2Ser( Temp, Bsp, 24 ) ;

 /*
  * Do the part common to both rates
  */

        /* Adaptive code book lags */
        Temp = (Word32) (*Line).Olp[0] - (Word32) PitchMin ;
        Bsp = Par2Ser( Temp, Bsp, 7 ) ;

        Temp = (Word32) (*Line).Sfs[1].AcLg ;
        Bsp = Par2Ser( Temp, Bsp, 2 ) ;

        Temp = (Word32) (*Line).Olp[1] - (Word32) PitchMin ;
        Bsp = Par2Ser( Temp, Bsp, 7 ) ;

        Temp = (Word32) (*Line).Sfs[3].AcLg ;
        Bsp = Par2Ser( Temp, Bsp, 2 ) ;

        /* Write combined 12 bit index of all the gains */
        for ( i = 0 ; i < SubFrames ; i ++ ) {
            Temp = (*Line).Sfs[i].AcGn*NumOfGainLev + (*Line).Sfs[i].Mamp ;
            if ( WrkRate == Rate63 )
                Temp += (Word32) (*Line).Sfs[i].Tran << 11 ;
            Bsp = Par2Ser( Temp, Bsp, 12 ) ;
            
            /*-->here AcGn(0..84),Mamp(0..23),Tran(0..1)*/
            /*
            in decode, if ( (WrkRate == Rate63) && (Line.Olp[i>>1] < (SubFrLen-2) ) )
            			Bound_AcGn = NbFilt085 ;
            */
		
        }

        /* Write all the Grid indices */
        for ( i = 0 ; i < SubFrames ; i ++ )
            *Bsp ++ = (*Line).Sfs[i].Grid ;

        /* High rate only part */
        if ( WrkRate == Rate63 ) {

            /* Write the reserved bit as 0 */