/*
 * BandwidthDetector.cpp
 *
 * Copyright 2021 HIMSA II K/S - www.himsa.com. Represented by EHIMA -
 * www.ehima.com
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "BandwidthDetector.hpp"

#include <cmath>

namespace Lc3Enc {

static const uint8_t I_bw_start_table[5][4] = {
    {53, 0, 0, 0},    // N_bw=1
    {47, 59, 0, 0},   // N_bw=2
    {44, 54, 60, 0},  // N_bw=3
    {41, 51, 57, 61}  // N_bw=4
};

static const uint8_t I_bw_stop_table[5][4] = {
    {63, 0, 0, 0},    // N_bw=1
    {56, 63, 0, 0},   // N_bw=2
    {52, 59, 63, 0},  // N_bw=3
    {49, 55, 60, 63}  // N_bw=4
};

static const uint8_t I_bw_start_table_7p5ms[5][4] = {
    {51, 0, 0, 0},    // N_bw=1
    {45, 58, 0, 0},   // N_bw=2
    {42, 53, 60, 0},  // N_bw=3
    {40, 51, 57, 61}  // N_bw=4
};

static const uint8_t I_bw_stop_table_7p5ms[5][4] = {
    {63, 0, 0, 0},    // N_bw=1
    {55, 63, 0, 0},   // N_bw=2
    {51, 58, 63, 0},  // N_bw=3
    {48, 55, 60, 63}  // N_bw=4
};

static const uint8_t nbits_bw_table[5] = {
    0, 1, 2, 2, 3};  // see 3.4.2.4 Bandwidth interpretation (d09r02_F2F)

static const uint8_t T_Q[4] = {20, 10, 10, 10};
static const uint8_t T_C[4] = {15, 23, 20, 20};
static const uint8_t L_10ms[4] = {4, 4, 3, 1};
static const uint8_t L_7p5ms[4] = {4, 4, 3, 2};

BandwidthDetector::BandwidthDetector(const Lc3Config& lc3Config_)
    : lc3Config(lc3Config_),
      N_bw(calc_N_bw(lc3Config.Fs_ind)),
      nbits_bw(nbits_bw_table[calc_N_bw(lc3Config.Fs_ind)]),
      P_bw(0) {
  if (N_bw != 0) {
    I_bw_start = (lc3Config.N_ms == Lc3Config::FrameDuration::d10ms)
                     ? I_bw_start_table[N_bw - 1]
                     : I_bw_start_table_7p5ms[N_bw - 1];
    I_bw_stop = (lc3Config.N_ms == Lc3Config::FrameDuration::d10ms)
                    ? I_bw_stop_table[N_bw - 1]
                    : I_bw_stop_table_7p5ms[N_bw - 1];
  }
  L = (lc3Config.N_ms == Lc3Config::FrameDuration::d10ms) ? L_10ms : L_7p5ms;
}

BandwidthDetector::~BandwidthDetector() {}

uint8_t BandwidthDetector::calc_N_bw(uint8_t fs_ind) {
  // see Table 3.5: Parameter table bandwidth detector
  // 3.3.5.2 Parameters (d09r02_F2F)
  return fs_ind;
}

void BandwidthDetector::run(const double* const E_B) {
  if (0 == N_bw) {
    return;
  }

  // 3.3.5.1 Algorithm (first stage)  (d09r02_F2F)
  // Note: it seems like there is no need to compute
  //   the entire specified sequence of flahs F_Q[k]
  //   when we start searching for an active band
  //   from the highest fequency band to lower bands
  uint8_t bw_0 = 0;
  for (uint8_t k = N_bw - 1; k < N_bw; k--)  // stops when 0-- goes to 255
  {
    uint8_t width = I_bw_stop[k] - I_bw_start[k] + 1;
    double Q = 0.0;
    for (uint8_t n = I_bw_start[k]; n <= I_bw_stop[k]; n++) {
      Q += E_B[n] / width;
    }
    if (Q >= T_Q[k]) {
      // activity detected (not quiet) -> this is the band index to be selected
      bw_0 = k + 1;
      break;
    }
  }

  // 3.3.5.1 Algorithm (second stage)  (d09r02_F2F)
  if (N_bw == bw_0) {
    P_bw = bw_0;
  } else {
    double Cmax = 0;
    for (uint8_t n = I_bw_start[bw_0] - L[bw_0] + 1; n <= I_bw_start[bw_0] + 1;
         n++) {
      double C = 10 * log10(E_B[n - L[bw_0]] / E_B[n]);
      if (C > Cmax) {
        Cmax = C;
      }
    }
    if (Cmax > T_C[bw_0]) {
      P_bw = bw_0;
    } else {
      P_bw = N_bw;
    }
  }
}

void BandwidthDetector::registerDatapoints(DatapointContainer* datapoints) {
  if (nullptr != datapoints) {
    datapoints->addDatapoint("P_bw", &P_bw, sizeof(P_bw));
  }
}

}  // namespace Lc3Enc