/*
 *  Copyright (c) 2017 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/audio_processing/aec3/adaptive_fir_filter.h"

#if defined(WEBRTC_HAS_NEON)
#include <arm_neon.h>
#endif
#include "webrtc/typedefs.h"
#if defined(WEBRTC_ARCH_X86_FAMILY)
#include <emmintrin.h>
#endif
#include <algorithm>
#include <functional>

#include "webrtc/modules/audio_processing/aec3/fft_data.h"
#include "webrtc/rtc_base/checks.h"

namespace webrtc {

namespace aec3 {

// Computes and stores the frequency response of the filter.
void UpdateFrequencyResponse(
    rtc::ArrayView<const FftData> H,
    std::vector<std::array<float, kFftLengthBy2Plus1>>* H2) {
  RTC_DCHECK_EQ(H.size(), H2->size());
  for (size_t k = 0; k < H.size(); ++k) {
    std::transform(H[k].re.begin(), H[k].re.end(), H[k].im.begin(),
                   (*H2)[k].begin(),
                   [](float a, float b) { return a * a + b * b; });
  }
}

#if defined(WEBRTC_HAS_NEON)
// Computes and stores the frequency response of the filter.
void UpdateFrequencyResponse_NEON(
    rtc::ArrayView<const FftData> H,
    std::vector<std::array<float, kFftLengthBy2Plus1>>* H2) {
  RTC_DCHECK_EQ(H.size(), H2->size());
  for (size_t k = 0; k < H.size(); ++k) {
    for (size_t j = 0; j < kFftLengthBy2; j += 4) {
      const float32x4_t re = vld1q_f32(&H[k].re[j]);
      const float32x4_t im = vld1q_f32(&H[k].im[j]);
      float32x4_t H2_k_j = vmulq_f32(re, re);
      H2_k_j = vmlaq_f32(H2_k_j, im, im);
      vst1q_f32(&(*H2)[k][j], H2_k_j);
    }
    (*H2)[k][kFftLengthBy2] = H[k].re[kFftLengthBy2] * H[k].re[kFftLengthBy2] +
                              H[k].im[kFftLengthBy2] * H[k].im[kFftLengthBy2];
  }
}
#endif

#if defined(WEBRTC_ARCH_X86_FAMILY)
// Computes and stores the frequency response of the filter.
void UpdateFrequencyResponse_SSE2(
    rtc::ArrayView<const FftData> H,
    std::vector<std::array<float, kFftLengthBy2Plus1>>* H2) {
  RTC_DCHECK_EQ(H.size(), H2->size());
  for (size_t k = 0; k < H.size(); ++k) {
    for (size_t j = 0; j < kFftLengthBy2; j += 4) {
      const __m128 re = _mm_loadu_ps(&H[k].re[j]);
      const __m128 re2 = _mm_mul_ps(re, re);
      const __m128 im = _mm_loadu_ps(&H[k].im[j]);
      const __m128 im2 = _mm_mul_ps(im, im);
      const __m128 H2_k_j = _mm_add_ps(re2, im2);
      _mm_storeu_ps(&(*H2)[k][j], H2_k_j);
    }
    (*H2)[k][kFftLengthBy2] = H[k].re[kFftLengthBy2] * H[k].re[kFftLengthBy2] +
                              H[k].im[kFftLengthBy2] * H[k].im[kFftLengthBy2];
  }
}
#endif

// Computes and stores the echo return loss estimate of the filter, which is the
// sum of the partition frequency responses.
void UpdateErlEstimator(
    const std::vector<std::array<float, kFftLengthBy2Plus1>>& H2,
    std::array<float, kFftLengthBy2Plus1>* erl) {
  erl->fill(0.f);
  for (auto& H2_j : H2) {
    std::transform(H2_j.begin(), H2_j.end(), erl->begin(), erl->begin(),
                   std::plus<float>());
  }
}

#if defined(WEBRTC_HAS_NEON)
// Computes and stores the echo return loss estimate of the filter, which is the
// sum of the partition frequency responses.
void UpdateErlEstimator_NEON(
    const std::vector<std::array<float, kFftLengthBy2Plus1>>& H2,
    std::array<float, kFftLengthBy2Plus1>* erl) {
  erl->fill(0.f);
  for (auto& H2_j : H2) {
    for (size_t k = 0; k < kFftLengthBy2; k += 4) {
      const float32x4_t H2_j_k = vld1q_f32(&H2_j[k]);
      float32x4_t erl_k = vld1q_f32(&(*erl)[k]);
      erl_k = vaddq_f32(erl_k, H2_j_k);
      vst1q_f32(&(*erl)[k], erl_k);
    }
    (*erl)[kFftLengthBy2] += H2_j[kFftLengthBy2];
  }
}
#endif

#if defined(WEBRTC_ARCH_X86_FAMILY)
// Computes and stores the echo return loss estimate of the filter, which is the
// sum of the partition frequency responses.
void UpdateErlEstimator_SSE2(
    const std::vector<std::array<float, kFftLengthBy2Plus1>>& H2,
    std::array<float, kFftLengthBy2Plus1>* erl) {
  erl->fill(0.f);
  for (auto& H2_j : H2) {
    for (size_t k = 0; k < kFftLengthBy2; k += 4) {
      const __m128 H2_j_k = _mm_loadu_ps(&H2_j[k]);
      __m128 erl_k = _mm_loadu_ps(&(*erl)[k]);
      erl_k = _mm_add_ps(erl_k, H2_j_k);
      _mm_storeu_ps(&(*erl)[k], erl_k);
    }
    (*erl)[kFftLengthBy2] += H2_j[kFftLengthBy2];
  }
}
#endif

// Adapts the filter partitions as H(t+1)=H(t)+G(t)*conj(X(t)).
void AdaptPartitions(const RenderBuffer& render_buffer,
                     const FftData& G,
                     rtc::ArrayView<FftData> H) {
  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  size_t index = render_buffer.Position();
  for (auto& H_j : H) {
    const FftData& X = render_buffer_data[index];
    for (size_t k = 0; k < kFftLengthBy2Plus1; ++k) {
      H_j.re[k] += X.re[k] * G.re[k] + X.im[k] * G.im[k];
      H_j.im[k] += X.re[k] * G.im[k] - X.im[k] * G.re[k];
    }

    index = index < (render_buffer_data.size() - 1) ? index + 1 : 0;
  }
}

#if defined(WEBRTC_HAS_NEON)
// Adapts the filter partitions. (NEON variant)
void AdaptPartitions_NEON(const RenderBuffer& render_buffer,
                          const FftData& G,
                          rtc::ArrayView<FftData> H) {
  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  const int lim1 =
      std::min(render_buffer_data.size() - render_buffer.Position(), H.size());
  const int lim2 = H.size();
  constexpr int kNumFourBinBands = kFftLengthBy2 / 4;
  FftData* H_j = &H[0];
  const FftData* X = &render_buffer_data[render_buffer.Position()];
  int limit = lim1;
  int j = 0;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      for (int k = 0, n = 0; n < kNumFourBinBands; ++n, k += 4) {
        const float32x4_t G_re = vld1q_f32(&G.re[k]);
        const float32x4_t G_im = vld1q_f32(&G.im[k]);
        const float32x4_t X_re = vld1q_f32(&X->re[k]);
        const float32x4_t X_im = vld1q_f32(&X->im[k]);
        const float32x4_t H_re = vld1q_f32(&H_j->re[k]);
        const float32x4_t H_im = vld1q_f32(&H_j->im[k]);
        const float32x4_t a = vmulq_f32(X_re, G_re);
        const float32x4_t e = vmlaq_f32(a, X_im, G_im);
        const float32x4_t c = vmulq_f32(X_re, G_im);
        const float32x4_t f = vmlsq_f32(c, X_im, G_re);
        const float32x4_t g = vaddq_f32(H_re, e);
        const float32x4_t h = vaddq_f32(H_im, f);

        vst1q_f32(&H_j->re[k], g);
        vst1q_f32(&H_j->im[k], h);
      }
    }

    X = &render_buffer_data[0];
    limit = lim2;
  } while (j < lim2);

  H_j = &H[0];
  X = &render_buffer_data[render_buffer.Position()];
  limit = lim1;
  j = 0;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      H_j->re[kFftLengthBy2] += X->re[kFftLengthBy2] * G.re[kFftLengthBy2] +
                                X->im[kFftLengthBy2] * G.im[kFftLengthBy2];
      H_j->im[kFftLengthBy2] += X->re[kFftLengthBy2] * G.im[kFftLengthBy2] -
                                X->im[kFftLengthBy2] * G.re[kFftLengthBy2];
    }

    X = &render_buffer_data[0];
    limit = lim2;
  } while (j < lim2);
}
#endif

#if defined(WEBRTC_ARCH_X86_FAMILY)
// Adapts the filter partitions. (SSE2 variant)
void AdaptPartitions_SSE2(const RenderBuffer& render_buffer,
                          const FftData& G,
                          rtc::ArrayView<FftData> H) {
  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  const int lim1 =
      std::min(render_buffer_data.size() - render_buffer.Position(), H.size());
  const int lim2 = H.size();
  constexpr int kNumFourBinBands = kFftLengthBy2 / 4;
  FftData* H_j;
  const FftData* X;
  int limit;
  int j;
  for (int k = 0, n = 0; n < kNumFourBinBands; ++n, k += 4) {
    const __m128 G_re = _mm_loadu_ps(&G.re[k]);
    const __m128 G_im = _mm_loadu_ps(&G.im[k]);

    H_j = &H[0];
    X = &render_buffer_data[render_buffer.Position()];
    limit = lim1;
    j = 0;
    do {
      for (; j < limit; ++j, ++H_j, ++X) {
        const __m128 X_re = _mm_loadu_ps(&X->re[k]);
        const __m128 X_im = _mm_loadu_ps(&X->im[k]);
        const __m128 H_re = _mm_loadu_ps(&H_j->re[k]);
        const __m128 H_im = _mm_loadu_ps(&H_j->im[k]);
        const __m128 a = _mm_mul_ps(X_re, G_re);
        const __m128 b = _mm_mul_ps(X_im, G_im);
        const __m128 c = _mm_mul_ps(X_re, G_im);
        const __m128 d = _mm_mul_ps(X_im, G_re);
        const __m128 e = _mm_add_ps(a, b);
        const __m128 f = _mm_sub_ps(c, d);
        const __m128 g = _mm_add_ps(H_re, e);
        const __m128 h = _mm_add_ps(H_im, f);
        _mm_storeu_ps(&H_j->re[k], g);
        _mm_storeu_ps(&H_j->im[k], h);
      }

      X = &render_buffer_data[0];
      limit = lim2;
    } while (j < lim2);
  }

  H_j = &H[0];
  X = &render_buffer_data[render_buffer.Position()];
  limit = lim1;
  j = 0;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      H_j->re[kFftLengthBy2] += X->re[kFftLengthBy2] * G.re[kFftLengthBy2] +
                                X->im[kFftLengthBy2] * G.im[kFftLengthBy2];
      H_j->im[kFftLengthBy2] += X->re[kFftLengthBy2] * G.im[kFftLengthBy2] -
                                X->im[kFftLengthBy2] * G.re[kFftLengthBy2];
    }

    X = &render_buffer_data[0];
    limit = lim2;
  } while (j < lim2);
}
#endif

// Produces the filter output.
void ApplyFilter(const RenderBuffer& render_buffer,
                 rtc::ArrayView<const FftData> H,
                 FftData* S) {
  S->re.fill(0.f);
  S->im.fill(0.f);

  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  size_t index = render_buffer.Position();
  for (auto& H_j : H) {
    const FftData& X = render_buffer_data[index];
    for (size_t k = 0; k < kFftLengthBy2Plus1; ++k) {
      S->re[k] += X.re[k] * H_j.re[k] - X.im[k] * H_j.im[k];
      S->im[k] += X.re[k] * H_j.im[k] + X.im[k] * H_j.re[k];
    }
    index = index < (render_buffer_data.size() - 1) ? index + 1 : 0;
  }
}

#if defined(WEBRTC_HAS_NEON)
// Produces the filter output (NEON variant).
void ApplyFilter_NEON(const RenderBuffer& render_buffer,
                      rtc::ArrayView<const FftData> H,
                      FftData* S) {
  RTC_DCHECK_GE(H.size(), H.size() - 1);
  S->re.fill(0.f);
  S->im.fill(0.f);

  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  const int lim1 =
      std::min(render_buffer_data.size() - render_buffer.Position(), H.size());
  const int lim2 = H.size();
  constexpr int kNumFourBinBands = kFftLengthBy2 / 4;
  const FftData* H_j = &H[0];
  const FftData* X = &render_buffer_data[render_buffer.Position()];

  int j = 0;
  int limit = lim1;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      for (int k = 0, n = 0; n < kNumFourBinBands; ++n, k += 4) {
        const float32x4_t X_re = vld1q_f32(&X->re[k]);
        const float32x4_t X_im = vld1q_f32(&X->im[k]);
        const float32x4_t H_re = vld1q_f32(&H_j->re[k]);
        const float32x4_t H_im = vld1q_f32(&H_j->im[k]);
        const float32x4_t S_re = vld1q_f32(&S->re[k]);
        const float32x4_t S_im = vld1q_f32(&S->im[k]);
        const float32x4_t a = vmulq_f32(X_re, H_re);
        const float32x4_t e = vmlsq_f32(a, X_im, H_im);
        const float32x4_t c = vmulq_f32(X_re, H_im);
        const float32x4_t f = vmlaq_f32(c, X_im, H_re);
        const float32x4_t g = vaddq_f32(S_re, e);
        const float32x4_t h = vaddq_f32(S_im, f);
        vst1q_f32(&S->re[k], g);
        vst1q_f32(&S->im[k], h);
      }
    }
    limit = lim2;
    X = &render_buffer_data[0];
  } while (j < lim2);

  H_j = &H[0];
  X = &render_buffer_data[render_buffer.Position()];
  j = 0;
  limit = lim1;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      S->re[kFftLengthBy2] += X->re[kFftLengthBy2] * H_j->re[kFftLengthBy2] -
                              X->im[kFftLengthBy2] * H_j->im[kFftLengthBy2];
      S->im[kFftLengthBy2] += X->re[kFftLengthBy2] * H_j->im[kFftLengthBy2] +
                              X->im[kFftLengthBy2] * H_j->re[kFftLengthBy2];
    }
    limit = lim2;
    X = &render_buffer_data[0];
  } while (j < lim2);
}
#endif

#if defined(WEBRTC_ARCH_X86_FAMILY)
// Produces the filter output (SSE2 variant).
void ApplyFilter_SSE2(const RenderBuffer& render_buffer,
                      rtc::ArrayView<const FftData> H,
                      FftData* S) {
  RTC_DCHECK_GE(H.size(), H.size() - 1);
  S->re.fill(0.f);
  S->im.fill(0.f);

  rtc::ArrayView<const FftData> render_buffer_data = render_buffer.Buffer();
  const int lim1 =
      std::min(render_buffer_data.size() - render_buffer.Position(), H.size());
  const int lim2 = H.size();
  constexpr int kNumFourBinBands = kFftLengthBy2 / 4;
  const FftData* H_j = &H[0];
  const FftData* X = &render_buffer_data[render_buffer.Position()];

  int j = 0;
  int limit = lim1;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      for (int k = 0, n = 0; n < kNumFourBinBands; ++n, k += 4) {
        const __m128 X_re = _mm_loadu_ps(&X->re[k]);
        const __m128 X_im = _mm_loadu_ps(&X->im[k]);
        const __m128 H_re = _mm_loadu_ps(&H_j->re[k]);
        const __m128 H_im = _mm_loadu_ps(&H_j->im[k]);
        const __m128 S_re = _mm_loadu_ps(&S->re[k]);
        const __m128 S_im = _mm_loadu_ps(&S->im[k]);
        const __m128 a = _mm_mul_ps(X_re, H_re);
        const __m128 b = _mm_mul_ps(X_im, H_im);
        const __m128 c = _mm_mul_ps(X_re, H_im);
        const __m128 d = _mm_mul_ps(X_im, H_re);
        const __m128 e = _mm_sub_ps(a, b);
        const __m128 f = _mm_add_ps(c, d);
        const __m128 g = _mm_add_ps(S_re, e);
        const __m128 h = _mm_add_ps(S_im, f);
        _mm_storeu_ps(&S->re[k], g);
        _mm_storeu_ps(&S->im[k], h);
      }
    }
    limit = lim2;
    X = &render_buffer_data[0];
  } while (j < lim2);

  H_j = &H[0];
  X = &render_buffer_data[render_buffer.Position()];
  j = 0;
  limit = lim1;
  do {
    for (; j < limit; ++j, ++H_j, ++X) {
      S->re[kFftLengthBy2] += X->re[kFftLengthBy2] * H_j->re[kFftLengthBy2] -
                              X->im[kFftLengthBy2] * H_j->im[kFftLengthBy2];
      S->im[kFftLengthBy2] += X->re[kFftLengthBy2] * H_j->im[kFftLengthBy2] +
                              X->im[kFftLengthBy2] * H_j->re[kFftLengthBy2];
    }
    limit = lim2;
    X = &render_buffer_data[0];
  } while (j < lim2);
}
#endif

}  // namespace aec3

AdaptiveFirFilter::AdaptiveFirFilter(size_t size_partitions,
                                     Aec3Optimization optimization,
                                     ApmDataDumper* data_dumper)
    : data_dumper_(data_dumper),
      fft_(),
      optimization_(optimization),
      H_(size_partitions),
      H2_(size_partitions, std::array<float, kFftLengthBy2Plus1>()) {
  RTC_DCHECK(data_dumper_);

  h_.fill(0.f);
  for (auto& H_j : H_) {
    H_j.Clear();
  }
  for (auto& H2_k : H2_) {
    H2_k.fill(0.f);
  }
  erl_.fill(0.f);
}

AdaptiveFirFilter::~AdaptiveFirFilter() = default;

void AdaptiveFirFilter::HandleEchoPathChange() {
  h_.fill(0.f);
  for (auto& H_j : H_) {
    H_j.Clear();
  }
  for (auto& H2_k : H2_) {
    H2_k.fill(0.f);
  }
  erl_.fill(0.f);
}

void AdaptiveFirFilter::Filter(const RenderBuffer& render_buffer,
                               FftData* S) const {
  RTC_DCHECK(S);
  switch (optimization_) {
#if defined(WEBRTC_ARCH_X86_FAMILY)
    case Aec3Optimization::kSse2:
      aec3::ApplyFilter_SSE2(render_buffer, H_, S);
      break;
#endif
#if defined(WEBRTC_HAS_NEON)
    case Aec3Optimization::kNeon:
      aec3::ApplyFilter_NEON(render_buffer, H_, S);
      break;
#endif
    default:
      aec3::ApplyFilter(render_buffer, H_, S);
  }
}

void AdaptiveFirFilter::Adapt(const RenderBuffer& render_buffer,
                              const FftData& G) {
  // Adapt the filter.
  switch (optimization_) {
#if defined(WEBRTC_ARCH_X86_FAMILY)
    case Aec3Optimization::kSse2:
      aec3::AdaptPartitions_SSE2(render_buffer, G, H_);
      break;
#endif
#if defined(WEBRTC_HAS_NEON)
    case Aec3Optimization::kNeon:
      aec3::AdaptPartitions_NEON(render_buffer, G, H_);
      break;
#endif
    default:
      aec3::AdaptPartitions(render_buffer, G, H_);
  }

  // Constrain the filter partitions in a cyclic manner.
  Constrain();

  // Update the frequency response and echo return loss for the filter.
  switch (optimization_) {
#if defined(WEBRTC_ARCH_X86_FAMILY)
    case Aec3Optimization::kSse2:
      aec3::UpdateFrequencyResponse_SSE2(H_, &H2_);
      aec3::UpdateErlEstimator_SSE2(H2_, &erl_);
      break;
#endif
#if defined(WEBRTC_HAS_NEON)
    case Aec3Optimization::kNeon:
      aec3::UpdateFrequencyResponse_NEON(H_, &H2_);
      aec3::UpdateErlEstimator_NEON(H2_, &erl_);
      break;
#endif
    default:
      aec3::UpdateFrequencyResponse(H_, &H2_);
      aec3::UpdateErlEstimator(H2_, &erl_);
  }
}

// Constrains the a partiton of the frequency domain filter to be limited in
// time via setting the relevant time-domain coefficients to zero.
void AdaptiveFirFilter::Constrain() {
  std::array<float, kFftLength> h;
  fft_.Ifft(H_[partition_to_constrain_], &h);

  static constexpr float kScale = 1.0f / kFftLengthBy2;
  std::for_each(h.begin(), h.begin() + kFftLengthBy2,
                [](float& a) { a *= kScale; });
  std::fill(h.begin() + kFftLengthBy2, h.end(), 0.f);

  std::copy(h.begin(), h.begin() + kFftLengthBy2,
            h_.begin() + partition_to_constrain_ * kFftLengthBy2);

  fft_.Fft(&h, &H_[partition_to_constrain_]);

  partition_to_constrain_ = partition_to_constrain_ < (H_.size() - 1)
                                ? partition_to_constrain_ + 1
                                : 0;
}

}  // namespace webrtc