/*****************************  MPEG Audio Encoder  ***************************

                     (C) Copyright Fraunhofer IIS (2004-2005)
                               All Rights Reserved

    Please be advised that this software and/or program delivery is
    Confidential Information of Fraunhofer and subject to and covered by the

    Fraunhofer IIS Software Evaluation Agreement
    between Google Inc. and  Fraunhofer
    effective and in full force since March 1, 2012.

    You may use this software and/or program only under the terms and
    conditions described in the above mentioned Fraunhofer IIS Software
    Evaluation Agreement. Any other and/or further use requires a separate agreement.


   $Id$
   Initial Authors:      M. Neuendorf, N. Rettelbach, M. Multrus
   Contents/Description: PS parameter extraction, encoding

   This software and/or program is protected by copyright law and international
   treaties. Any reproduction or distribution of this software and/or program,
   or any portion of it, may result in severe civil and criminal penalties, and
   will be prosecuted to the maximum extent possible under law.

******************************************************************************/
/*!
  \file
  \brief  PS parameter extraction, encoding functions $Revision: 37142 $
*/

#include "ps_main.h"


#include "sbr_ram.h"
#include "ps_encode.h"

#include "qmf.h"

#include "ps_const.h"
#include "sbr_misc.h"

#include "genericStds.h"

inline void FDKsbrEnc_addFIXP_DBL(const FIXP_DBL *X, const FIXP_DBL *Y, FIXP_DBL *Z, INT n)
{
  for (INT i=0; i<n; i++)
    Z[i] = (X[i]>>1) + (Y[i]>>1);
}

#define LOG10_2_10             3.01029995664f /* 10.0f*log10(2.f) */

static const INT iidGroupBordersLoRes[QMF_GROUPS_LO_RES + SUBQMF_GROUPS_LO_RES + 1] =
{
  0, 1, 2, 3, 4, 5,    /* 6 subqmf subbands - 0th qmf subband */
  6, 7,                /* 2 subqmf subbands - 1st qmf subband */
  8, 9,                /* 2 subqmf subbands - 2nd qmf subband */
  10, 11, 12, 13, 14, 15, 16, 18, 21, 25, 30, 42, 71
};

static const UCHAR iidGroupWidthLdLoRes[QMF_GROUPS_LO_RES + SUBQMF_GROUPS_LO_RES] =
{
  0, 0, 0, 0, 0, 0,
  0, 0,
  0, 0,
  0, 0, 0, 0, 0, 0, 1, 2, 2, 3, 4, 5
};


static const INT subband2parameter20[QMF_GROUPS_LO_RES + SUBQMF_GROUPS_LO_RES] =
{
  1, 0, 0, 1, 2, 3,   /* 6 subqmf subbands - 0th qmf subband */
  4, 5,               /* 2 subqmf subbands - 1st qmf subband */
  6, 7,               /* 2 subqmf subbands - 2nd qmf subband */
  8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
};


typedef enum {
  MAX_TIME_DIFF_FRAMES = 20,
  MAX_PS_NOHEADER_CNT  = 10,
  MAX_NOENV_CNT        = 10,
  DO_NOT_USE_THIS_MODE = 0x7FFFFF
} __PS_CONSTANTS;



static const FIXP_DBL iidQuant_fx[15] = {
  0xce000000, 0xdc000000, 0xe4000000, 0xec000000, 0xf2000000, 0xf8000000, 0xfc000000, 0x00000000,
  0x04000000, 0x08000000, 0x0e000000, 0x14000000, 0x1c000000, 0x24000000, 0x32000000
};

static const FIXP_DBL iidQuantFine_fx[31] = {
  0x9c000001, 0xa6000001, 0xb0000001, 0xba000001, 0xc4000000, 0xce000000, 0xd4000000, 0xda000000,
  0xe0000000, 0xe6000000, 0xec000000, 0xf0000000, 0xf4000000, 0xf8000000, 0xfc000000, 0x00000000,
  0x04000000, 0x08000000, 0x0c000000, 0x10000000, 0x14000000, 0x1a000000, 0x20000000, 0x26000000,
  0x2c000000, 0x32000000, 0x3c000000, 0x45ffffff, 0x4fffffff, 0x59ffffff, 0x63ffffff
};



static const FIXP_DBL iccQuant[8] = {
  0x7fffffff, 0x77ef9d7f, 0x6babc97f, 0x4ceaf27f, 0x2f0ed3c0, 0x00000000, 0xb49ba601, 0x80000000
};

static FDK_PSENC_ERROR InitPSData(
        HANDLE_PS_DATA            hPsData
        )
{
  FDK_PSENC_ERROR error = PSENC_OK;

  if(hPsData == NULL) {
    error = PSENC_INVALID_HANDLE;
  }
  else {
    int i, env;
    FDKmemclear(hPsData,sizeof(PS_DATA));

    for (i=0; i<PS_MAX_BANDS; i++) {
      hPsData->iidIdxLast[i] = 0;
      hPsData->iccIdxLast[i] = 0;
    }

    hPsData->iidEnable    = hPsData->iidEnableLast = 0;
    hPsData->iccEnable    = hPsData->iccEnableLast = 0;
    hPsData->iidQuantMode = hPsData->iidQuantModeLast = PS_IID_RES_COARSE;
    hPsData->iccQuantMode = hPsData->iccQuantModeLast = PS_ICC_ROT_A;

    for(env=0; env<PS_MAX_ENVELOPES; env++) {
      hPsData->iccDiffMode[env] = PS_DELTA_FREQ;
      hPsData->iccDiffMode[env] = PS_DELTA_FREQ;

      for (i=0; i<PS_MAX_BANDS; i++) {
        hPsData->iidIdx[env][i] = 0;
        hPsData->iccIdx[env][i] = 0;
      }
    }

    hPsData->nEnvelopesLast = 0;

    hPsData->headerCnt  = MAX_PS_NOHEADER_CNT;
    hPsData->iidTimeCnt = MAX_TIME_DIFF_FRAMES;
    hPsData->iccTimeCnt = MAX_TIME_DIFF_FRAMES;
    hPsData->noEnvCnt   = MAX_NOENV_CNT;
  }

  return error;
}

static FIXP_DBL quantizeCoef( const FIXP_DBL *RESTRICT input,
                              const INT       nBands,
                              const FIXP_DBL *RESTRICT quantTable,
                              const INT       idxOffset,
                              const INT       nQuantSteps,
                              INT            *RESTRICT quantOut)
{
  INT idx, band;
  FIXP_DBL quantErr = FL2FXCONST_DBL(0.f);

  for (band=0; band<nBands;band++) {
    for(idx=0; idx<nQuantSteps-1; idx++){
      if( fixp_abs((input[band]>>1)-(quantTable[idx+1]>>1)) >
          fixp_abs((input[band]>>1)-(quantTable[idx]>>1)) )
      {
        break;
      }
    }
    quantErr      += (fixp_abs(input[band]-quantTable[idx])>>PS_QUANT_SCALE);   /* don't scale before subtraction; diff smaller (64-25)/64 */
    quantOut[band] = idx - idxOffset;
  }

  return quantErr;
}

static INT getICCMode(const INT nBands,
                      const INT rotType)
{
  INT mode = 0;

  switch(nBands) {
  case PS_BANDS_COARSE:
    mode = PS_RES_COARSE;
    break;
  case PS_BANDS_MID:
    mode = PS_RES_MID;
    break;
  default:
    mode = 0;
  }
  if(rotType==PS_ICC_ROT_B){
    mode += 3;
  }

  return mode;
}


static INT getIIDMode(const INT nBands,
                      const INT iidRes)
{
  INT mode = 0;

  switch(nBands) {
  case PS_BANDS_COARSE:
    mode = PS_RES_COARSE;
    break;
  case PS_BANDS_MID:
    mode = PS_RES_MID;
    break;
  default:
    mode = 0;
    break;
  }

  if(iidRes == PS_IID_RES_FINE){
    mode += 3;
  }

  return mode;
}


static INT envelopeReducible(FIXP_DBL iid[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                             FIXP_DBL icc[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                             INT psBands,
                             INT nEnvelopes)
{
  #define THRESH_SCALE     7

  INT reducible = 1; /* true */
  INT e = 0, b = 0;
  FIXP_DBL dIid = FL2FXCONST_DBL(0.f);
  FIXP_DBL dIcc = FL2FXCONST_DBL(0.f);

  FIXP_DBL iidErrThreshold, iccErrThreshold;
  FIXP_DBL iidMeanError, iccMeanError;

  /* square values to prevent sqrt,
     multiply bands to prevent division; bands shifted DFRACT_BITS instead (DFRACT_BITS-1) because fMultDiv2 used*/
  iidErrThreshold = fMultDiv2 ( FL2FXCONST_DBL(6.5f*6.5f/(IID_SCALE_FT*IID_SCALE_FT)), (FIXP_DBL)(psBands<<((DFRACT_BITS)-THRESH_SCALE)) );
  iccErrThreshold = fMultDiv2 ( FL2FXCONST_DBL(0.75f*0.75f),                           (FIXP_DBL)(psBands<<((DFRACT_BITS)-THRESH_SCALE)) );

  if (nEnvelopes <= 1) {
    reducible = 0;
  } else {

    /* mean error criterion */
    for (e=0; (e < nEnvelopes/2) && (reducible!=0 ) ; e++) {
      iidMeanError = iccMeanError = FL2FXCONST_DBL(0.f);
      for(b=0; b<psBands; b++) {
        dIid = (iid[2*e][b]>>1) - (iid[2*e+1][b]>>1);   /* scale 1 bit; squared -> 2 bit */
        dIcc = (icc[2*e][b]>>1) - (icc[2*e+1][b]>>1);
        iidMeanError += fPow2Div2(dIid)>>(5-1);    /* + (bands=20) scale = 5 */
        iccMeanError += fPow2Div2(dIcc)>>(5-1);
      }                                                 /* --> scaling = 7 bit = THRESH_SCALE !! */

      /* instead sqrt values are squared!
         instead of division, multiply threshold with psBands
         scaling necessary!! */

      /* quit as soon as threshold is reached */
      if ( (iidMeanError > (iidErrThreshold)) ||
           (iccMeanError > (iccErrThreshold)) ) {
        reducible = 0;
      }
    }
  } /* nEnvelopes != 1 */

  return reducible;
}


static void processIidData(PS_DATA       *psData,
                           FIXP_DBL       iid[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                           const INT      psBands,
                           const INT      nEnvelopes,
                           const FIXP_DBL quantErrorThreshold)
{
  INT iidIdxFine  [PS_MAX_ENVELOPES][PS_MAX_BANDS];
  INT iidIdxCoarse[PS_MAX_ENVELOPES][PS_MAX_BANDS];

  FIXP_DBL errIID = FL2FXCONST_DBL(0.f);
  FIXP_DBL errIIDFine = FL2FXCONST_DBL(0.f);
  INT   bitsIidFreq = 0;
  INT   bitsIidTime = 0;
  INT   bitsFineTot = 0;
  INT   bitsCoarseTot = 0;
  INT   error = 0;
  INT   env, band;
  INT   diffMode[PS_MAX_ENVELOPES], diffModeFine[PS_MAX_ENVELOPES];
  INT loudnDiff = 0;
  INT iidTransmit = 0;

  bitsIidFreq = bitsIidTime = 0;

  /* Quantize IID coefficients */
  for(env=0;env<nEnvelopes; env++) {
    errIID     += quantizeCoef(iid[env], psBands, iidQuant_fx,      7, 15, iidIdxCoarse[env]);
    errIIDFine += quantizeCoef(iid[env], psBands, iidQuantFine_fx, 15, 31, iidIdxFine[env]);
  }

  /* normalize error to number of envelopes, ps bands
     errIID /= psBands*nEnvelopes;
     errIIDFine /= psBands*nEnvelopes; */


  /* Check if IID coefficients should be used in this frame */
  psData->iidEnable = 0;
  for(env=0;env<nEnvelopes; env++) {
    for(band=0;band<psBands;band++) {
      loudnDiff   += fixp_abs(iidIdxCoarse[env][band]);
      iidTransmit ++;
    }
  }

  if(loudnDiff > fMultI(FL2FXCONST_DBL(0.7f),iidTransmit)){    /* 0.7f empiric value */
    psData->iidEnable = 1;
  }

  /* if iid not active -> RESET data */
  if(psData->iidEnable==0) {
    psData->iidTimeCnt = MAX_TIME_DIFF_FRAMES;
    for(env=0;env<nEnvelopes; env++) {
      psData->iidDiffMode[env] = PS_DELTA_FREQ;
      FDKmemclear(psData->iidIdx[env], sizeof(INT)*psBands);
    }
    return;
  }

  /* count COARSE quantization bits for first envelope*/
  bitsIidFreq = FDKsbrEnc_EncodeIid(NULL, iidIdxCoarse[0], NULL, psBands, PS_IID_RES_COARSE, PS_DELTA_FREQ, &error);

  if( (psData->iidTimeCnt>=MAX_TIME_DIFF_FRAMES) || (psData->iidQuantModeLast==PS_IID_RES_FINE) ) {
    bitsIidTime     = DO_NOT_USE_THIS_MODE;
  }
  else {
    bitsIidTime     = FDKsbrEnc_EncodeIid(NULL, iidIdxCoarse[0], psData->iidIdxLast, psBands, PS_IID_RES_COARSE, PS_DELTA_TIME, &error);
  }

  /* decision DELTA_FREQ vs DELTA_TIME */
  if(bitsIidTime>bitsIidFreq) {
    diffMode[0]   = PS_DELTA_FREQ;
    bitsCoarseTot = bitsIidFreq;
  }
  else {
    diffMode[0]   = PS_DELTA_TIME;
    bitsCoarseTot = bitsIidTime;
  }

  /* count COARSE quantization bits for following envelopes*/
  for(env=1;env<nEnvelopes; env++) {
    bitsIidFreq  = FDKsbrEnc_EncodeIid(NULL, iidIdxCoarse[env], NULL,                psBands, PS_IID_RES_COARSE, PS_DELTA_FREQ, &error);
    bitsIidTime  = FDKsbrEnc_EncodeIid(NULL, iidIdxCoarse[env], iidIdxCoarse[env-1], psBands, PS_IID_RES_COARSE, PS_DELTA_TIME, &error);

    /* decision DELTA_FREQ vs DELTA_TIME */
    if(bitsIidTime>bitsIidFreq) {
      diffMode[env]  = PS_DELTA_FREQ;
      bitsCoarseTot += bitsIidFreq;
    }
    else {
      diffMode[env]  = PS_DELTA_TIME;
      bitsCoarseTot += bitsIidTime;
    }
  }


  /* count FINE quantization bits for first envelope*/
  bitsIidFreq = FDKsbrEnc_EncodeIid(NULL, iidIdxFine[0],   NULL, psBands, PS_IID_RES_FINE,   PS_DELTA_FREQ, &error);

  if( (psData->iidTimeCnt>=MAX_TIME_DIFF_FRAMES) || (psData->iidQuantModeLast==PS_IID_RES_COARSE) ) {
    bitsIidTime = DO_NOT_USE_THIS_MODE;
  }
  else {
    bitsIidTime = FDKsbrEnc_EncodeIid(NULL, iidIdxFine[0],  psData->iidIdxLast, psBands, PS_IID_RES_FINE, PS_DELTA_TIME, &error);
  }

  /* decision DELTA_FREQ vs DELTA_TIME */
  if(bitsIidTime>bitsIidFreq) {
    diffModeFine[0]   = PS_DELTA_FREQ;
    bitsFineTot       = bitsIidFreq;
  }
  else {
    diffModeFine[0]   = PS_DELTA_TIME;
    bitsFineTot       = bitsIidTime;
  }

  /* count FINE quantization bits for following envelopes*/
  for(env=1;env<nEnvelopes; env++) {
    bitsIidFreq = FDKsbrEnc_EncodeIid(NULL, iidIdxFine[env],   NULL,              psBands, PS_IID_RES_FINE, PS_DELTA_FREQ, &error);
    bitsIidTime = FDKsbrEnc_EncodeIid(NULL, iidIdxFine[env],   iidIdxFine[env-1], psBands, PS_IID_RES_FINE, PS_DELTA_TIME, &error);

    /* decision DELTA_FREQ vs DELTA_TIME */
    if(bitsIidTime>bitsIidFreq) {
      diffModeFine[env]  = PS_DELTA_FREQ;
      bitsFineTot += bitsIidFreq;
    }
    else {
      diffModeFine[env]  = PS_DELTA_TIME;
      bitsFineTot       += bitsIidTime;
    }
  }

  if(bitsFineTot == bitsCoarseTot){
    /* if same number of bits is needed, use the quantization with lower error */
    if(errIIDFine < errIID){
      bitsCoarseTot = DO_NOT_USE_THIS_MODE;
    } else {
      bitsFineTot = DO_NOT_USE_THIS_MODE;
    }
  } else {
    /* const FIXP_DBL minThreshold = FL2FXCONST_DBL(0.2f/(IID_SCALE_FT*PS_QUANT_SCALE_FT)*(psBands*nEnvelopes)); */
    const FIXP_DBL minThreshold = (FIXP_DBL)((LONG)0x00019999 * (psBands*nEnvelopes));

    /* decision RES_FINE vs RES_COARSE                 */
    /* test if errIIDFine*quantErrorThreshold < errIID */
    /* shiftVal 2 comes from scaling of quantErrorThreshold */
    if(fixMax(((errIIDFine>>1)+(minThreshold>>1))>>1, fMult(quantErrorThreshold,errIIDFine)) < (errIID>>2) ) {
      bitsCoarseTot = DO_NOT_USE_THIS_MODE;
    }
    else if(fixMax(((errIID>>1)+(minThreshold>>1))>>1, fMult(quantErrorThreshold,errIID)) < (errIIDFine>>2) ) {
      bitsFineTot = DO_NOT_USE_THIS_MODE;
    }
  }

  /* decision RES_FINE vs RES_COARSE */
  if(bitsFineTot<bitsCoarseTot) {
    psData->iidQuantMode = PS_IID_RES_FINE;
    for(env=0;env<nEnvelopes; env++) {
      psData->iidDiffMode[env] = diffModeFine[env];
      FDKmemcpy(psData->iidIdx[env], iidIdxFine[env], psBands*sizeof(INT));
    }
  }
  else {
    psData->iidQuantMode = PS_IID_RES_COARSE;
    for(env=0;env<nEnvelopes; env++) {
      psData->iidDiffMode[env] = diffMode[env];
      FDKmemcpy(psData->iidIdx[env], iidIdxCoarse[env], psBands*sizeof(INT));
    }
  }

  /* Count DELTA_TIME encoding streaks */
  for(env=0;env<nEnvelopes; env++) {
    if(psData->iidDiffMode[env]==PS_DELTA_TIME)
      psData->iidTimeCnt++;
    else
      psData->iidTimeCnt=0;
  }
}


static INT similarIid(PS_DATA   *psData,
                      const INT  psBands,
                      const INT  nEnvelopes)
{
  const INT diffThr = (psData->iidQuantMode == PS_IID_RES_COARSE) ? 2 : 3;
  const INT sumDiffThr = diffThr * psBands/4;
  INT similar = 0;
  INT diff    = 0;
  INT sumDiff = 0;
  INT env = 0;
  INT b   = 0;
  if ((nEnvelopes == psData->nEnvelopesLast) && (nEnvelopes==1)) {
    similar = 1;
    for (env=0; env<nEnvelopes; env++) {
      sumDiff = 0;
      b = 0;
      do {
        diff = fixp_abs(psData->iidIdx[env][b] - psData->iidIdxLast[b]);
        sumDiff += diff;
        if ( (diff > diffThr) /* more than x quantization steps in any band */
             || (sumDiff > sumDiffThr) ) {  /* more than x quantisations steps overall difference */
          similar = 0;
        }
        b++;
      } while ((b<psBands) && (similar>0));
    }
  } /* nEnvelopes==1  */

  return similar;
}


static INT similarIcc(PS_DATA *psData,
                      const INT    psBands,
                      const INT    nEnvelopes)
{
  const INT diffThr = 2;
  const INT sumDiffThr = diffThr * psBands/4;
  INT similar = 0;
  INT diff    = 0;
  INT sumDiff = 0;
  INT env = 0;
  INT b   = 0;
  if ((nEnvelopes == psData->nEnvelopesLast) && (nEnvelopes==1)) {
    similar = 1;
    for (env=0; env<nEnvelopes; env++) {
      sumDiff = 0;
      b = 0;
      do {
        diff = fixp_abs(psData->iccIdx[env][b] - psData->iccIdxLast[b]);
        sumDiff += diff;
        if ( (diff > diffThr) /* more than x quantisation step in any band */
             || (sumDiff > sumDiffThr) ) {  /* more than x quantisations steps overall difference */
          similar = 0;
        }
        b++;
      } while ((b<psBands) && (similar>0));
    }
  } /* nEnvelopes==1  */

  return similar;
}

static void processIccData(PS_DATA   *psData,
                           FIXP_DBL   icc[PS_MAX_ENVELOPES][PS_MAX_BANDS], /* const input values: unable to declare as const, since it does not poINT to const memory */
                           const INT  psBands,
                           const INT  nEnvelopes)
{
  FIXP_DBL errICC = FL2FXCONST_DBL(0.f);
  INT   env, band;
  INT   bitsIccFreq, bitsIccTime;
  INT   error = 0;
  INT   inCoherence=0, iccTransmit=0;
  INT  *iccIdxLast;

  iccIdxLast = psData->iccIdxLast;

  /* Quantize ICC coefficients */
  for(env=0;env<nEnvelopes; env++) {
    errICC += quantizeCoef(icc[env], psBands, iccQuant, 0, 8, psData->iccIdx[env]);
  }

  /* Check if ICC coefficients should be used */
  psData->iccEnable = 0;
  for(env=0;env<nEnvelopes; env++) {
    for(band=0;band<psBands;band++) {
      inCoherence += psData->iccIdx[env][band];
      iccTransmit ++;
    }
  }
  if(inCoherence > fMultI(FL2FXCONST_DBL(0.5f),iccTransmit)){   /* 0.5f empiric value */
    psData->iccEnable = 1;
  }

  if(psData->iccEnable==0) {
    psData->iccTimeCnt = MAX_TIME_DIFF_FRAMES;
    for(env=0;env<nEnvelopes; env++) {
      psData->iccDiffMode[env] = PS_DELTA_FREQ;
      FDKmemclear(psData->iccIdx[env], sizeof(INT)*psBands);
    }
    return;
  }

  for(env=0;env<nEnvelopes; env++) {
    bitsIccFreq  = FDKsbrEnc_EncodeIcc(NULL, psData->iccIdx[env],   NULL,       psBands, PS_DELTA_FREQ, &error);

    if(psData->iccTimeCnt<MAX_TIME_DIFF_FRAMES) {
      bitsIccTime  = FDKsbrEnc_EncodeIcc(NULL, psData->iccIdx[env], iccIdxLast, psBands, PS_DELTA_TIME, &error);
    }
    else {
      bitsIccTime  = DO_NOT_USE_THIS_MODE;
    }

    if(bitsIccFreq>bitsIccTime) {
      psData->iccDiffMode[env] = PS_DELTA_TIME;
      psData->iccTimeCnt++;
    }
    else {
      psData->iccDiffMode[env] = PS_DELTA_FREQ;
      psData->iccTimeCnt=0;
    }
    iccIdxLast = psData->iccIdx[env];
  }
}

static void calculateIID(FIXP_DBL ldPwrL[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL ldPwrR[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL iid[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         INT   nEnvelopes,
                         INT   psBands)
{
  INT i=0;
  INT env=0;
  for(env=0; env<nEnvelopes;env++) {
    for (i=0; i<psBands; i++) {

      /* iid[env][i] = 10.0f*(float)log10(pwrL[env][i]/pwrR[env][i]);
      */
      FIXP_DBL IID = fMultDiv2( FL2FXCONST_DBL(LOG10_2_10/IID_SCALE_FT), (ldPwrL[env][i]-ldPwrR[env][i]) );

      IID = fixMin( IID, (FIXP_DBL)(MAXVAL_DBL>>(LD_DATA_SHIFT+1)) );
      IID = fixMax( IID, (FIXP_DBL)(MINVAL_DBL>>(LD_DATA_SHIFT+1)) );
      iid[env][i] = IID << (LD_DATA_SHIFT+1);
    }
  }
}

static void calculateICC(FIXP_DBL ldPwrL[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL ldPwrR[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL pwrCr[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL pwrCi[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         FIXP_DBL icc[PS_MAX_ENVELOPES][PS_MAX_BANDS],
                         INT   nEnvelopes,
                         INT   psBands)
{
  INT i = 0;
  INT env = 0;
  INT border = psBands;

  switch (psBands) {
  case PS_BANDS_COARSE:
    border = 5;
    break;
  case PS_BANDS_MID:
    border = 11;
    break;
  default:
    break;
  }

  /* :TRICKY: ndf 20041012 It is unclear which formula should be used here.
     The first one does not quite correspond to the scientific formula for coherence.
     The second formula is correct mathematically speaking but returns values in the range of [0 1],
     where the MPEG standard allows quantization down to -1. This then doesnt seem to make sense. */
  /* ndf 20041119 According to rtb/hrr the 1. formula will interpret small time
     delays as incoherence whereas the 2. formula will consider only truly
     uncorrelated signals as incoherent. */

  for(env=0; env<nEnvelopes;env++) {
    for (i=0; i<border; i++) {

      /* icc[env][i] = min( pwrCr[env][i] / (float) sqrt(pwrL[env][i] * pwrR[env][i]) , 1.f);
      */
      FIXP_DBL ICC, invNrg = CalcInvLdData ( -((ldPwrL[env][i]>>1) + (ldPwrR[env][i]>>1) + (FIXP_DBL)1) );
      INT      scale, invScale = CountLeadingBits(invNrg);

      scale = (DFRACT_BITS-1) - invScale;
      ICC = fMult(pwrCr[env][i], invNrg<<invScale) ;
      icc[env][i] = SATURATE_LEFT_SHIFT(ICC, scale, DFRACT_BITS);
    }

    for (; i<psBands; i++) {
      INT sc1, sc2;
      FIXP_DBL cNrgR, cNrgI, ICC;

      sc1 = CountLeadingBits( fixMax(fixp_abs(pwrCr[env][i]),fixp_abs(pwrCi[env][i])) ) ;
      cNrgR = fPow2Div2((pwrCr[env][i]<<sc1));       /* squared nrg's expect explicit scaling */
      cNrgI = fPow2Div2((pwrCi[env][i]<<sc1));

      ICC = CalcInvLdData( (CalcLdData((cNrgR + cNrgI)>>1)>>1) - (FIXP_DBL)((sc1-1)<<(DFRACT_BITS-1-LD_DATA_SHIFT)) );

      FIXP_DBL invNrg = CalcInvLdData ( -((ldPwrL[env][i]>>1) + (ldPwrR[env][i]>>1) + (FIXP_DBL)1) );
      sc1 = CountLeadingBits(invNrg);
      invNrg <<= sc1;

      sc2 = CountLeadingBits(ICC);
      ICC = fMult(ICC<<sc2,invNrg);

      sc1 = ( (DFRACT_BITS-1) - sc1 - sc2 );
      if (sc1 < 0) {
          ICC >>= -sc1;
      }
      else {
          if (ICC >= ((FIXP_DBL)MAXVAL_DBL>>sc1) )
              ICC = (FIXP_DBL)MAXVAL_DBL;
          else
              ICC <<= sc1;
      }

      icc[env][i] = ICC;
    }
  }
}

void FDKsbrEnc_initPsBandNrgScale(HANDLE_PS_ENCODE hPsEncode)
{
  INT group, bin;
  INT nIidGroups   = hPsEncode->nQmfIidGroups + hPsEncode->nSubQmfIidGroups;

  FDKmemclear(hPsEncode->psBandNrgScale, PS_MAX_BANDS*sizeof(SCHAR));

  for (group=0; group < nIidGroups; group++) {
    /* Translate group to bin */
    bin = hPsEncode->subband2parameterIndex[group];

    /* Translate from 20 bins to 10 bins */
    if (hPsEncode->psEncMode == PS_BANDS_COARSE) {
      bin = bin>>1;
    }

    hPsEncode->psBandNrgScale[bin] = (hPsEncode->psBandNrgScale[bin]==0)
                          ? (hPsEncode->iidGroupWidthLd[group] + 5)
                          : (fixMax(hPsEncode->iidGroupWidthLd[group],hPsEncode->psBandNrgScale[bin]) + 1) ;

  }
}

FDK_PSENC_ERROR FDKsbrEnc_CreatePSEncode(
        HANDLE_PS_ENCODE         *phPsEncode
        )
{
  FDK_PSENC_ERROR error = PSENC_OK;

  if (phPsEncode==NULL) {
    error = PSENC_INVALID_HANDLE;
  }
  else {
    HANDLE_PS_ENCODE hPsEncode = NULL;
    if (NULL==(hPsEncode = GetRam_PsEncode())) {
      error = PSENC_MEMORY_ERROR;
      goto bail;
    }
    FDKmemclear(hPsEncode,sizeof(PS_ENCODE));
    *phPsEncode = hPsEncode; /* return allocated handle */
  }
bail:
  return error;
}

FDK_PSENC_ERROR FDKsbrEnc_InitPSEncode(
        HANDLE_PS_ENCODE          hPsEncode,
        const PS_BANDS            psEncMode,
        const FIXP_DBL            iidQuantErrorThreshold
        )
{
  FDK_PSENC_ERROR error = PSENC_OK;

  if (NULL==hPsEncode) {
    error = PSENC_INVALID_HANDLE;
  }
  else {
    if (PSENC_OK != (InitPSData(&hPsEncode->psData))) {
      goto bail;
    }

    switch(psEncMode){
      case PS_BANDS_COARSE:
      case PS_BANDS_MID:
        hPsEncode->nQmfIidGroups    = QMF_GROUPS_LO_RES;
        hPsEncode->nSubQmfIidGroups = SUBQMF_GROUPS_LO_RES;
        FDKmemcpy(hPsEncode->iidGroupBorders,        iidGroupBordersLoRes, (hPsEncode->nQmfIidGroups + hPsEncode->nSubQmfIidGroups + 1)*sizeof(INT));
        FDKmemcpy(hPsEncode->subband2parameterIndex, subband2parameter20,  (hPsEncode->nQmfIidGroups + hPsEncode->nSubQmfIidGroups)    *sizeof(INT));
        FDKmemcpy(hPsEncode->iidGroupWidthLd,        iidGroupWidthLdLoRes, (hPsEncode->nQmfIidGroups + hPsEncode->nSubQmfIidGroups)    *sizeof(UCHAR));
        break;
      default:
        error = PSENC_INIT_ERROR;
        goto bail;
    }

    hPsEncode->psEncMode = psEncMode;
    hPsEncode->iidQuantErrorThreshold = iidQuantErrorThreshold;
    FDKsbrEnc_initPsBandNrgScale(hPsEncode);
  }
bail:
  return error;
}


FDK_PSENC_ERROR FDKsbrEnc_DestroyPSEncode(
        HANDLE_PS_ENCODE         *phPsEncode
        )
{
  FDK_PSENC_ERROR error = PSENC_OK;

  if (NULL !=phPsEncode) {
    FreeRam_PsEncode(phPsEncode);
  }

  return error;
}

typedef struct {
  FIXP_DBL pwrL[PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL pwrR[PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL ldPwrL[PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL ldPwrR[PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL pwrCr[PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL pwrCi[PS_MAX_ENVELOPES][PS_MAX_BANDS];

} PS_PWR_DATA;


FDK_PSENC_ERROR FDKsbrEnc_PSEncode(
        HANDLE_PS_ENCODE          hPsEncode,
        HANDLE_PS_OUT             hPsOut,
        UCHAR                    *dynBandScale,
        UINT                      maxEnvelopes,
        FIXP_DBL                 *hybridData[HYBRID_FRAMESIZE][MAX_PS_CHANNELS][2],
        const INT                 frameSize,
        const INT                 sendHeader
        )
{
  FDK_PSENC_ERROR error = PSENC_OK;

  HANDLE_PS_DATA hPsData = &hPsEncode->psData;
  FIXP_DBL iid [PS_MAX_ENVELOPES][PS_MAX_BANDS];
  FIXP_DBL icc [PS_MAX_ENVELOPES][PS_MAX_BANDS];
  int envBorder[PS_MAX_ENVELOPES+1];

  int group, bin, col, subband, band;
  int i = 0;

  int env = 0;
  int psBands      = (int) hPsEncode->psEncMode;
  int nIidGroups   = hPsEncode->nQmfIidGroups + hPsEncode->nSubQmfIidGroups;
  int nEnvelopes   = fixMin(maxEnvelopes, (UINT)PS_MAX_ENVELOPES);

  C_ALLOC_SCRATCH_START(pwrData, PS_PWR_DATA, 1);

  for(env=0; env<nEnvelopes+1;env++) {
    envBorder[env] = fMultI(GetInvInt(nEnvelopes),frameSize*env);
  }

  for(env=0; env<nEnvelopes;env++) {

    /* clear energy array */
    for (band=0; band<psBands; band++) {
      pwrData->pwrL[env][band] = pwrData->pwrR[env][band] = pwrData->pwrCr[env][band] = pwrData->pwrCi[env][band] = FIXP_DBL(1);
    }

    /**** calculate energies and correlation ****/

    /* start with hybrid data */
    for (group=0; group < nIidGroups; group++) {
      /* Translate group to bin */
      bin = hPsEncode->subband2parameterIndex[group];

      /* Translate from 20 bins to 10 bins */
      if (hPsEncode->psEncMode == PS_BANDS_COARSE) {
        bin >>= 1;
      }

      /* determine group border */
      int bScale = hPsEncode->psBandNrgScale[bin];

      FIXP_DBL pwrL_env_bin = pwrData->pwrL[env][bin];
      FIXP_DBL pwrR_env_bin = pwrData->pwrR[env][bin];
      FIXP_DBL pwrCr_env_bin = pwrData->pwrCr[env][bin];
      FIXP_DBL pwrCi_env_bin = pwrData->pwrCi[env][bin];

      int scale = (int)dynBandScale[bin];
      for (col=envBorder[env]; col<envBorder[env+1]; col++) {
        for (subband = hPsEncode->iidGroupBorders[group]; subband < hPsEncode->iidGroupBorders[group+1]; subband++) {
          FIXP_QMF l_real = (hybridData[col][0][0][subband]) << scale;
          FIXP_QMF l_imag = (hybridData[col][0][1][subband]) << scale;
          FIXP_QMF r_real = (hybridData[col][1][0][subband]) << scale;
          FIXP_QMF r_imag = (hybridData[col][1][1][subband]) << scale;

          pwrL_env_bin  += (fPow2Div2(l_real) + fPow2Div2(l_imag)) >> bScale;
          pwrR_env_bin  += (fPow2Div2(r_real) + fPow2Div2(r_imag)) >> bScale;
          pwrCr_env_bin += (fMultDiv2(l_real, r_real) + fMultDiv2(l_imag, r_imag)) >> bScale;
          pwrCi_env_bin += (fMultDiv2(r_real, l_imag) - fMultDiv2(l_real, r_imag)) >> bScale;
        }
      }
      /* assure, nrg's of left and right channel are not negative; necessary on 16 bit multiply units */
      pwrData->pwrL[env][bin] = fixMax((FIXP_DBL)0,pwrL_env_bin);
      pwrData->pwrR[env][bin] = fixMax((FIXP_DBL)0,pwrR_env_bin);

      pwrData->pwrCr[env][bin] = pwrCr_env_bin;
      pwrData->pwrCi[env][bin] = pwrCi_env_bin;

    } /* nIidGroups */

    /* calc logarithmic energy */
    LdDataVector(pwrData->pwrL[env], pwrData->ldPwrL[env], psBands);
    LdDataVector(pwrData->pwrR[env], pwrData->ldPwrR[env], psBands);

  } /* nEnvelopes */

  /* calculate iid and icc */
  calculateIID(pwrData->ldPwrL, pwrData->ldPwrR, iid, nEnvelopes, psBands);
  calculateICC(pwrData->ldPwrL, pwrData->ldPwrR, pwrData->pwrCr, pwrData->pwrCi, icc, nEnvelopes, psBands);

  /*** Envelope Reduction ***/
  while (envelopeReducible(iid,icc,psBands,nEnvelopes)) {
    int e=0;
    /* sum energies of two neighboring envelopes */
    nEnvelopes >>= 1;
    for (e=0; e<nEnvelopes; e++) {
      FDKsbrEnc_addFIXP_DBL(pwrData->pwrL[2*e], pwrData->pwrL[2*e+1], pwrData->pwrL[e], psBands);
      FDKsbrEnc_addFIXP_DBL(pwrData->pwrR[2*e], pwrData->pwrR[2*e+1], pwrData->pwrR[e], psBands);
      FDKsbrEnc_addFIXP_DBL(pwrData->pwrCr[2*e],pwrData->pwrCr[2*e+1],pwrData->pwrCr[e],psBands);
      FDKsbrEnc_addFIXP_DBL(pwrData->pwrCi[2*e],pwrData->pwrCi[2*e+1],pwrData->pwrCi[e],psBands);

      /* calc logarithmic energy */
      LdDataVector(pwrData->pwrL[e], pwrData->ldPwrL[e], psBands);
      LdDataVector(pwrData->pwrR[e], pwrData->ldPwrR[e], psBands);

      /* reduce number of envelopes and adjust borders */
      envBorder[e] = envBorder[2*e];
    }
    envBorder[nEnvelopes] = envBorder[2*nEnvelopes];

    /* re-calculate iid and icc */
    calculateIID(pwrData->ldPwrL, pwrData->ldPwrR, iid, nEnvelopes, psBands);
    calculateICC(pwrData->ldPwrL, pwrData->ldPwrR, pwrData->pwrCr, pwrData->pwrCi, icc, nEnvelopes, psBands);
  }


  /*  */
  if(sendHeader) {
    hPsData->headerCnt  = MAX_PS_NOHEADER_CNT;
    hPsData->iidTimeCnt = MAX_TIME_DIFF_FRAMES;
    hPsData->iccTimeCnt = MAX_TIME_DIFF_FRAMES;
    hPsData->noEnvCnt   = MAX_NOENV_CNT;
  }

  /*** Parameter processing, quantisation etc ***/
  processIidData(hPsData, iid, psBands, nEnvelopes, hPsEncode->iidQuantErrorThreshold);
  processIccData(hPsData, icc, psBands, nEnvelopes);


  /*** Initialize output struct ***/

  /* PS Header on/off ? */
  if( (hPsData->headerCnt<MAX_PS_NOHEADER_CNT)
       && ( (hPsData->iidQuantMode == hPsData->iidQuantModeLast) && (hPsData->iccQuantMode == hPsData->iccQuantModeLast) )
       && ( (hPsData->iidEnable    == hPsData->iidEnableLast)    && (hPsData->iccEnable    == hPsData->iccEnableLast)  ) ) {
    hPsOut->enablePSHeader = 0;
  }
  else {
    hPsOut->enablePSHeader = 1;
    hPsData->headerCnt = 0;
  }

  /* nEnvelopes = 0 ? */
  if ( (hPsData->noEnvCnt < MAX_NOENV_CNT)
       && (similarIid(hPsData, psBands, nEnvelopes))
       && (similarIcc(hPsData, psBands, nEnvelopes)) ) {
    hPsOut->nEnvelopes = nEnvelopes = 0;
    hPsData->noEnvCnt++;
  } else {
    hPsData->noEnvCnt = 0;
  }


  if (nEnvelopes>0) {

    hPsOut->enableIID      = hPsData->iidEnable;
    hPsOut->iidMode        = getIIDMode(psBands, hPsData->iidQuantMode);

    hPsOut->enableICC      = hPsData->iccEnable;
    hPsOut->iccMode        = getICCMode(psBands, hPsData->iccQuantMode);

    hPsOut->enableIpdOpd  = 0;
    hPsOut->frameClass    = 0;
    hPsOut->nEnvelopes    = nEnvelopes;

    for(env=0; env<nEnvelopes; env++) {
      hPsOut->frameBorder[env] = envBorder[env+1];
    }

    for(env=0; env<hPsOut->nEnvelopes; env++) {
      hPsOut->deltaIID[env] = (PS_DELTA)hPsData->iidDiffMode[env];

      for(band=0; band<psBands; band++) {
        hPsOut->iid[env][band] = hPsData->iidIdx[env][band];
      }
    }

    for(env=0; env<hPsOut->nEnvelopes; env++) {
      hPsOut->deltaICC[env] = (PS_DELTA)hPsData->iccDiffMode[env];
      for(band=0; band<psBands; band++) {
        hPsOut->icc[env][band] = hPsData->iccIdx[env][band];
      }
    }

    /* IPD OPD not supported right now */
    FDKmemclear(hPsOut->ipd, PS_MAX_ENVELOPES*PS_MAX_BANDS*sizeof(PS_DELTA));
    for(env=0; env<PS_MAX_ENVELOPES; env++) {
      hPsOut->deltaIPD[env] = PS_DELTA_FREQ;
      hPsOut->deltaOPD[env] = PS_DELTA_FREQ;
    }

    FDKmemclear(hPsOut->ipdLast, PS_MAX_BANDS*sizeof(INT));
    FDKmemclear(hPsOut->opdLast, PS_MAX_BANDS*sizeof(INT));

    for(band=0; band<PS_MAX_BANDS; band++) {
      hPsOut->iidLast[band] = hPsData->iidIdxLast[band];
      hPsOut->iccLast[band] = hPsData->iccIdxLast[band];
    }

    /* save iids and iccs for differential time coding in the next frame */
    hPsData->nEnvelopesLast   = nEnvelopes;
    hPsData->iidEnableLast    = hPsData->iidEnable;
    hPsData->iccEnableLast    = hPsData->iccEnable;
    hPsData->iidQuantModeLast = hPsData->iidQuantMode;
    hPsData->iccQuantModeLast = hPsData->iccQuantMode;
    for (i=0; i<psBands; i++) {
      hPsData->iidIdxLast[i] = hPsData->iidIdx[nEnvelopes-1][i];
      hPsData->iccIdxLast[i] = hPsData->iccIdx[nEnvelopes-1][i];
    }
  } /* Envelope > 0 */

  C_ALLOC_SCRATCH_END(pwrData, PS_PWR_DATA, 1)

  return error;
}