/* aec.cpp
 *
 * Copyright (C) DFS Deutsche Flugsicherung (2004, 2005). 
 * All Rights Reserved.
 *
 * Acoustic Echo Cancellation NLMS-pw algorithm
 *
 * Version 0.3.4 H-infinity LMS-pw algorithm
 * Version 0.3.1 Allow change of stability parameter delta
 * Version 0.3 filter created with www.dsptutor.freeuk.com
 */

#include <stdio.h>

#include <stdlib.h>

#include <string.h>


extern "C" {

	#include <math.h>

	#include "aec_cwrap.h"

	#include <iaxclient.h>

}
//#include "oss.h"


#include "aec.h"

//#include "intercomd.h"


#if 1

/* Vector Dot Product */
REAL dotp(REAL a[], REAL b[])
{
  REAL sum0 = 0.0, sum1 = 0.0;
  int j;

  for (j = 0; j < NLMS_LEN; j += 2) {
    // optimize: partial loop unrolling

    sum0 += a[j] * b[j];
    sum1 += a[j + 1] * b[j + 1];
  }
  return sum0 + sum1;
}

#else


/* this is unfinished work in progress. */
#include <xmmintrin.h>


inline float dotp(float a[], float b[])
{
  int i;
  __m128 xmm0, xmm1, xmm2;
  __m128 xmm3, xmm4, xmm5;
  float sum[8];

  /* XMM convolution - Intel Pentium 3 and above */
  
  sum[0] = sum[1] = sum[2] = sum[3] = 0.0f;
  xmm2 = _mm_load_ps(sum);
    xmm5 = _mm_load_ps(sum);
  for (i = 0; i < NLMS_LEN; i += 8) {
    /* load not aligned data */
    xmm0 = _mm_loadu_ps(&a[i]);    
      xmm3 = _mm_loadu_ps(&a[i+4]);
       
    xmm1 = _mm_loadu_ps(&b[i]);
      xmm4 = _mm_loadu_ps(&b[i+4]);
    
    /* Intel notation: first operand is destination */
    /* GNU as notation: first operand is source */
    xmm0 = _mm_mul_ps(xmm0, xmm1);
      xmm3 = _mm_mul_ps(xmm3, xmm4);
    
    xmm2 = _mm_add_ps(xmm2, xmm0);      
      xmm5 = _mm_add_ps(xmm5, xmm3);
  }
  _mm_store_ps(sum, xmm2);
  _mm_store_ps(&sum[4], xmm5);
 
  return sum[0] + sum[1] + sum[2] + sum[3]
    + sum[4] + sum[5] + sum[6] + sum[7];
}
#endif



AEC::AEC()
{
  max_max_x = 0.0f;
  hangover = 0;
  memset(max_x, 0, sizeof(max_x));
  dtdCnt = dtdNdx = 0;

  max_max_u = 0.0f;
  memset(max_u, 0, sizeof(max_u));
  c_max_u = i_max_u = 0;

  memset(x, 0, sizeof(x));
  memset(xf, 0, sizeof(xf));
  memset(w, 0, sizeof(w));
  j = NLMS_EXT;
  delta = 0.0f;
  setambient(NoiseFloor);
  dfast = dslow = M50dB_PCM;
  xfast = xslow = M40dB_PCM;
  gain = 1.0f;
  Fx.init(2000.0f/RATE);
  Fe.init(2000.0f/RATE);
  Fxx.init(2000.0f/RATE);
  Fdd.init(2000.0f/RATE);
}

inline double AEC::max_dotp_xf_xf(double u)
{
  // optimized implementation of max(u[0], u[1], .., u[L-1]):

  // calculate max of block (DTD_LEN values)

  if (u > max_u[i_max_u]) {
    max_u[i_max_u] = u;
    if (u > max_max_u) {
      max_max_u = u;
      // printf("max_dotp_xf_xf %f\n", sqrt(max_max_u/NLMS_LEN)); 

    }
  }
  if (++c_max_u >= DTD_LEN) {
    c_max_u = 0;
    // calculate max of max

    max_max_u = 0.0f;
    for (int i = 0; i < NLMS_LEN / DTD_LEN; ++i) {
      if (max_u[i] > max_max_u) {
        max_max_u = max_u[i];
      }
    }
    // rotate Ndx

    if (++i_max_u >= NLMS_LEN / DTD_LEN)
      i_max_u = 0;
    max_u[i_max_u] = 0.0f;
  }
  return max_max_u;
}

float ratio;

#if 1

// Adrian soft decision DTD

// (Dual Average Near-End to Far-End signal Ratio DTD)

// This algorithm uses exponential smoothing with differnt

// ageing parameters to get fast and slow near-end and far-end

// signal averages. The ratio of NFRs term

// (dfast / xfast) / (dslow / xslow) is used to compute the stepsize

// A ratio value of 2.5 is mapped to stepsize 0, a ratio of 0 is

// mapped to 1.0 with a limited linear function.

inline float AEC::dtd(REAL d, REAL x)
{
  float stepsize;
  
  d = Fdd.highpass(d);
  x = Fxx.highpass(x);

  // fast near-end and far-end average

  dfast += 2e-3f * (fabsf(d) - dfast);
  xfast += 2e-3f * (fabsf(x) - xfast);
  
  // slow near-end and far-end average

  dslow += 1e-6f * (fabsf(d) - dslow);
  xslow += 1e-6f * (fabsf(x) - xslow);
  
  if (xfast < M70dB_PCM) {
    return 0.0;   // no Spk signal

  }
  
  if (dfast < M70dB_PCM) {
    return 0.0;   // no Mic signal

  }
  
  // ratio of NFRs

  ratio = (dfast * xslow) / (dslow * xfast);
  
  // begrenzte lineare Kennlinie

  const float M = (STEPY2 - STEPY1) / (STEPX2 - STEPX1);
  if (ratio < STEPX1) {
    stepsize = STEPY1;
  } else if (ratio > STEPX2) {
    stepsize = STEPY2;
  } else {
    // Punktrichtungsform einer Geraden

    stepsize = M * (ratio - STEPX1) + STEPY1;
  }

  return stepsize;
}

#else


// Geigel DTD. return is 0.0 or 1.0

inline float AEC::dtd(REAL d, REAL x)
{
  // optimized implementation of max(|x[0]|, |x[1]|, .., |x[L-1]|):

  // calculate max of block (DTD_LEN values)

  x = fabsf(x);
  if (x > max_x[dtdNdx]) {
    max_x[dtdNdx] = x;
    if (x > max_max_x) {
      max_max_x = x;
    }
  }
  if (++dtdCnt >= DTD_LEN) {
    dtdCnt = 0;
    // calculate max of max

    max_max_x = 0.0f;
    for (int i = 0; i < NLMS_LEN / DTD_LEN; ++i) {
      if (max_x[i] > max_max_x) {
        max_max_x = max_x[i];
      }
    }
    // rotate Ndx

    if (++dtdNdx >= NLMS_LEN / DTD_LEN)
      dtdNdx = 0;
    max_x[dtdNdx] = 0.0f;
  }
  // The Geigel DTD algorithm with Hangover timer Thold

  if (fabsf(d) >= GeigelThreshold * max_max_x) {
    hangover = Thold;
  }

  if (hangover)
    --hangover;
   
  // Silence is the same as Double Talk 

  if (max_max_x < M40dB_PCM) {
    // return 0;

  }

  if (hangover > 0) {
    return STEPY2;
  } else {
    return STEPY1;
  }
}

#endif



inline REAL AEC::nlms_pw(REAL d, REAL x_, float stepsize)
{
  x[j] = x_;
  xf[j] = Fx.highpass(x_);     // pre-whitening of x


  // calculate error value 

  // (mic signal - estimated mic signal from spk signal)

  REAL e = d - dotp(w, x + j);
  REAL ef = Fe.highpass(e);     // pre-whitening of e

  
  // optimize: iterative dotp(xf, xf)

  dotp_xf_xf += (xf[j] * xf[j] - xf[j + NLMS_LEN - 1] * xf[j + NLMS_LEN - 1]);
    
  if (stepsize > 0.0) {
    // calculate variable step size

    REAL mikro_ef = stepsize * ef / dotp_xf_xf;
    
    // inspired by H-infinity theory: use max_dotp_xf_xf(dotp_xf_xf)

    // REAL mikro_ef = 1.0f * ef / max_dotp_xf_xf(dotp_xf_xf);


    // update tap weights (filter learning)

    int i;
    for (i = 0; i < NLMS_LEN; i += 2) {
      // optimize: partial loop unrolling

      w[i] += mikro_ef * xf[i + j];
      w[i + 1] += mikro_ef * xf[i + j + 1];
    }
  }

  if (--j < 0) {
    // optimize: decrease number of memory copies

    j = NLMS_EXT;
    memmove(x + j + 1, x, (NLMS_LEN - 1) * sizeof(REAL));
    memmove(xf + j + 1, xf, (NLMS_LEN - 1) * sizeof(REAL));
  }
  
  // Saturation

  if (e > MAXPCM) {
    return MAXPCM;
  } else if (e < -MAXPCM) {
    return -MAXPCM;
  } else {
    return e;
  }
}

// soft decision Acoustic Echo Suppression (AES) or

// Non Linear Processor (NLP): 

// attenuate d for large x signals (like AGC)

inline float AEC::aes(REAL d)
{
  const float X1 = M70dB_PCM, Y1 = M0dB;
  const float X2 = M40dB_PCM, Y2 = M24dB;
  const float M = (Y2 - Y1) / (X2 - X1);

  // begrenzte lineare Kennlinie

  float atten;
  if (xfast < X1) {
    atten = Y1;
  } else if (xfast > X2) {
    atten = Y2;
  } else {
    // Punktrichtungsform einer Geraden

    atten = M * (xfast - X1) + Y1;
  }
  return d * atten;
}


int AEC::doAEC(int d_, int x_, int enable)
{
  extern int noaes;

  REAL d = (REAL) d_;
  REAL x = (REAL) x_;

  // Mic Highpass Filter - to remove DC

  d = acMic.highpass(d);

  // Mic Highpass Filter - cut-off below 300Hz

  d = cutoff.highpass(d);
  
  // Amplify, for e.g. Soundcards with -6dB max. volume

  d *= gain;

  // Spk Highpass Filter - to remove DC

  if (enable) 
    x = acSpk.highpass(x);

  // Double Talk Detector

  float stepsize = dtd(d, x);
  
  // Acoustic Echo Cancellation

  d = nlms_pw(d, x, stepsize);
  
  // Acoustic Echo Suppression

  //if (NO == noaes) {

    d = aes(d);
  //}


  return (int) d;
}