/*
//
//                  INTEL CORPORATION PROPRIETARY INFORMATION
//     This software is supplied under the terms of a license agreement or
//     nondisclosure agreement with Intel Corporation and may not be copied
//     or disclosed except in accordance with the terms of that agreement.
//          Copyright(c) 2003-2006 Intel Corporation. All Rights Reserved.
//
//     Intel(R) Integrated Performance Primitives Advanced Aurora Sample for Windows*
//
//   By downloading and installing this sample, you hereby agree that the
//   accompanying Materials are being provided to you under the terms and
//   conditions of the End User License Agreement for the Intel(R) Integrated
//   Performance Primitives product previously accepted by you. Please refer
//   to the file ippEULA.rtf located in the root directory of your Intel(R) IPP
//   product installation for more information.
//
//   ES 202 050 v1.1.1 is the international standard promoted by ETSI
//   and other organizations. Implementations of these standards, or the standard
//   enabled platforms may require licenses from various entities, including
//   Intel Corporation.
//
*/

#include <malloc.h>
#include <stdio.h>
#include <math.h>

#include "encoderapi.h"
#include "tab_8kHz.inc"
#include "tab_16kHz.inc"
#include "mframe.h"

#define STARTING_FREQ            64.0f
#define WINHANNSIZE 200
#define WAVEFORMSIZE 200

#define FFTSIZE 256
#define FFTSPEC ((FFTSIZE>>2)+1)
#define FFTSPEC16 128
#define FFTORDER 8
#define FRAMESHIFT 80

/* 16KHz*/
#define KFB16 3
#define FFTSPEC16 128

/*VAD*/
#define NB_FRAME_THRESHOLD_LTE 10
#define LAMBDA_LTE 0.97
#define SNR_THRESHOLD_UPD_LTE 20
#define ENERGY_FLOOR 80
#define MIN_FRAME 10
#define lambdaLTEhigherE 0.99
#define SNR_THRESHOLD_VAD 15
#define MIN_SPEECH_FRAME_HANGOVER 4
#define HANGOVER 15

/* Wiener filter design */
#define NB_FRAME_THRESHOLD_NSE 100
#define LAMBDA_NSE 0.99f
#define EPS  1e-16f
#define BETA 0.98f
#define NTH 0.079432823

/*MelFilter*/
#define NUM_CHANNELS 23
#define FL 17
#define FILTERLENGTH 118

static int append=0;

static float RefCep[NUM_CEP_COEFF-1] = { -6.618909f, 0.198269f, -0.740308f, 0.055132f, -0.227086f,
   0.144280f, -0.112451f, -0.146940f, -0.327466f, 0.134571f, 0.027884f, -0.114905f} ;

static float FIRCoef[FILTERLENGTH]={0.000189f, 0.000096f, -0.000500f, -0.001071f, -0.000714f,
 0.000308f, 0.000555f, -0.000362f, -0.000835f, 0.000184f, 0.001069f, 0.000068f, -0.001302f,
 -0.000448f, 0.001469f, 0.000948f, -0.001525f, -0.001555f, 0.001421f, 0.002240f, -0.001112f,
 -0.002957f, 0.000560f, 0.003644f, 0.000266f, -0.004225f, -0.001374f, 0.004611f, 0.002755f,
 -0.004706f, -0.004375f, 0.004411f, 0.006175f, -0.003626f, -0.008065f, 0.002260f, 0.009932f,
 -0.000230f, -0.011631f, -0.002537f, 0.012990f, 0.006105f, -0.013809f, -0.010542f, 0.013844f,
 0.015940f, -0.012786f, -0.022471f, 0.010192f, 0.030493f, -0.005322f, -0.040853f, -0.003360f,
 0.055801f, 0.020003f, -0.083066f, -0.061967f, 0.172388f, 0.424683f, 0.424683f, 0.172388f,
 -0.061967f, -0.083066f, 0.020003f, 0.055801f, -0.003360f, -0.040853f, -0.005322f, 0.030493f,
 0.010192f, -0.022471f, -0.012786f, 0.015940f, 0.013844f, -0.010542f, -0.013809f, 0.006105f,
 0.012990f, -0.002537f, -0.011631f, -0.000230f, 0.009932f, 0.002260f, -0.008065f, -0.003626f,
 0.006175f, 0.004411f, -0.004375f, -0.004706f, 0.002755f, 0.004611f, -0.001374f, -0.004225f,
 0.000266f, 0.003644f, 0.000560f, -0.002957f, -0.001112f, 0.002240f, 0.001421f, -0.001555f,
 -0.001525f, 0.000948f, 0.001469f, -0.000448f, -0.001302f, 0.000068f, 0.001069f, 0.000184f,
 -0.000835f, -0.000362f, 0.000555f, 0.000308f, -0.000714f, -0.001071f, -0.000500f, 0.000096f,
 0.000189f};

static Ipp32f pLambdaNSE[]={0.000000f,0.500000f,0.666667f,0.750000f,0.800000f,0.833333f,0.857143f,0.875000f,
  0.888889f,0.900000f,0.909091f,0.916667f,0.923077f,0.928571f,0.933333f,0.937500f,0.941176f,0.944444f,0.947368f,
  0.950000f,0.952381f,0.954545f,0.956522f,0.958333f,0.960000f,0.961538f,0.962963f,0.964286f,0.965517f,0.966667f,
  0.967742f,0.968750f,0.969697f,0.970588f,0.971429f,0.972222f,0.972973f,0.973684f,0.974359f,0.975000f,0.975610f,
  0.976190f,0.976744f,0.977273f,0.977778f,0.978261f,0.978723f,0.979167f,0.979592f,0.980000f,0.980392f,0.980769f,
  0.981132f,0.981481f,0.981818f,0.982143f,0.982456f,0.982759f,0.983051f,0.983333f,0.983607f,0.983871f,0.984127f,
  0.984375f,0.984615f,0.984848f,0.985075f,0.985294f,0.985507f,0.985714f,0.985915f,0.986111f,0.986301f,0.986486f,
  0.986667f,0.986842f,0.987013f,0.987179f,0.987342f,0.987500f,0.987654f,0.987805f,0.987952f,0.988095f,0.988235f,
  0.988372f,0.988506f,0.988636f,0.988764f,0.988889f,0.989011f,0.989130f,0.989247f,0.989362f,0.989474f,0.989583f,
  0.989691f,0.989796f,0.989899f};

struct VADN{
 int nbSpeechFrame;
 float Eloglowtrack;
 char flagVAD;
 int hangOver;
 float pNoise16[KFB16];
};


struct s16Khz{
    IppsFBankState_32f* pFBank16;
   float dataHP[560];
   struct VADN vad16;
};

struct NoiseReduction{
    IppsFBankState_32f* pFBank;
    IppsFFTSpec_R_32f* ctxFFT;
    float winHannCoeff[WINHANNSIZE];
    float winHannFilter[FL];
    int FFTOrder;
   float pMulCoeff[NUM_CHANNELS+2];
   float **pIDCT;
    float pPSDprev_1[FFTSPEC];
    float pPSDprev_2[FFTSPEC];
   float pDenPrev_1[FFTSPEC];
   float pDenPrev_2[FFTSPEC];
    float pNoisePrev_2[FFTSPEC];
    float pNoiseTn[FFTSPEC];
    float pNoiseTnPrev_1[FFTSPEC];
   float pBStateBig[640+FILTERLENGTH];
    float pBStateBuf[960];
   float pFFT[FFTSIZE*6];
    float Eden,Eden1,Eden2;
    float SNRlowPrev, SNRlow;
   float agfPrev;
   float pSrc0, Dst0;
   int Cur_TPrpocess;
   int tail;
   int start;
   int end;
   struct VADN vad;
   struct s16Khz s16;
};

struct s11Khz{
    IppsResamlingPolyphaseFixed_16s **ppState;
    int history;
    double time;
    int lastread;
    short *p11;
    int len;
};

struct EncoderStruct{
    IppsFBankState_32f* pFBank;
    IppsDCTLifterState_32f* pDCTLifter;
    IppsCdbkState_32f  **ppCdbkState;
     IppsFFTSpec_R_32f* ctxFFT;
    float bias[NUM_CEP_COEFF-1];
    struct s11Khz s11;
    struct NoiseReduction noise;
    char *pVadMem;
    float *workBuffer;
    unsigned char pIndexVQBuffer[2*NUM_CODEBOOK];
    float *pFeatBuffer;
    float *pFeatCyclicBuf;
    float *HammingWindow;
    MFrame *pFrame;
    int FFTOrder;
    int FrameLength;
    int FrameShift;
    int FFTLength;
    int numCepCoeff;
    int numChannels;
    float Dst0;
    float pSrc0;
    float preFloat;
    int curPosition;
    AuroraDataType EncoderOutput;
    AuroraDataType EncoderInput;
    AuroraRate freq;
    double weight_c0;
    double weight_logE;
    int iCountFrame;
    int NumberFrame;
    int mframeCounter;
    int crc;
    int BufferData;
    int Curr;
    int Prev;
    int Last;
    int Start;
    int BufferSize;
    int done;
    int VadFrame;
};

void SpecSubHighFreqBands (AuroraEncoder *pCodec, float *pDataInOut, float *pHPBands, int t ){

  int i;
  int flagVAD;
  float lambdaNSE;
  float Elog, Eloglowtrack;

  Eloglowtrack = pCodec->noise.s16.vad16.Eloglowtrack;

  if (t < NB_FRAME_THRESHOLD_NSE){
    lambdaNSE = pLambdaNSE[t-1];
  }
  else
    lambdaNSE = LAMBDA_NSE;

  Elog = 0.0f;
  for (i=0 ; i<KFB16 ; i++)
    Elog += pDataInOut[i];

  if (Elog > 0.001)
    ippsLn_32f_I(&Elog,1);
  else
    Elog = -6.90776f;

  if (((Elog - Eloglowtrack) < 1.2) || (t < 10)){
     if (t < 10){
        Eloglowtrack = (1 - lambdaNSE) * Elog + lambdaNSE * Eloglowtrack;
     }else{
        if (Elog < Eloglowtrack)
           Eloglowtrack = 0.02f * Elog + 0.98f * Eloglowtrack;
          else
           Eloglowtrack = 0.005f * Elog + 0.995f *  Eloglowtrack;
     }
  }

  if ((Elog - Eloglowtrack) > 2.2f){
      flagVAD = 1;
      pCodec->noise.s16.vad16.nbSpeechFrame++;
  }else{
     if (pCodec->noise.s16.vad16.nbSpeechFrame > MIN_SPEECH_FRAME_HANGOVER)
        pCodec->noise.s16.vad16.hangOver = 5;
       pCodec->noise.s16.vad16.nbSpeechFrame = 0;
      if (pCodec->noise.s16.vad16.hangOver != 0){
         pCodec->noise.s16.vad16.hangOver--;
         flagVAD = 1;
      } else
         flagVAD = 0;
  }

  if (flagVAD == 0){
     for (i=0 ; i<KFB16 ; i++){
        pCodec->noise.s16.vad16.pNoise16 [i] = (1-lambdaNSE) * pCodec->noise.s16.vad16.pNoise16 [i] + lambdaNSE * pDataInOut [i];
     }
  }

  for (i=0 ; i<KFB16 ; i++){
     pHPBands[i] = IPP_MAX(pDataInOut [i] - 1.5f * pCodec->noise.s16.vad16.pNoise16 [i], 0.1f * pDataInOut [i]);
  }
  pCodec->noise.s16.vad16.Eloglowtrack = Eloglowtrack;
  pCodec->noise.s16.vad16.flagVAD = flagVAD;

}


/* Functions for noise reduction block */
/* Copy buffers for noise reduction */
void CopyBuffers(AuroraEncoder *pCodec,short *pInputSignal, int offset, int InputLength){
   ippsConvert_16s32f(pInputSignal, pCodec->noise.pBStateBig+offset+append,
           InputLength);
   pCodec->noise.start = offset;
   pCodec->noise.end = InputLength;
}

/* Prepare buffers for perform noise reduction and count number frame */
int PrepareForCalculation(AuroraEncoder *pCodec, short *pInputSignal, int InputLength){
   int FrameCounter;
   int CurLen = InputLength;
   float *pBState_2 = pCodec->noise.pBStateBuf+320;
   float *pBstate_1 = pBState_2+320;
   if(pCodec->noise.Cur_TPrpocess < 1){
      if(InputLength+pCodec->noise.tail<pCodec->FrameShift*3){
         CopyBuffers(pCodec,pInputSignal,pCodec->FrameShift+pCodec->noise.tail,InputLength);
         pCodec->noise.tail+=InputLength;
         return 0;
      }
      CopyBuffers(pCodec,pInputSignal,pCodec->noise.tail+pCodec->FrameShift,pCodec->FrameShift*3 - pCodec->noise.tail);
      pCodec->noise.start = pCodec->FrameShift;
      pCodec->noise.end = pCodec->FrameShift*3;

      if(pCodec->freq == r16KHz){
         ippsLowHighFilter_Aurora_32f(pCodec->noise.pBStateBig+160,
            pBstate_1+80, pCodec->noise.s16.dataHP+320, 160, FIRCoef, FILTERLENGTH);

         ippsLowHighFilter_Aurora_32f(pCodec->noise.pBStateBig+320,
            pBstate_1+160, pCodec->noise.s16.dataHP+400, 160, FIRCoef, FILTERLENGTH);
      }

      CurLen = InputLength+pCodec->noise.tail;

      pCodec->noise.tail = pCodec->noise.tail + InputLength - pCodec->FrameShift*3;

      FrameCounter = (CurLen)/pCodec->FrameShift-1;
   }
   else{
      if(InputLength+pCodec->noise.tail < pCodec->FrameShift){
         CopyBuffers(pCodec,pInputSignal,3*pCodec->FrameShift+pCodec->noise.tail,InputLength);
         pCodec->noise.tail+=InputLength;
         return 0;
      }
      CopyBuffers(pCodec,pInputSignal,3*pCodec->FrameShift+pCodec->noise.tail,pCodec->FrameShift - pCodec->noise.tail);
      CurLen = InputLength+pCodec->noise.tail;
      FrameCounter = (CurLen )/pCodec->FrameShift;
      pCodec->noise.tail = CurLen - pCodec->FrameShift;
      pCodec->noise.start = pCodec->FrameShift*3;
      pCodec->noise.end = pCodec->FrameShift;

      if(CurLen - (FrameCounter-1)*pCodec->FrameShift >= pCodec->FrameShift)
         FrameCounter++;
   }
   return FrameCounter;
}


/*VAD for noise estimation (VADNest)*/
char VADNest(struct VADN *vad,int tc, float *pBuffer){
    float frameEn;
   float lambdaLTE;
    float sum=0.0f;

    if( (tc) < NB_FRAME_THRESHOLD_LTE)
        lambdaLTE = 1 - 1.0f/(tc);
    else
        lambdaLTE = (float)LAMBDA_LTE;

    ippsDotProd_32f(pBuffer,pBuffer,FRAMESHIFT,&sum);
    sum+=64.0f;
   ippsLn_32f_I(&sum,1);