// Copyright 2019 The Fuchsia Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
#include <unistd.h>
#include <zircon/assert.h>

#include <limits>

#include <soc/as370/audio-dsp.h>
// Cascaded integrator–comb filter.
// TODO(andresoportus) generalize and place in signal processing library.
#if defined(__clang__)
// Integrator and differentiator states are allowed to overflow and wrap,
// differentiator undoes intergrator's overflow and wrapping.
[[clang::no_sanitize("undefined")]]
#endif
uint32_t
CicFilter::Filter(uint32_t index,  // e.g. 0.
                  void* input,
                  uint32_t input_size,  // e.g. 16K.
                  void* output,
                  uint32_t input_total_channels,   // e.g. 2.
                  uint32_t input_channel,          // e.g. 0 or 1.
                  uint32_t output_total_channels,  // e.f. 2.
                  uint32_t output_channel,         // e.g. 0  or 1.
                  uint32_t multiplier_shift) {
#ifdef TESTING_CAPTURE_PDM
  static_cast<void>(integrator_state);
  static_cast<void>(differentiator_state);

  // Since input bits per channel is 32, we use 32 bits pointers.
  uint32_t* in = static_cast<uint32_t*>(input);
  uint32_t* in_end = reinterpret_cast<uint32_t*>(static_cast<uint8_t*>(input) + input_size);

  // Since output bits per channel is 32, we use 32 bits pointers.
  uint32_t* out = static_cast<uint32_t*>(output);

  if (index > kMaxIndex) {
    return 0;
  }

  uint32_t amount_pcm = 0;
  while (in < in_end) {
    uint32_t bits = in[input_channel];
    in += input_total_channels;
    out[output_channel] = bits;
    out += output_total_channels;
    amount_pcm += static_cast<uint32_t>(output_total_channels * sizeof(int32_t));
  }
#else

  // Since input bits per channel is 32, we use 32 bits pointers.
  uint32_t* in = static_cast<uint32_t*>(input);
  uint32_t* in_end = reinterpret_cast<uint32_t*>(static_cast<uint8_t*>(input) + input_size);

  // 16 output bits per channel.
  ZX_ASSERT(kOutputBitsPerSample == 16);
  uint16_t* out = static_cast<uint16_t*>(output);

  if (index > kMaxIndex) {
    return 0;
  }

  uint32_t amount_pcm = 0;
  while (in < in_end) {
    // Integrate.
    for (size_t word = 0; word < kInputBitsPerSample / (sizeof(uint32_t) * 8); ++word) {
      uint32_t bits = in[input_channel];
      in += input_total_channels;
      for (uint32_t i = 0; i < sizeof(uint32_t) * 8; ++i, bits >>= 1) {
        // Integrator state is allowed to overflow and wrap, this is ok becuase of modulo
        // arithmetic, the differentiation will undo the wrapping.
        auto plus_or_minus = (static_cast<int32_t>(bits & 1) << 1) - 1;  // +1/-1 from 1/0;
        integrator_state[index][0] += plus_or_minus;
        for (uint32_t stage = 1; stage < kOrder; ++stage) {
          integrator_state[index][stage] += integrator_state[index][stage - 1];
        }
      }
    }

    // COMB (differentiator).
    int32_t acc = integrator_state[index][kOrder - 1];
    int32_t old_acc = 0;
    for (uint32_t stage = 0; stage < kOrder; ++stage) {
      old_acc = acc;
      acc -= differentiator_state[index][stage];
      differentiator_state[index][stage] = old_acc;
    }

    // Output pre-amplified by multiplier_shift.
    int32_t result = acc;
    if (result >= ((1 << (31 - multiplier_shift)) - 1)) {
      result = std::numeric_limits<int32_t>::max();
    } else {
      result <<= multiplier_shift;
    }

    // 16 output bits per channel.
    out[output_channel] = static_cast<int16_t>((result - dc[index]) >> 16);
    out += output_total_channels;
    amount_pcm += static_cast<uint32_t>(output_total_channels * sizeof(int16_t));

    // DC is calculated via a low pass filter as an exponentially weighted moving average
    // using a constant k = 1 / 4096 that makes the calculation fast and has a frequency corner
    // fc = k / ((1 - k) x 2 x pi x dT) = 1.87Hz for a 48kHz rate input (dT = 1 / 48K = 20.83usecs).
    // TODO(andresoportus): Potential improvements include a more sophisticated CIC filter,
    // configuration via metadata of the parameters used (here shift_dc_filter), and paralelization
    // of the filtering process.
    constexpr uint32_t shift_dc_filter = 12;
    dc[index] += (result - dc[index]) >> shift_dc_filter;
  }
#endif
  return amount_pcm;
}