/* /////////////////////////////////////////////////////////////////////////////
//
//                  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) 2005 Intel Corporation. All Rights Reserved.
//
//     Intel(R) Integrated Performance Primitives EC Sample
//
//  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 ipplic.htm located in the root directory of your Intel(R) IPP
//  product installation for more information.
//
//
//  Purpose: Echo Canceller, subband algorithm
//
*/

#include <stdlib.h>
#include "ipps.h"
#include "ippsc.h"
#include "ec_api_int.h"
#include "ownec_int.h"

/* init scratch memory buffer */
int ec_isb_InitBuff(_isbECState *state, char *buff) {
#if !defined (NO_SCRATCH_MEMORY_USED)
    if(NULL==state)
        return 1;
    if(NULL != buff)
       state->Mem.base = buff;
    else
       if (NULL == state->Mem.base)
          return 1;
    state->Mem.CurPtr = state->Mem.base;
    state->Mem.VecPtr = (int *)(state->Mem.base+EC_SCRATCH_MEMORY_SIZE);
#endif
    return 0;
}
/* Returns size of frame */
int ec_isb_GetFrameSize(IppPCMFrequency freq, int taptime_ms, int *s)
{
    *s = SB_FRAME_SIZE;
    return 0;
}

int ec_isb_GetSegNum(isbECState *state)
{
      return state->numSegments;
}
int ec_isb_GetSubbandNum(isbECState *state)
{
   return state->numSubbands;
}

/* Returns size of buffer for state structure */
int ec_isb_GetSize(IppPCMFrequency freq, int taptime_ms, int *s)
{
    int size = 0, numSegments;
    int pSizeSpec, pSizeInit, pSizeBuf;
    int pSizeInit1, pSizeBuf1, csize;

    size += ALIGN(sizeof(_isbECState));

    ippsFFTGetSize_R_16s32s(SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit, &pSizeBuf);

    size += ALIGN(pSizeSpec);

    ippsFFTGetSize_C_32sc(SB_FFT_ORDER, IPP_FFT_DIV_FWD_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit1, &pSizeBuf1);

    size += ALIGN(pSizeSpec);

    csize = pSizeInit;
    if (csize < pSizeInit1)
        csize = pSizeInit1;
    if (csize < pSizeBuf)
        csize = pSizeBuf;
    if (csize < pSizeBuf1)
        csize = pSizeBuf1;

    ippsFFTGetSize_R_32s(SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit1, &pSizeBuf1);

    size += ALIGN(pSizeSpec);

    if (csize < pSizeInit1)
        csize = pSizeInit1;

    if (csize < pSizeBuf1)
        csize = pSizeBuf1;

    size += ALIGN(csize);

    if (freq == IPP_PCM_FREQ_8000) {
        numSegments = (taptime_ms * 8 - 1) / SB_FRAME_SIZE + 1;
    } else if (freq == IPP_PCM_FREQ_16000) {
        numSegments = (taptime_ms * 16 - 1) / SB_FRAME_SIZE + 1;
    } else {
        return 1;
    }

    size += ALIGN(3 * numSegments * sizeof(Ipp32sc *));

    size += ALIGN(3 * numSegments * SB_SUBBANDS * sizeof(Ipp32sc));

    ippsSubbandControllerGetSize_EC_16s(SB_SUBBANDS, SB_FRAME_SIZE, numSegments, freq, &csize);

    size += ALIGN(csize);

    ippsToneDetectGetStateSize_EC_16s(freq, &csize);
    size += ALIGN(csize * 2);

    *s = size;

    return 0;
}

/* Returns delay introduced to send-path */
int ec_isb_GetSendPathDelay(int *delay)
{
    *delay = 0;
    return 0;
}

/* acquire NLP gain coeff */
int ec_isb_GetNLPGain(_isbECState *state)
{
   return state->sgain;
}

/* Initializes state structure */
int ec_isb_Init(_isbECState *state, IppPCMFrequency freq, int taptime_ms)
{
    int i, numSegments;
    int pSizeSpec, pSizeInit, pSizeBuf;
    int pSizeInit1, pSizeBuf1, csize;
    Ipp8u *ptr = (Ipp8u *)state, *ptr0, *ptr1, *ptr2, *buf;

    ptr += ALIGN(sizeof(_isbECState));


    ippsFFTGetSize_R_16s32s(SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit, &pSizeBuf);

    ptr0 = ptr;
    ptr += ALIGN(pSizeSpec);

    ippsFFTGetSize_C_32sc(SB_FFT_ORDER, IPP_FFT_DIV_FWD_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit1, &pSizeBuf1);

    ptr1 = ptr;
    ptr += ALIGN(pSizeSpec);

    csize = pSizeInit;
    if (csize < pSizeInit1)
        csize = pSizeInit1;
    if (csize < pSizeBuf)
        csize = pSizeBuf;
    if (csize < pSizeBuf1)
        csize = pSizeBuf1;

    ippsFFTGetSize_R_32s(SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        &pSizeSpec, &pSizeInit1, &pSizeBuf1);

    ptr2 = ptr;
    ptr += ALIGN(pSizeSpec);

    if (csize < pSizeInit1)
        csize = pSizeInit1;

    if (csize < pSizeBuf1)
        csize = pSizeBuf1;

    ippsFFTInit_R_16s32s(&(state->spec_fft), SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        ptr0, ptr);
    ippsFFTInit_R_32s(&(state->spec_fft32), SB_FFT_ORDER, IPP_FFT_DIV_INV_BY_N, ippAlgHintAccurate,
        ptr1, ptr);
    ippsFFTInit_C_32sc(&(state->spec_fftC), SB_FFT_ORDER, IPP_FFT_DIV_FWD_BY_N, ippAlgHintAccurate,
        ptr2, ptr);

    /* Work buffer for FFT */
    state->pBuf = ptr;

    ptr += ALIGN(csize);

    state->FFTSize = 128;
    state->numSubbands = 65;
    state->frameSize = 64;
    state->windowLen = SB_WIN_LEN;

    if (freq == IPP_PCM_FREQ_8000) {
        numSegments = (taptime_ms * 8 - 1) / state->frameSize + 1;
    } else if (freq == IPP_PCM_FREQ_16000) {
        numSegments = (taptime_ms * 16 - 1) / state->frameSize + 1;
    } else {
        return 1;
    }

    state->numSegments = numSegments;

    /* receive-in subband history */
    state->ppRinSubbandHistory = (Ipp32sc **)ptr;
    ptr += ALIGN(3 * numSegments * sizeof(Ipp32sc *));
    /* adaptive coeffs */
    state->ppAdaptiveCoefs = state->ppRinSubbandHistory + numSegments;
    /* fixed coeffs */
    state->ppFixedCoefs = state->ppRinSubbandHistory + numSegments * 2;

    buf = ptr;
    ptr += ALIGN(3 * numSegments * SB_SUBBANDS * sizeof(Ipp32sc));

    /* Zero coeffs buffer and history buffer */
    ippsZero_32sc((Ipp32sc *)buf, 3 * numSegments * SB_SUBBANDS);

    /* Initialize receive-in subband history array */
    for (i = 0; i < numSegments; i++) {
        (state->ppRinSubbandHistory)[i] = (Ipp32sc *)buf + i * (state->numSubbands);
    }
    buf += numSegments * (state->numSubbands) * sizeof(Ipp32sc);

    /* Initialize adaptive coeffs array */
    for (i = 0; i < numSegments; i++) {
        (state->ppAdaptiveCoefs)[i] = (Ipp32sc *)buf + i * (state->numSubbands);
    }
    buf += numSegments * (state->numSubbands) * sizeof(Ipp32sc);

    /* Initialize fixed coeffs array */
    for (i = 0; i < numSegments; i++) {
        (state->ppFixedCoefs)[i] = (Ipp32sc *)buf + i * (state->numSubbands);
    }

    /* Initialize subband controller */
    state->controller = (IppsSubbandControllerState_EC_16s *)ptr;
    if (ippsSubbandControllerInit_EC_16s(state->controller, state->numSubbands, state->frameSize, numSegments, freq) != ippStsOk)
        return 1;

    ippsSubbandControllerGetSize_EC_16s(SB_SUBBANDS, SB_FRAME_SIZE, numSegments, freq, &csize);

    ptr += ALIGN(csize);

    /* Initialize tone detection */
    ippsToneDetectGetStateSize_EC_16s(freq, &csize);
    state->tdr = (IppsToneDetectState_EC_16s *)ptr;
    ptr += csize;
    state->tds = (IppsToneDetectState_EC_16s *)ptr;
    ippsToneDetectInit_EC_16s(state->tdr, freq);
    ippsToneDetectInit_EC_16s(state->tds, freq);

    /* zero receive-in history buffer */
    for (i = 0; i < state->windowLen; i++)
        state->pRinHistory[i] = 0;

    /* enable adaptation */
    state->mode = 1;

    state->r_in_pwr = 0;
    state->s_in_pwr = 0;
    state->td_coeff = COEF_ONE - COEF_ONE * 4 * state->frameSize / freq;
    state->td_thres = 500;
    state->td_resr = state->td_ress = 0;
    state->sgain = IPP_MAX_16S;
    state->constrainSubbandNum=0;
    state->convergedSBNum=0;
    return 0;
}

/* Do operation or set mode */
int ec_isb_ModeOp(_isbECState *state, ECOpcode op)
{
    int i, j;

    switch (op) {
    case (EC_COEFFS_ZERO):      /* Zero coeffs of filters */
        {
            for (i = 0; i < state->numSegments; i++)
                for (j = 0; j < state->numSubbands; j++)
                    state->ppAdaptiveCoefs[i][j].re = state->ppAdaptiveCoefs[i][j].im =
                        state->ppFixedCoefs[i][j].re = state->ppFixedCoefs[i][j].im = 0;
        }
        break;
    case(EC_ADAPTATION_ENABLE): /* Enable adaptation */
        if (!(state->mode & 1))
            ippsSubbandControllerReset_EC_16s(state->controller);
        state->mode |= 1;
        break;
    case(EC_ADAPTATION_ENABLE_LITE): /* Enable adaptation */
        if (!(state->mode & 1))
            ippsSubbandControllerReset_EC_16s(state->controller);
        state->mode |= 9; /* Enable adaptation + no constrain */
        break;
    case(EC_ADAPTATION_DISABLE): /* Disable adaptation */
        state->mode &= ~1;
        break;
    case(EC_ADAPTATION_DISABLE_LITE): /* Disable adaptation */
        state->mode &= ~8;
        break;
    case(EC_NLP_ENABLE):    /* Enable NLP */
        state->mode |= 2;
        break;
    case(EC_NLP_DISABLE):   /* Disable NLP */
        state->mode &= ~2;
        break;
    case(EC_TD_ENABLE):     /* Enable ToneDisabler */
        state->mode |= 4;
        break;
    case(EC_TD_DISABLE):    /* Disable ToneDisabler */
        state->mode &= ~4;
        break;
    default:
        break;
    }

    return 0;
}


/* Tone Disabler */
static int ec_isb_ToneDisabler(_isbECState *state, Ipp16s *r_in, Ipp16s *s_in)
{
    int resr, ress;
    int i;
    Ipp64s r_in_pwr = 0, s_in_pwr = 0;

    /* Detect 2100 Hz with periodical phase reversal */
    ippsToneDetect_EC_16s(r_in, state->frameSize, &resr, state->tdr);
    ippsToneDetect_EC_16s(s_in, state->frameSize, &ress, state->tds);

    /* Update receive-in signal and send-in signal powers */