/*
 * ===================================================================
 *  TS 26.104
 *  REL-5 V5.4.0 2004-03
 *  REL-6 V6.1.0 2004-03
 *  3GPP AMR Floating-point Speech Codec
 * ===================================================================
 *
 */

/*
 * sp_dec.c
 *
 *
 * Project:
 *    AMR Floating-Point Codec
 *
 * Contains:
 *    This module contains all the functions needed decoding AMR
 *    encoder parameters to 16-bit speech samples
 *
 */
/*
 * include files
 */
#include <stdio.h>
#include <stdlib.h>
//#include <memory.h>
#include <string.h>
#include <math.h>
#include "sp_dec.h"
#include "rom_dec.h"

#define SPEECH		0
#define DTX			1
#define	DTX_MUTE	2

/*
 * Declare structure types
 */
/*enum DTXStateType
{
   SPEECH = 0, DTX, DTX_MUTE
};*/

/*
 * Decoder memory structure
 */
typedef struct
{
   /* history vector of past synthesis speech energy */
   Word32 frameEnergyHist[L_ENERGYHIST];


   /* state flags */
   Word16 bgHangover;   /* counter; number of frames after last speech frame */


}Bgn_scdState;
typedef struct
{
   Word32 hangCount;   /* counter; */
   /* history vector of past synthesis speech energy */
   Word32 cbGainHistory[L_CBGAINHIST];
   Word16 hangVar;   /* counter; */

}Cb_gain_averageState;
typedef struct
{
   Word32 lsp_meanSave[M];   /* Averaged LSPs saved for efficiency  */


}lsp_avgState;
typedef struct
{
   Word32 past_r_q[M];   /* Past quantized prediction error, Q15 */
   Word32 past_lsf_q[M];   /* Past dequantized lsfs, Q15 */


}D_plsfState;
typedef struct
{
   Word32 pbuf[5];
   Word32 past_gain_pit;
   Word32 prev_gp;


}ec_gain_pitchState;
typedef struct
{
   Word32 gbuf[5];
   Word32 past_gain_code;
   Word32 prev_gc;


}ec_gain_codeState;
typedef struct
{
   /*
    * normal MA predictor memory, Q10
    * (contains 20*log10(quaErr))
    */
   Word32 past_qua_en[4];


   /*
    * MA predictor memory for MR122 mode, Q10
    * (contains log2(quaErr))
    */
   Word32 past_qua_en_MR122[4];


}gc_predState;
typedef struct
{
   Word32 gainMem[PHDGAINMEMSIZE];
   Word32 prevCbGain;
   Word32 prevState;
   Word16 lockFull;
   Word16 onset;


}ph_dispState;
typedef struct
{
   int dtxGlobalState;   /* contains previous state */

   Word32 log_en;
   Word32 old_log_en;
   Word32 pn_seed_rx;
   Word32 lsp[M];
   Word32 lsp_old[M];
   Word32 lsf_hist[M * DTX_HIST_SIZE];
   Word32 lsf_hist_mean[M * DTX_HIST_SIZE];
   Word32 log_en_hist[DTX_HIST_SIZE];
   Word32 true_sid_period_inv;
   Word16 since_last_sid;
   Word16 lsf_hist_ptr;
   Word16 log_pg_mean;
   Word16 log_en_hist_ptr;
   Word16 log_en_adjust;
   Word16 dtxHangoverCount;
   Word16 decAnaElapsedCount;
   Word16 sid_frame;
   Word16 valid_data;
   Word16 dtxHangoverAdded;


   /* updated in main decoder */
   Word16 data_updated;   /* marker to know if CNI data is ever renewed */


}dtx_decState;
typedef struct
{
   Word32 past_gain;


}agcState;
typedef struct
{
   /* Excitation vector */
   Word32 old_exc[L_SUBFR + PIT_MAX + L_INTERPOL];
   Word32 *exc;
   Word32 lsp_old[M];


   /* Filter's memory */
   Word32 mem_syn[M];


   /* pitch sharpening */
   Word32 sharp;
   Word32 old_T0;


   /* Variable holding received ltpLag, used in background noise and BFI */
   Word32 T0_lagBuff;


   /* Variables for the source characteristic detector (SCD) */
   Word32 inBackgroundNoise;
   Word32 voicedHangover;
   Word32 ltpGainHistory[9];


   /* Memories for bad frame handling */
   Word32 excEnergyHist[9];
   Word16 prev_bf;
   Word16 prev_pdf;
   Word16 state;
   Word16 nodataSeed;


   Bgn_scdState * background_state;
   Cb_gain_averageState * Cb_gain_averState;
   lsp_avgState * lsp_avg_st;
   D_plsfState * lsfState;
   ec_gain_pitchState * ec_gain_p_st;
   ec_gain_codeState * ec_gain_c_st;
   gc_predState * pred_state;
   ph_dispState * ph_disp_st;
   dtx_decState * dtxDecoderState;
}Decoder_amrState;
typedef struct
{
   Word32 res2[L_SUBFR];
   Word32 mem_syn_pst[M];
   Word32 synth_buf[M + L_FRAME];
   Word32 preemph_state_mem_pre;
   agcState * agc_state;
}Post_FilterState;
typedef struct
{
   Word32 y2_hi;
   Word32 y2_lo;
   Word32 y1_hi;
   Word32 y1_lo;
   Word32 x0;
   Word32 x1;


}Post_ProcessState;
typedef struct
{
   Decoder_amrState * decoder_amrState;
   Post_FilterState * post_state;
   Post_ProcessState * postHP_state;
}Speech_Decode_FrameState;


/*
 * CodAmrReset
 *
 *
 * Parameters:
 *    state             B: state structure
 *    mode              I: AMR mode
 *
 * Function:
 *    Resets state memory
 *
 * Returns:
 *    void
 */
static void Decoder_amr_reset( Decoder_amrState *state, int mode )
{
   Word32 i;

   /* Cb_gain_average_reset */
   memset(state->Cb_gain_averState->cbGainHistory, 0, L_CBGAINHIST << 2);
   state->Cb_gain_averState->hangVar = 0;
   state->Cb_gain_averState->hangCount= 0;

   /* Initialize static pointer */
   state->exc = state->old_exc + PIT_MAX + L_INTERPOL;

   /* Static vectors to zero */
   memset( state->old_exc, 0, ( PIT_MAX + L_INTERPOL )<<2 );

   if ( mode != MRDTX )
      memset( state->mem_syn, 0, M <<2 );

   /* initialize pitch sharpening */
   state->sharp = SHARPMIN;
   state->old_T0 = 40;

   /* Initialize state->lsp_old [] */
   if ( mode != MRDTX ) {
      state->lsp_old[0] = 30000;
      state->lsp_old[1] = 26000;
      state->lsp_old[2] = 21000;
      state->lsp_old[3] = 15000;
      state->lsp_old[4] = 8000;
      state->lsp_old[5] = 0;
      state->lsp_old[6] = -8000;
      state->lsp_old[7] = -15000;
      state->lsp_old[8] = -21000;
      state->lsp_old[9] = -26000;
   }

   /* Initialize memories of bad frame handling */
   state->prev_bf = 0;
   state->prev_pdf = 0;
   state->state = 0;
   state->T0_lagBuff = 40;
   state->inBackgroundNoise = 0;
   state->voicedHangover = 0;

   if ( mode != MRDTX )
      memset( state->excEnergyHist, 0, 9 <<2 );
   memset( state->ltpGainHistory, 0, 9 <<2 );

   if ( mode != MRDTX ) {
      state->lsp_avg_st->lsp_meanSave[0] = 1384;
      state->lsp_avg_st->lsp_meanSave[1] = 2077;
      state->lsp_avg_st->lsp_meanSave[2] = 3420;
      state->lsp_avg_st->lsp_meanSave[3] = 5108;
      state->lsp_avg_st->lsp_meanSave[4] = 6742;
      state->lsp_avg_st->lsp_meanSave[5] = 8122;
      state->lsp_avg_st->lsp_meanSave[6] = 9863;
      state->lsp_avg_st->lsp_meanSave[7] = 11092;
      state->lsp_avg_st->lsp_meanSave[8] = 12714;
      state->lsp_avg_st->lsp_meanSave[9] = 13701;
   }
   memset( state->lsfState->past_r_q, 0, M <<2 );

   /* Past dequantized lsfs */
   state->lsfState->past_lsf_q[0] = 1384;
   state->lsfState->past_lsf_q[1] = 2077;
   state->lsfState->past_lsf_q[2] = 3420;
   state->lsfState->past_lsf_q[3] = 5108;
   state->lsfState->past_lsf_q[4] = 6742;
   state->lsfState->past_lsf_q[5] = 8122;
   state->lsfState->past_lsf_q[6] = 9863;
   state->lsfState->past_lsf_q[7] = 11092;
   state->lsfState->past_lsf_q[8] = 12714;
   state->lsfState->past_lsf_q[9] = 13701;

   for ( i = 0; i < 5; i++ )
      state->ec_gain_p_st->pbuf[i] = 1640;
   state->ec_gain_p_st->past_gain_pit = 0;
   state->ec_gain_p_st->prev_gp = 16384;

   for ( i = 0; i < 5; i++ )
      state->ec_gain_c_st->gbuf[i] = 1;
   state->ec_gain_c_st->past_gain_code = 0;
   state->ec_gain_c_st->prev_gc = 1;

   if ( mode != MRDTX ) {
      for ( i = 0; i < NPRED; i++ ) {
         state->pred_state->past_qua_en[i] = MIN_ENERGY;
         state->pred_state->past_qua_en_MR122[i] = MIN_ENERGY_MR122;
      }
   }
   state->nodataSeed = 21845;

   /* Static vectors to zero */
   memset( state->background_state->frameEnergyHist, 0, L_ENERGYHIST <<2 );

   /* Initialize hangover handling */
   state->background_state->bgHangover = 0;

   /* phDispReset */
   memset( state->ph_disp_st->gainMem, 0, PHDGAINMEMSIZE <<2 );
   state->ph_disp_st->prevState = 0;
   state->ph_disp_st->prevCbGain = 0;
   state->ph_disp_st->lockFull = 0;
   state->ph_disp_st->onset = 0;   /* assume no onset in start */

   if ( mode != MRDTX ) {
      state->dtxDecoderState->since_last_sid = 0;
      state->dtxDecoderState->true_sid_period_inv = 8192;
      state->dtxDecoderState->log_en = 3500;
      state->dtxDecoderState->old_log_en = 3500;

      /* low level noise for better performance in  DTX handover cases*/
      state->dtxDecoderState->pn_seed_rx = PN_INITIAL_SEED;

      /* Initialize state->lsp [] */
      state->dtxDecoderState->lsp[0] = 30000;
      state->dtxDecoderState->lsp[1] = 26000;
      state->dtxDecoderState->lsp[2] = 21000;
      state->dtxDecoderState->lsp[3] = 15000;
      state->dtxDecoderState->lsp[4] = 8000;
      state->dtxDecoderState->lsp[5] = 0;
      state->dtxDecoderState->lsp[6] = -8000;
      state->dtxDecoderState->lsp[7] = -15000;
      state->dtxDecoderState->lsp[8] = -21000;
      state->dtxDecoderState->lsp[9] = -26000;

      /* Initialize state->lsp_old [] */
      state->dtxDecoderState->lsp_old[0] = 30000;
      state->dtxDecoderState->lsp_old[1] = 26000;
      state->dtxDecoderState->lsp_old[2] = 21000;
      state->dtxDecoderState->lsp_old[3] = 15000;
      state->dtxDecoderState->lsp_old[4] = 8000;
      state->dtxDecoderState->lsp_old[5] = 0;
      state->dtxDecoderState->lsp_old[6] = -8000;
      state->dtxDecoderState->lsp_old[7] = -15000;
      state->dtxDecoderState->lsp_old[8] = -21000;
      state->dtxDecoderState->lsp_old[9] = -26000;
      state->dtxDecoderState->lsf_hist_ptr = 0;
      state->dtxDecoderState->log_pg_mean = 0;
      state->dtxDecoderState->log_en_hist_ptr = 0;

      /* initialize decoder lsf history */
      state->dtxDecoderState->lsf_hist[0] = 1384;
      state->dtxDecoderState->lsf_hist[1] = 2077;
      state->dtxDecoderState->lsf_hist[2] = 3420;
      state->dtxDecoderState->lsf_hist[3] = 5108;
      state->dtxDecoderState->lsf_hist[4] = 6742;
      state->dtxDecoderState->lsf_hist[5] = 8122;
      state->dtxDecoderState->lsf_hist[6] = 9863;
      state->dtxDecoderState->lsf_hist[7] = 11092;
      state->dtxDecoderState->lsf_hist[8] = 12714;
      state->dtxDecoderState->lsf_hist[9] = 13701;

      for ( i = 1; i < DTX_HIST_SIZE; i++ ) {
         memcpy( &state->dtxDecoderState->lsf_hist[M * i], &state->
               dtxDecoderState->lsf_hist[0], M <<2 );
      }
      memset( state->dtxDecoderState->lsf_hist_mean, 0, M * DTX_HIST_SIZE <<2 );

      /* initialize decoder log frame energy */
      for ( i = 0; i < DTX_HIST_SIZE; i++ ) {
         state->dtxDecoderState->log_en_hist[i] = state->dtxDecoderState->log_en
         ;
      }
      state->dtxDecoderState->log_en_adjust = 0;
      state->dtxDecoderState->dtxHangoverCount = DTX_HANG_CONST;
      state->dtxDecoderState->decAnaElapsedCount = 31;
      state->dtxDecoderState->sid_frame = 0;
      state->dtxDecoderState->valid_data = 0;
      state->dtxDecoderState->dtxHangoverAdded = 0;
      state->dtxDecoderState->dtxGlobalState = DTX;
      state->dtxDecoderState->data_updated = 0;
   }
   return;
}


/*
 * rx_dtx_handler
 *
 *
 * Parameters:
 *    st->dtxGlobalState      I: DTX state
 *    st->since_last_sid      B: Frames after last SID frame
 *    st->data_updated        I: SID update flag
 *    st->decAnaElapsedCount  B: state machine that synch with the GSMEFR txDtx machine
 *    st->dtxHangoverAdded    B: DTX hangover
 *    st->sid_frame           O: SID frame indicator
 *    st->valid_data          O: Vaild data indicator
 *    frame_type              O: Frame type
 *
 * Function:
 *    Find the new DTX state
 *
 * Returns:
 *    DTXStateType            DTX, DTX_MUTE or SPEECH
 */
static int rx_dtx_handler( dtx_decState *st, int frame_type )
{
   int newState;
   int encState;

   /* DTX if SID frame or previously in DTX{_MUTE} and (NO_RX OR BAD_SPEECH) */
   if ( table_SID[frame_type] | ( ( st->dtxGlobalState != SPEECH ) &
         table_speech_bad[frame_type] ) ) {
      newState = DTX;

      /* stay in mute for these input types */
      if ( ( st->dtxGlobalState == DTX_MUTE ) & table_mute[frame_type] ) {
         newState = DTX_MUTE;
      }

      /*
       * evaluate if noise parameters are too old
       * since_last_sid is reset when CN parameters have been updated
       */
      st->since_last_sid += 1;

      /* no update of sid parameters in DTX for a long while */
      if ((frame_type != RX_SID_UPDATE) & ( st->since_last_sid > DTX_MAX_EMPTY_THRESH )) {
         newState = DTX_MUTE;
      }
   }
   else {
      newState = SPEECH;
      st->since_last_sid = 0;
   }

   /*
    * reset the decAnaElapsed Counter when receiving CNI data the first
    * time, to robustify counter missmatch after handover
    * this might delay the bwd CNI analysis in the new decoder slightly.
    */
   if ( ( st->data_updated == 0 ) & ( frame_type == RX_SID_UPDATE ) ) {
      st->decAnaElapsedCount = 0;
   }

   /*
    * update the SPE-SPD DTX hangover synchronization
    * to know when SPE has added dtx hangover
    */
   st->decAnaElapsedCount += 1;
   st->dtxHangoverAdded = 0;
   encState = SPEECH;

   if ( table_DTX[frame_type] ) {
      encState = DTX;
      if( ( frame_type == RX_NO_DATA ) & ( newState == SPEECH ) ) {
         encState = SPEECH;
      }
   }

   if ( encState == SPEECH ) {
      st->dtxHangoverCount = DTX_HANG_CONST;
   }
   else {
      if ( st->decAnaElapsedCount > DTX_ELAPSED_FRAMES_THRESH ) {
         st->dtxHangoverAdded = 1;
         st->decAnaElapsedCount = 0;
         st->dtxHangoverCount = 0;
      }
      else if ( st->dtxHangoverCount == 0 ) {
         st->decAnaElapsedCount = 0;
      }
      else {
         st->dtxHangoverCount -= 1;
      }
   }

   if ( newState != SPEECH ) {
      /*
       * DTX or DTX_MUTE
       * CN data is not in a first SID, first SIDs are marked as SID_BAD
       * but will do backwards analysis if a hangover period has been added
       * according to the state machine above
       */
      st->sid_frame = 0;
      st->valid_data = 0;

      if ( frame_type == RX_SID_FIRST ) {
         st->sid_frame = 1;
      }
      else if ( frame_type == RX_SID_UPDATE ) {
         st->sid_frame = 1;
         st->valid_data = 1;
      }
      else if ( frame_type == RX_SID_BAD ) {
         st->sid_frame = 1;

         /* use old data */
         st->dtxHangoverAdded = 0;
      }
   }

   /* newState is used by both SPEECH AND DTX synthesis routines */
   return newState;
}


/*
 * Lsf_lsp
 *
 *
 * Parameters:
 *    lsf               I: vector of LSFs
 *    lsp               O: vector of LSPs
 *
 * Function:
 *    Transformation lsf to lsp, order M
 *
 * Returns:
 *    void
 */
static void Lsf_lsp( Word32 lsf[], Word32 lsp[] )
{
   Word32 i, ind, offset, tmp;