/*
//
//                  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 NUM_CHANNELS             23
#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 31
#define SNR_THRESHOLD_UPD_LTE (20<<10)
#define ENERGY_FLOOR (80<<10)
#define MIN_FRAME 10
#define SNR_THRESHOLD_VAD (15<<10)
#define MIN_SPEECH_FRAME_HANGOVER 4
#define HANGOVER 15

/* Wiener filter design */
#define NB_FRAME_THRESHOLD_NSE 100
#define LAMBDA_NSE 0.99f
#define BETA 0.98f
#define NTH 0.079432823

/*MelFilter*/
#define NUM_CHANNELS 23
#define FL 17
#define FILTERLENGTH 118
#define SATURATE_16s(a) ((Ipp16s)((a>IPP_MAX_16S)?IPP_MAX_16S:(a<IPP_MIN_16S)?IPP_MIN_16S:a))
#define SATURATE_32s(a) ((Ipp32s)((a>IPP_MAX_32S)?IPP_MAX_32S:(a<IPP_MIN_32S)?IPP_MIN_32S:a))
static int append=0;
static Ipp16s pIDCTCoeff[25][25]={
   {256, 512, 512, 512, 597, 682, 767, 853, 938, 1024, 1024, 1024, 1108, 1279, 1450, 1535, 1620, 1705, 1876, 2046, 2218, 2388, 2559, 2819, 1454},
   {256, 511, 509, 506, 585, 659, 727, 783, 827, 851, 791, 724, 703, 693, 620, 446, 238, -27, -335, -751, -1201, -1715, -2194, -2710, -1454},
   {256, 509, 502, 489, 551, 591, 609, 583, 521, 391, 199, 0, -216, -527, -919, -1276, -1550, -1705, -1756, -1495, -917, 75, 1203, 2392, 1454},
   {256, 506, 489, 462, 496, 482, 426, 287, 92, -199, -482, -724, -977, -1265, -1407, -1187, -693, 82, 963, 1849, 2194, 1607, 130, -1889, -1454},
   {256, 502, 473, 425, 422, 341, 198, -55, -359, -724, -946, -1024, -1024, -843, -283, 586, 1346, 1702, 1412, 137, -1459, -2383, -1427, 1239, 1454},
   {256, 496, 451, 379, 331, 176, -50, -389, -725, -1004, -979, -724, -321, 350, 1164, 1528, 1089, -136, -1467, -1950, -614, 1817, 2317, -495, -1454},
   {256, 489, 425, 324, 228, 0, -293, -659, -920, -946, -568, 0, 615, 1223, 1279, 301, -1026, -1697, -887, 1294, 2124, -225, -2546, -287, 1454},
   {256, 482, 395, 263, 116, -176, -506, -820, -898, -568, 100, 724, 1103, 976, -69, -1353, -1391, 191, 1785, 1000, -1686, -1492, 2048, 1048, -1454},
   {256, 473, 362, 195, 0, -341, -665, -846, -663, 0, 724, 1024, 783, -165, -1339, -1087, 618, 1691, 249, -2028, -298, 2369, -967, -1728, 1454},
   {256, 462, 324, 124, -116, -482, -753, -732, -272, 568, 1019, 724, -108, -1155, -1075, 722, 1572, -245, -1874, 488, 2009, -1911, -390, 2274, -1454},
   {256, 451, 284, 50, -228, -591, -761, -497, 183, 946, 851, 0, -921, -1087, 419, 1506, -156, -1683, 420, 1669, -1877, 375, 1636, -2645, 1454},
   {256, 439, 241, -25, -331, -659, -688, -180, 595, 1004, 297, -724, -1060, -22, 1434, 153, -1618, 299, 1724, -1714, 23, 1371, -2415, 2811, -1454},
   {256, 425, 195, -99, -422, -682, -543, 166, 867, 724, -391, -1024, -424, 1062, 807, -1417, -319, 1674, -1036, -410, 1852, -2346, 2506, -2760, 1454},
   {256, 411, 148, -172, -496, -659, -339, 485, 934, 199, -903, -724, 522, 1174, -743, -976, 1524, -353, -1353, 2016, -2029, 1997, -1882, 2496, -1454},
   {256, 395, 99, -241, -551, -591, -100, 724, 780, -391, -1004, 0, 1087, 210, -1443, 850, 767, -1662, 1520, -1069, 345, -523, 721, -2039, 1454},
   {256, 379, 50, -304, -585, -482, 149, 844, 442, -851, -649, 724, 857, -946, -491, 1470, -1299, 407, 810, -1231, 1655, -1245, 645, 1424, -1454},
   {256, 362, 0, -362, -597, -341, 383, 824, 0, -1024, 0, 1024, 0, -1236, 1023, 3, -1149, 1649, -1809, 1973, -2137, 2312, -1828, -699, 1454},
   {256, 343, -50, -411, -585, -176, 577, 668, -442, -851, 649, 724, -857, -393, 1366, -1468, 961, -460, -163, -217, 659, -2076, 2489, -79, -1454},
   {256, 324, -99, -451, -551, 0, 709, 402, -780, -391, 1004, 0, -1087, 809, 145, -856, 1431, -1635, 1868, -1813, 1423, 669, -2440, 851, 1454},
   {256, 304, -148, -482, -496, 176, 766, 70, -934, 199, 903, -724, -522, 1271, -1242, 970, -541, 512, -504, 1548, -2200, 1114, 1695, -1558, -1454},
   {256, 284, -195, -502, -422, 341, 741, -273, -867, 724, 391, -1024, 424, 568, -1208, 1420, -1590, 1618, -1688, 676, 960, -2270, -467, 2145, 1454},
   {256, 263, -241, -511, -331, 482, 638, -572, -595, 1004, -297, -724, 1060, -654, 208, -145, 73, -564, 1107, -2045, 1161, 2146, -894, -2565, -1454},
   {256, 241, -284, -509, -228, 590, 467, -777, -183, 946, -851, 0, 921, -1278, 1386, -1505, 1612, -1600, 1292, 825, -2217, -812, 2000, 2788, 1454},
   {256, 218, -324, -496, -116, 659, 247, -853, 272, 568, -1019, 724, 108, -730, 977, -728, 399, 616, -1569, 1439, 1240, -979, -2536, -2795, -1454},
   {256, 195, -362, -473, 0, 682, 0, -788, 663, 0, -724, 1024, -783, 486, -550, 1081, -1494, 1580, -731, -1882, 874, 2219, 2349, 2586, 1454}
};

static const Ipp16s pHammingWindow[]={1311,  1319,  1333,  1356,  1385,  1422,  1467,  1518,  1577,  1643,  1717,  1797,  1884,  1978,  2079,
  2186,  2300,  2421,  2548,  2681,  2820,  2965,  3116,  3273,  3435,  3602,  3775,  3952,  4135,  4322,  4513,  4709,  4909,
  5113,  5320,  5531,  5745,  5963,  6183,  6406,  6631,  6858,  7087,  7319,  7551,  7785,  8020,  8256,  8492,  8728,  8965,
  9202,  9438,  9674,  9909,  10143,  10375,  10606,  10836,  11063,  11288,  11511,  11731,  11948,  12163,  12373,  12581,  12785,
  12985,  13180,  13372,  13559,  13742,  13919,  14092,  14259,  14421,  14578,  14729,  14874,  15013,  15146,  15273,
  15393,  15507,  15615,  15716,  15810,  15897,  15977,  16050,  16116,  16175,  16227,  16271,  16308,  16338,  16360,  16375,
  16383,  16383,  16375,  16360,  16338,  16308,  16271,  16227,  16175,  16116,  16050,  15977,  15897,  15810,  15716,  15615,
  15507,  15393,  15273,  15146,  15013,  14874,  14729,  14578,  14421,  14259,  14092,  13919,  13742,  13559,  13372,  13180,
  12985,  12785,  12581,  12373,  12163,  11948,  11731,  11511,  11288,  11063,  10836,  10606,  10375,  10143,  9909,  9674,  9438,
  9202,  8965,  8728,  8492,  8256,  8020,  7785,  7551,  7319,  7087,  6858,  6631,  6406,  6183,  5963,  5745,  5531,  5320,  5113,
  4909,  4709,  4513,  4322,  4135,  3952,  3775,  3602,  3435,  3273,  3116,  2965,  2820,  2681,  2548,  2421,  2300,  2186,  2079,
  1978,  1884,  1797,  1717,  1643,  1577,  1518,  1467,  1422,  1385,  1356,  1333,  1319,  1311};

static Ipp16s RefCep[NUM_CEP_COEFF-1] = { -423,  12,  -47,  3,  -14,  9,  -7,  -9,  -20,  8,  1,  -7} ;

static Ipp16s pLambdaLTE[]={1024, 512, 341, 256, 205, 171, 146, 128, 114};
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};
/* FIRCoef - Q14*/
static Ipp16s FIRCoef[FILTERLENGTH]={ 6, 3, -16, -35, -23, 10, 18, -11, -27, 6, 35, 2, -42, -14, 48, 31,
 -49, -50, 46, 73, -36, -96, 18, 119, 8, -138, -45, 151, 90, -154, -143, 144, 202, -118, -264, 74, 325,
 -7, -381, -83, 425, 200, -452, -345, 453, 522, -418, -736, 333, 999, -174, -1338, -110, 1828, 655, -2721,
 -2030, 5648, 13916, 13916, 5648, -2030, -2721, 655, 1828, -110, -1338, -174, 999, 333, -736, -418, 522,
 453, -345, -452, 200, 425, -83, -381, -7, 325, 74, -264, -118, 202, 144, -143, -154, 90, 151, -45, -138,
 8, 119, 18, -96, -36, 73, 46, -50, -49, 31, 48, -14, -42, 2, 35, 6, -27, -11, 18, 10, -23, -35, -16, 3, 6};

static Ipp16s winHannCoeffQ14[WINHANNSIZE] = {1,9,25,49,81,121,170,226,290,362,441,528,623,725,835,952,
  1076,1207,1345,1489,1640,1798,1962,2132,2309,2491,2678,2871,3070,3273,3481,3694,3911,4133,
  4358,4588,4820,5057,5296,5538,5783,6030,6279,6530,6783,7037,7293,7549,7806,8063,8320,8577,
  8834,9090,9346,9600,9853,10104,10353,10600,10845,11087,11326,11563,11795,12025,12250,12472,
  12689,12902,13110,13313,13512,13705,13892,14074,14251,14421,14585,14743,14894,15038,15176,
  15307,15431,15548,15658,15760,15855,15942,16021,16093,16157,16213,16262,16302,16334,16358,
  16374,16382,16382,16374,16358,16334,16302,16262,16213,16157,16093,16021,15942,15855,15760,
  15658,15548,15431,15307,15176,15038,14894,14743,14585,14421,14251,14074,13892,13705,13512,
  13313,13110,12902,12689,12472,12250,12025,11795,11563,11326,11087,10845,10600,10353,10104,
  9853,9600,9346,9090,8834,8577,8320,8063,7806,7549,7293,7037,6783,6530,6279,6030,5783,5538,
  5296,5057,4820,4588,4358,4133,3911,3694,3481,3273,3070,2871,2678,2491,2309,2132,1962,1798,
  1640,1489,1345,1207,1076,952,835,725,623,528,441,362,290,226,170,121,81,49,25,9,1};

static Ipp16s winHannFilter16s[FL] = {8,76,203,371,559,740,890,989,1024,989,890,740,559,371,203,76,8};
//{139,1227,3255,5950,8947,11843,14245,15830,16384,15830,14245,11843,8947,5950,3255,1227,139};

struct VADN{
 Ipp32s nbSpeechFrame;
 Ipp32s Eloglowtrack;
 Ipp8u flagVAD;
 Ipp32s hangOver;
 Ipp32s pNoise16[KFB16];
};


struct s16Khz{
     IppsFBankState_16s* pFBank16;
   Ipp16s dataHP[560];
   struct VADN vad16;
};

struct NoiseReduction{
   IppsFBankState_16s* pFBank;
   IppsFFTSpec_R_32s* ctxFFT;
   Ipp32s FFTOrder;
   Ipp16s **pIDCT;

   Ipp32s pPSDprev_1[FFTSPEC];
   Ipp32s pPSDprev_2[FFTSPEC];
   Ipp16s pDenPrev_1[FFTSPEC];
   Ipp16s pDenPrev_2[FFTSPEC];
   Ipp32s pNoisePrev_2[FFTSPEC];
   Ipp16s pNoiseTn[FFTSPEC];
   Ipp16s pNoiseTnPrev_1[FFTSPEC];

   Ipp16s pBStateBig[640+FILTERLENGTH];
   Ipp16s pBStateBuf[960];

   Ipp32s pFFT[FFTSIZE*6];
   Ipp32s Eden,Eden1,Eden2;
   Ipp32s SNRlowPrev32s;
   Ipp32s agfPrev32s;
   Ipp16s pSrc0, Dst0;
   Ipp32s Cur_TPrpocess;
   Ipp32s tail;
   Ipp32s start;
   Ipp32s end;
   struct VADN vad;
   struct s16Khz s16;
};

struct s11Khz{
    IppsResamlingPolyphaseFixed_16s **ppState;
    Ipp32s history;
    Ipp64f time;
    Ipp32s lastread;
    Ipp16s *p11;
    Ipp32s len;
};

struct EncoderStruct{
    Ipp8u   *pFBank;
    IppsDCTLifterState_16s* pDCTLifter;
    IppsCdbkState_16s  **ppCdbkState;
    IppsFFTSpec_R_32s* ctxFFT;
    Ipp16s bias[NUM_CEP_COEFF-1];
    struct s11Khz s11;
    struct NoiseReduction noise;
    Ipp8u *pVadMem;
    Ipp16s *workBuffer;
    Ipp32s *pBuffer32s;
    Ipp8u pIndexVQBuffer[2*NUM_CODEBOOK];
    Ipp16s *pFeatBuffer;
    Ipp16s *pFeatCyclicBuf;
    Ipp16s *HammingWindow;
    Ipp16s *FirCoeff16sAlign;
    MFrame *pFrame;
    Ipp32s FFTOrder;
    Ipp32s FrameLength;
    Ipp32s FrameShift;
    Ipp32s FFTLength;
    Ipp32s numCepCoeff;
    Ipp32s numChannels;
    Ipp16s Dst0;
    Ipp16s pSrc0;
    Ipp16s preFloat;
    Ipp32s curPosition;
    AuroraDataType EncoderOutput;
    AuroraDataType EncoderInput;
    AuroraRate freq;
    Ipp16s weight_c0;
    Ipp16s weight_logE;
    Ipp32s iCountFrame;
    Ipp32s NumberFrame;
    Ipp32s mframeCounter;
    Ipp32s crc;
    Ipp32s BufferData;
    Ipp32s Curr;
    Ipp32s Prev;
    Ipp32s Last;
    Ipp32s Start;
    Ipp32s BufferSize;
    Ipp32s done;
    Ipp32s VadFrame;
};


/* Functions for noise reduction block */
/* Copy buffers for noise reduction */
void CopyBuffers(AuroraEncoder *pCodec,short *pInputSignal, int offset, int InputLength){
   ippsCopy_16s(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;
   short *pBState_2 = pCodec->noise.pBStateBuf+320;
   short *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_16s_Sfs(pCodec->noise.pBStateBig+160,
            pBstate_1+80, pCodec->noise.s16.dataHP+320, 160, pCodec->FirCoeff16sAlign, FILTERLENGTH,15);

         ippsLowHighFilter_Aurora_16s_Sfs(pCodec->noise.pBStateBig+320,
            pBstate_1+160, pCodec->noise.s16.dataHP+400, 160, pCodec->FirCoeff16sAlign, FILTERLENGTH,15);
      }

      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, short *pBuffer){
   Ipp32s frameEn;
   Ipp16s lambdaLTE;
   Ipp32s sum=0;

   if( (tc) < NB_FRAME_THRESHOLD_LTE)
      lambdaLTE = pLambdaLTE[tc];
   else
      lambdaLTE = LAMBDA_LTE;