/*****************************  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: 36847 $
*/

#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
};




/*
  name:        static HANDLE_ERROR_INFO CreatePSData()
  description: Creates struct (buffer) to store ps data
  returns:     error code of type HANDLE_ERROR_INFO
  input:       none
  output:      - HANDLE_PS_DATA *hPsData: according handle
*/
static HANDLE_ERROR_INFO CreatePSData(HANDLE_PS_DATA *hPsData)
{
  HANDLE_ERROR_INFO error = noError;

  *hPsData = GetRam_PsData();
  if (*hPsData==NULL) {
    error = 1;
    goto bail;
  }
  FDKmemclear(*hPsData,sizeof(PS_DATA));

bail:
  return error;
}


/*
  name:        static HANDLE_ERROR_INFO DestroyPSData()
  description: frees according data handle
  returns:     error code of type HANDLE_ERROR_INFO
  input:       - HANDLE_PS_DATA *hPsData
  output:      none
*/
static HANDLE_ERROR_INFO DestroyPSData(HANDLE_PS_DATA *phPsData)
{
  FreeRam_PsData(phPsData);

  return noError;
}


static HANDLE_ERROR_INFO InitPSData(HANDLE_PS_DATA hPsData)
{
  INT i, env;
  HANDLE_ERROR_INFO error = noError;

  if(hPsData != NULL){

    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;
  } else {
    error = ERROR(CDI, "Unable to write to hPsData.");
  }

  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;
  case PS_BANDS_FINE:
    mode = PS_RES_FINE;
    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;
  case PS_BANDS_FINE:
    mode = PS_RES_FINE;
    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)(FL2FXCONST_DBL( 1.f)>>(LD_DATA_SHIFT+1)) );
      IID = fixMax( IID, (FIXP_DBL)(FL2FXCONST_DBL(-1.f)>>(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;
  case PS_BANDS_FINE:
    border = 16;
    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) ;

  }
}

HANDLE_ERROR_INFO FDKsbrEnc_CreatePSEncode(HANDLE_PS_ENCODE *phPsEncode){

  HANDLE_ERROR_INFO error = noError;

  HANDLE_PS_ENCODE hPsEncode = GetRam_PsEncode();
  FDKmemclear(hPsEncode,sizeof(PS_ENCODE));

  if(error == noError){
    if(noError != (error = CreatePSData(&hPsEncode->hPsData))){
      error = handBack(error);
    }
  }

  *phPsEncode = hPsEncode;

  return error;
}

HANDLE_ERROR_INFO FDKsbrEnc_InitPSEncode(HANDLE_PS_ENCODE hPsEncode, const PS_BANDS psEncMode, const FIXP_DBL iidQuantErrorThreshold){

  HANDLE_ERROR_INFO error = noError;

  if(error == noError){
    if(noError != (InitPSData(hPsEncode->hPsData))){
      error = handBack(error);
    }
  }

  if(error == noError){
    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;
    case PS_BANDS_FINE:
      FDK_ASSERT(0); /* we don't support this mode! */

      break;
    default:
      error = ERROR(CDI, "Invalid stereo band configuration.");
      break;
    }
  }

  if(error == noError){
    hPsEncode->psEncMode = psEncMode;
    hPsEncode->iidQuantErrorThreshold = iidQuantErrorThreshold;
    FDKsbrEnc_initPsBandNrgScale(hPsEncode);
  }

  return error;
}


HANDLE_ERROR_INFO FDKsbrEnc_DestroyPSEncode(HANDLE_PS_ENCODE *phPsEncode){

  HANDLE_ERROR_INFO error = noError;

  if(error == noError){
    DestroyPSData(&(*phPsEncode)->hPsData);
    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;

HANDLE_ERROR_INFO FDKsbrEnc_PSEncode(HANDLE_PS_ENCODE RESTRICT hPsEncode,
                           HANDLE_PS_OUT RESTRICT hPsOut,
                           HANDLE_PS_CHANNEL_DATA RESTRICT hChanDatal,
                           HANDLE_PS_CHANNEL_DATA RESTRICT hChanDatar,
                           UCHAR *RESTRICT dynBandScale,
                           UINT maxEnvelopes,
                           const int sendHeader)
{
  HANDLE_ERROR_INFO error = noError;
  HANDLE_PS_DATA hPsData = hPsEncode->hPsData;
  HANDLE_PS_HYBRID_DATA hHybDatal = hChanDatal->hHybData;
  HANDLE_PS_HYBRID_DATA hHybDatar = hChanDatar->hHybData;
  FIXP_QMF **RESTRICT lr = NULL, **RESTRICT li = NULL, **RESTRICT rr = NULL, **RESTRICT ri = NULL;
  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, border, col, subband, band;
  int i = 0;

  int env = 0;
  int psBands      = (int) hPsEncode->psEncMode;
  int frameSize    = FDKsbrEnc_GetHybridFrameSize(hHybDatal); /* same as FDKsbrEnc_GetHybridFrameSize(hHybDatar) */
  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++) {
    INT nHybridQmfOffset = 0;
    int descale = 0;

    /* 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 */
    lr = hHybDatal->rHybData; li = hHybDatal->iHybData;
    rr = hHybDatar->rHybData; ri = hHybDatar->iHybData;
    UCHAR switched = 0;

    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;
      }

      if (!switched && group == hPsEncode->nSubQmfIidGroups) {
        /* switch to qmf data */
        lr = hChanDatal->hPsQmfData->rQmfData; li = hChanDatal->hPsQmfData->iQmfData;
        rr = hChanDatar->hPsQmfData->rQmfData; ri = hChanDatar->hPsQmfData->iQmfData;
        /* calc offset between hybrid subsubbands and qmf bands */
        nHybridQmfOffset = FDKsbrEnc_GetNumberHybridQmfBands(hHybDatal) - FDKsbrEnc_GetNumberHybridBands(hHybDatal);
        switched = 1;
      }

      /* determine group border */
      int bScale = 2*descale + hPsEncode->psBandNrgScale[bin];
      border = hPsEncode->iidGroupBorders[group+1];

      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 < border; subband++) {
          FIXP_QMF l_real = (lr[col][subband + nHybridQmfOffset]) << scale;
          FIXP_QMF l_imag = (li[col][subband + nHybridQmfOffset]) << scale;
          FIXP_QMF r_real = (rr[col][subband + nHybridQmfOffset]) << scale;
          FIXP_QMF r_imag = (ri[col][subband + nHybridQmfOffset]) << 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;
}