/******************************************************************************
 *                                                                            *
 * Copyright (C) 2023 The Android Open Source Project
 *
 * 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.
 *
 *****************************************************************************
 * Originally developed and contributed by Ittiam Systems Pvt. Ltd, Bangalore
 */

#include <string.h>
#include <math.h>
#include "ixheaac_type_def.h"
#include "ixheaace_adjust_threshold_data.h"
#include "iusace_bitbuffer.h"

/* DRC */
#include "impd_drc_common_enc.h"
#include "impd_drc_uni_drc.h"
#include "impd_drc_tables.h"
#include "impd_drc_api.h"
#include "impd_drc_uni_drc_eq.h"
#include "impd_drc_uni_drc_filter_bank.h"
#include "impd_drc_gain_enc.h"
#include "impd_drc_struct_def.h"

#include "iusace_cnst.h"
#include "iusace_tns_usac.h"
#include "iusace_psy_mod.h"
#include "iusace_fd_qc_util.h"
#include "iusace_tns_usac.h"
#include "iusace_config.h"
#include "iusace_arith_enc.h"
#include "iusace_fd_quant.h"
#include "iusace_block_switch_const.h"
#include "iusace_block_switch_struct_def.h"
#include "iusace_ms.h"
#include "iusace_signal_classifier.h"
#include "ixheaace_sbr_header.h"
#include "ixheaace_config.h"
#include "ixheaace_asc_write.h"
#include "iusace_main.h"
#include "iusace_func_prototypes.h"
#include "iusace_lpd_rom.h"
#include "ixheaace_common_utils.h"

WORD32 ia_get_sample_rate(WORD32 sample_rate) {
  if (92017 <= sample_rate) {
    return 11;
  }
  if (75132 <= sample_rate) {
    return 10;
  }
  if (55426 <= sample_rate) {
    return 9;
  }
  if (46009 <= sample_rate) {
    return 8;
  }
  if (37566 <= sample_rate) {
    return 7;
  }
  if (27713 <= sample_rate) {
    return 6;
  }
  if (23004 <= sample_rate) {
    return 5;
  }
  if (18783 <= sample_rate) {
    return 4;
  }
  if (13856 <= sample_rate) {
    return 3;
  }
  if (11502 <= sample_rate) {
    return 2;
  }
  if (9391 <= sample_rate) {
    return 1;
  }
  return 0;
}

VOID iusace_write_bits2buf(WORD32 value, WORD32 no_of_bits, WORD16 *bitstream) {
  WORD16 *pt_bitstream;
  WORD32 i;
  pt_bitstream = bitstream + no_of_bits;
  for (i = 0; i < no_of_bits; i++) {
    *--pt_bitstream = (WORD16)(value & MASK);
    value >>= 1;
  }
  return;
}

WORD32 iusace_get_num_params(WORD32 *qn) {
  return 2 + ((qn[0] > 0) ? 9 : 0) + ((qn[1] > 0) ? 9 : 0);
}

FLOAT32 iusace_cal_segsnr(FLOAT32 *sig1, FLOAT32 *sig2, WORD16 len, WORD16 nseg) {
  FLOAT32 snr = 0.0f;
  FLOAT32 signal, noise, error, fac;
  WORD16 i, j;
  for (i = 0; i < len; i += nseg) {
    signal = 1e-6f;
    noise = 1e-6f;
    for (j = 0; j < nseg; j++) {
      signal += (*sig1) * (*sig1);
      error = *sig1++ - *sig2++;
      noise += error * error;
    }
    snr += (FLOAT32)log10((FLOAT64)(signal / noise));
  }
  fac = ((FLOAT32)(10 * nseg)) / (FLOAT32)len;
  snr = fac * snr;
  if (snr < -99.0f) {
    snr = -99.0f;
  }
  return (snr);
}

VOID iusace_highpass_50hz_12k8(FLOAT32 *signal, WORD32 lg, FLOAT32 *mem, WORD32 fscale) {
  WORD32 i;
  WORD32 sr_idx = 0;
  FLOAT32 x0, x1, x2, y0, y1, y2;
  const FLOAT32 *a = NULL, *b = NULL;

  y1 = mem[0];
  y2 = mem[1];
  x0 = mem[2];
  x1 = mem[3];
  sr_idx = ia_get_sample_rate(fscale);
  a = &iusace_hp20_filter_coeffs[sr_idx][0];
  b = &iusace_hp20_filter_coeffs[sr_idx][2];

  for (i = 0; i < lg; i++) {
    x2 = x1;
    x1 = x0;
    x0 = signal[i];
    y0 = (y1 * a[0]) + (y2 * a[1]) + (x0 * b[1]) + (x1 * b[0]) + (x2 * b[1]);
    signal[i] = y0;
    y2 = y1;
    y1 = y0;
  }

  mem[0] = ((y1 > 1e-10) | (y1 < -1e-10)) ? y1 : 0;
  mem[1] = ((y2 > 1e-10) | (y2 < -1e-10)) ? y2 : 0;
  mem[2] = ((x0 > 1e-10) | (x0 < -1e-10)) ? x0 : 0;
  mem[3] = ((x1 > 1e-10) | (x1 < -1e-10)) ? x1 : 0;
}

VOID iusace_apply_preemph(FLOAT32 *signal, FLOAT32 factor, WORD32 length, FLOAT32 *mem) {
  WORD32 i;
  FLOAT32 temp;
  temp = signal[length - 1];
  for (i = length - 1; i > 0; i--) {
    signal[i] = signal[i] - factor * signal[i - 1];
  }
  signal[0] -= factor * (*mem);
  *mem = temp;
}

VOID iusace_apply_deemph(FLOAT32 *signal, FLOAT32 factor, WORD32 length, FLOAT32 *mem) {
  WORD32 i;
  signal[0] = signal[0] + factor * (*mem);
  for (i = 1; i < length; i++) {
    signal[i] = signal[i] + factor * signal[i - 1];
  }
  *mem = signal[length - 1];
  if ((*mem < 1e-10) & (*mem > -1e-10)) {
    *mem = 0;
  }
}

VOID iusace_synthesis_tool_float(FLOAT32 *a, FLOAT32 *x, FLOAT32 *y, WORD32 l, FLOAT32 *mem,
                                 FLOAT32 *scratch_synth_tool) {
  FLOAT32 s;
  FLOAT32 *yy;
  WORD32 i, j;
  memcpy(scratch_synth_tool, mem, ORDER * sizeof(FLOAT32));
  yy = &scratch_synth_tool[ORDER];
  for (i = 0; i < l; i++) {
    s = x[i];
    for (j = 1; j <= ORDER; j += 4) {
      s -= a[j] * yy[i - j];
      s -= a[j + 1] * yy[i - (j + 1)];
      s -= a[j + 2] * yy[i - (j + 2)];
      s -= a[j + 3] * yy[i - (j + 3)];
    }
    yy[i] = s;
    y[i] = s;
  }
}

VOID iusace_compute_lp_residual(FLOAT32 *a, FLOAT32 *x, FLOAT32 *y, WORD32 l) {
  FLOAT32 s;
  WORD32 i;
  for (i = 0; i < l; i++) {
    s = x[i];
    s += a[1] * x[i - 1];
    s += a[2] * x[i - 2];
    s += a[3] * x[i - 3];
    s += a[4] * x[i - 4];
    s += a[5] * x[i - 5];
    s += a[6] * x[i - 6];
    s += a[7] * x[i - 7];
    s += a[8] * x[i - 8];
    s += a[9] * x[i - 9];
    s += a[10] * x[i - 10];
    s += a[11] * x[i - 11];
    s += a[12] * x[i - 12];
    s += a[13] * x[i - 13];
    s += a[14] * x[i - 14];
    s += a[15] * x[i - 15];
    s += a[16] * x[i - 16];
    y[i] = s;
  }
}

VOID iusace_convolve(FLOAT32 *signal, FLOAT32 *wsynth_filter_ir, FLOAT32 *conv_out) {
  FLOAT32 temp;
  WORD32 i, n;
  for (n = 0; n < LEN_SUBFR; n += 2) {
    temp = 0.0f;
    for (i = 0; i <= n; i++) {
      temp += signal[i] * wsynth_filter_ir[n - i];
    }
    conv_out[n] = temp;
    temp = 0.0f;
    for (i = 0; i <= (n + 1); i += 2) {
      temp += signal[i] * wsynth_filter_ir[(n + 1) - i];
      temp += signal[i + 1] * wsynth_filter_ir[n - i];
    }
    conv_out[n + 1] = temp;
  }
}

VOID iusace_autocorr_plus(FLOAT32 *speech, FLOAT32 *auto_corr_vector, WORD32 window_len,
                          const FLOAT32 *lp_analysis_win, FLOAT32 *temp_aut_corr) {
  FLOAT32 val;
  WORD16 i, j;
  for (i = 0; i < window_len; i++) {
    temp_aut_corr[i] = speech[i] * lp_analysis_win[i];
  }
  for (i = 0; i <= ORDER; i++) {
    val = 0.0f;
    for (j = 0; j < window_len - i; j++) {
      val += temp_aut_corr[j] * temp_aut_corr[j + i];
    }
    auto_corr_vector[i] = val;
  }
  if (auto_corr_vector[0] < 1.0) {
    auto_corr_vector[0] = 1.0;
  }
}

static VOID iusace_get_norm_correlation(FLOAT32 *exc, FLOAT32 *xn, FLOAT32 *wsyn_filt_ir,
                                        WORD32 min_interval, WORD32 max_interval,
                                        FLOAT32 *norm_corr) {
  WORD32 i, j, k;
  FLOAT32 filt_prev_exc[LEN_SUBFR];
  FLOAT32 energy_filt_exc, corr, norm;
  k = -min_interval;

  iusace_convolve(&exc[k], wsyn_filt_ir, filt_prev_exc);

  for (i = min_interval; i <= max_interval; i++) {
    corr = 0.0F;
    energy_filt_exc = 0.01F;
    for (j = 0; j < LEN_SUBFR; j++) {
      corr += xn[j] * filt_prev_exc[j];
      energy_filt_exc += filt_prev_exc[j] * filt_prev_exc[j];
    }

    norm = (FLOAT32)(1.0f / sqrt(energy_filt_exc));
    norm_corr[i - min_interval] = corr * norm;

    if (i != max_interval) {
      k--;
      for (j = LEN_SUBFR - 1; j > 0; j--) {
        filt_prev_exc[j] = filt_prev_exc[j - 1] + exc[k] * wsyn_filt_ir[j];
      }
      filt_prev_exc[0] = exc[k];
    }
  }
}

static FLOAT32 iusace_corr_interpolate(FLOAT32 *x, WORD32 fraction) {
  FLOAT32 interpol_value, *x1, *x2;
  const FLOAT32 *p1_interp4_1_table, *p2_interp4_1_table;
  if (fraction < 0) {
    fraction += 4;
    x--;
  }
  x1 = &x[0];
  x2 = &x[1];
  p1_interp4_1_table = &iusace_interp4_1[fraction];
  p2_interp4_1_table = &iusace_interp4_1[4 - fraction];
  interpol_value = x1[0] * p1_interp4_1_table[0] + x2[0] * p2_interp4_1_table[0];
  interpol_value += x1[-1] * p1_interp4_1_table[4] + x2[1] * p2_interp4_1_table[4];
  interpol_value += x1[-2] * p1_interp4_1_table[8] + x2[2] * p2_interp4_1_table[8];
  interpol_value += x1[-3] * p1_interp4_1_table[12] + x2[3] * p2_interp4_1_table[12];

  return interpol_value;
}

VOID iusace_open_loop_search(FLOAT32 *wsp, WORD32 min_pitch_lag, WORD32 max_pitch_lag,
                             WORD32 num_frame, WORD32 *ol_pitch_lag,
                             ia_usac_td_encoder_struct *st) {
  WORD32 i, j, k;
  FLOAT32 r, corr, energy1, energy2, corr_max = -1.0e23f;
  const FLOAT32 *p1_ol_cw_table, *p2_ol_cw_table;
  FLOAT32 *data_a, *data_b, *hp_wsp, *p, *p1;

  p1_ol_cw_table = &iusace_ol_corr_weight[453];
  p2_ol_cw_table = &iusace_ol_corr_weight[259 + max_pitch_lag - st->prev_pitch_med];
  *ol_pitch_lag = 0;
  for (i = max_pitch_lag; i > min_pitch_lag; i--) {
    p = &wsp[0];
    p1 = &wsp[-i];
    corr = 0.0;
    for (j = 0; j < num_frame; j += 2) {
      corr += p[j] * p1[j];
      corr += p[j + 1] * p1[j + 1];
    }
    corr *= *p1_ol_cw_table--;
    if ((st->prev_pitch_med > 0) && (st->ol_wght_flg == 1)) {
      corr *= *p2_ol_cw_table--;
    }
    if (corr >= corr_max) {
      corr_max = corr;
      *ol_pitch_lag = i;
    }
  }
  data_a = st->hp_ol_ltp_mem;
  data_b = st->hp_ol_ltp_mem + HP_ORDER;
  hp_wsp = st->prev_hp_wsp + max_pitch_lag;
  for (k = 0; k < num_frame; k++) {
    data_b[0] = data_b[1];
    data_b[1] = data_b[2];
    data_b[2] = data_b[3];
    data_b[HP_ORDER] = wsp[k];
    r = data_b[0] * 0.83787057505665F;
    r += data_b[1] * -2.50975570071058F;
    r += data_b[2] * 2.50975570071058F;
    r += data_b[3] * -0.83787057505665F;
    r -= data_a[0] * -2.64436711600664F;
    r -= data_a[1] * 2.35087386625360F;
    r -= data_a[2] * -0.70001156927424F;
    data_a[2] = data_a[1];
    data_a[1] = data_a[0];
    data_a[0] = r;
    hp_wsp[k] = r;
  }
  p = &hp_wsp[0];
  p1 = &hp_wsp[-(*ol_pitch_lag)];
  corr = 0.0F;
  energy1 = 0.0F;
  energy2 = 0.0F;
  for (j = 0; j < num_frame; j++) {
    energy1 += p1[j] * p1[j];
    energy2 += p[j] * p[j];
    corr += p[j] * p1[j];
  }
  st->ol_gain = (FLOAT32)(corr / (sqrt(energy1 * energy2) + 1e-5));
  memmove(st->prev_hp_wsp, &st->prev_hp_wsp[num_frame], max_pitch_lag * sizeof(FLOAT32));
}

WORD32 iusace_get_ol_lag_median(WORD32 prev_ol_lag, WORD32 *prev_ol_lags) {
  WORD32 sorted_ol_lags_out[NUM_OPEN_LOOP_LAGS + 1] = {0};
  WORD32 i, j, idx, val;
  WORD32 num_lags = NUM_OPEN_LOOP_LAGS;
  for (i = NUM_OPEN_LOOP_LAGS - 1; i > 0; i--) {
    prev_ol_lags[i] = prev_ol_lags[i - 1];
  }
  prev_ol_lags[0] = prev_ol_lag;
  for (i = 0; i < NUM_OPEN_LOOP_LAGS; i++) {
    sorted_ol_lags_out[i + 1] = prev_ol_lags[i];
  }

  idx = (NUM_OPEN_LOOP_LAGS >> 1) + 1;
  for (;;) {
    if (idx > 1) {
      val = sorted_ol_lags_out[--idx];
    } else {
      val = sorted_ol_lags_out[num_lags];
      sorted_ol_lags_out[num_lags] = sorted_ol_lags_out[1];
      if (--num_lags == 1) {
        sorted_ol_lags_out[1] = val;
        break;
      }
    }
    i = idx;
    j = idx << 1;
    while (j <= num_lags) {
      if (j < num_lags && sorted_ol_lags_out[j] < sorted_ol_lags_out[j + 1]) {
        ++j;
      }
      if (val < sorted_ol_lags_out[j]) {
        sorted_ol_lags_out[i] = sorted_ol_lags_out[j];
        i = j;
        j *= 2;
      } else {
        j = num_lags + 1;
      }
    }
    sorted_ol_lags_out[i] = val;
  }

  return sorted_ol_lags_out[OPEN_LOOP_LAG_MEDIAN];
}

VOID iusace_closed_loop_search(FLOAT32 *exc, FLOAT32 *xn, FLOAT32 *wsyn_filt_ir,
                               WORD32 search_range_min, WORD32 search_range_max, WORD32 *pit_frac,
                               WORD32 is_first_subfrm, WORD32 min_pitch_lag_res1_2,
                               WORD32 min_pitch_lag_res_1, WORD32 *pitch_lag_out) {
  WORD32 i, fraction, step;
  FLOAT32 corr_vector[15 + 2 * LEN_INTERPOL1 + 1] = {0};
  FLOAT32 corr_max, temp;
  FLOAT32 *p_norm_corr_vector;
  WORD32 min_interval, max_interval;
  min_interval = search_range_min - LEN_INTERPOL1;
  max_interval = search_range_max + LEN_INTERPOL1;
  p_norm_corr_vector = &corr_vector[0];
  iusace_get_norm_correlation(exc, xn, wsyn_filt_ir, min_interval, max_interval,
                              p_norm_corr_vector);

  corr_max = p_norm_corr_vector[LEN_INTERPOL1];
  *pitch_lag_out = search_range_min;
  for (i = search_range_min + 1; i <= search_range_max; i++) {
    if (p_norm_corr_vector[i - search_range_min + LEN_INTERPOL1] > corr_max) {
      corr_max = p_norm_corr_vector[i - search_range_min + LEN_INTERPOL1];
      *pitch_lag_out = i;
    }
  }
  if ((is_first_subfrm == 0) && (*pitch_lag_out >= min_pitch_lag_res_1)) {
    *pit_frac = 0;
  } else {
    step = 1;
    fraction = -3;
    if (((is_first_subfrm == 0) && (*pitch_lag_out >= min_pitch_lag_res1_2)) ||
        (min_pitch_lag_res1_2 == TMIN)) {
      step = 2;
      fraction = -2;
    }
    if (*pitch_lag_out == search_range_min) {
      fraction = 0;
    }
    corr_max = iusace_corr_interpolate(
        &p_norm_corr_vector[(*pitch_lag_out) - search_range_min + LEN_INTERPOL1], fraction);
    for (i = (fraction + step); i <= 3; i += step) {
      temp = iusace_corr_interpolate(
          &p_norm_corr_vector[(*pitch_lag_out) - search_range_min + LEN_INTERPOL1], i);
      if (temp > corr_max) {
        corr_max = temp;
        fraction = i;
      }
    }
    if (fraction < 0) {
      fraction += 4;
      (*pitch_lag_out) -= 1;
    }
    *pit_frac = fraction;
  }
}

VOID iusace_decim2_fir_filter(FLOAT32 *signal, WORD32 length, FLOAT32 *mem,
                              FLOAT32 *scratch_fir_sig_buf) {
  FLOAT32 *sig_buf = scratch_fir_sig_buf;
  FLOAT32 temp;
  WORD32 i, j;
  memcpy(sig_buf, mem, DECIM2_FIR_FILT_MEM_SIZE * sizeof(FLOAT32));
  memcpy(sig_buf + DECIM2_FIR_FILT_MEM_SIZE, signal, length * sizeof(FLOAT32));
  for (i = 0; i < DECIM2_FIR_FILT_MEM_SIZE; i++) {
    mem[i] = ((signal[length - DECIM2_FIR_FILT_MEM_SIZE + i] > 1e-10) ||
              (signal[length - DECIM2_FIR_FILT_MEM_SIZE + i] < -1e-10))
                 ? signal[length - DECIM2_FIR_FILT_MEM_SIZE + i]
                 : 0;
  }
  for (i = 0, j = 0; i < length; i += 2, j++) {
    temp = sig_buf[i] * 0.13F;
    temp += sig_buf[i + 1] * 0.23F;
    temp += sig_buf[i + 2] * 0.28F;
#ifdef _WIN32
#pragma warning(suppress : 6385)
#endif
    temp += sig_buf[i + 3] * 0.23F;
    temp += sig_buf[i + 4] * 0.13F;
    signal[j] = temp;
  }
}

FLOAT32 iusace_calc_sq_gain(FLOAT32 *x, WORD32 num_bits, WORD32 length,
                            FLOAT32 *scratch_sq_gain_en) {
  WORD32 i, j, k;
  FLOAT32 gain, ener, temp, target, factor, offset;
  FLOAT32 *en = scratch_sq_gain_en;

  for (i = 0; i < length; i += 4) {
    ener = 0.01f;
    for (j = i; j < i + 4; j++) {
      ener += x[j] * x[j];
    }

    temp = (FLOAT32)log10(ener);
    en[i / 4] = 9.0f + 10.0f * temp;
  }

  target = (6.0f / 4.0f) * (FLOAT32)(num_bits - (length / 16));

  factor = 128.0f;
  offset = factor;

  for (k = 0; k < 10; k++) {
    factor *= 0.5f;
    offset -= factor;
    ener = 0.0f;
    for (i = 0; i < length / 4; i++) {
      temp = en[i] - offset;

      if (temp > 3.0f) {
        ener += temp;
      }
    }
    if (ener > target) {
      offset += factor;
    }
  }

  gain = (FLOAT32)pow(10.0f, offset / 20.0f);

  return (gain);
}

VOID iusace_lpc_coef_gen(FLOAT32 *lsf_old, FLOAT32 *lsf_new, FLOAT32 *a, WORD32 nb_subfr,
                         WORD32 m) {
  FLOAT32 lsf[ORDER] = {0}, *ptr_a;
  FLOAT32 inc, fnew, fold;
  WORD32 i = 0;

  ptr_a = a;

  inc = 1.0f / (FLOAT32)nb_subfr;
  fnew = 0.5f - (0.5f * inc);
  fold = 1.0f - fnew;
  for (i = 0; i < m; i++) {
    lsf[i] = (lsf_old[i] * fold) + (lsf_new[i] * fnew);
  }
  iusace_lsp_to_lp_conversion(lsf, ptr_a);
  ptr_a += (m + 1);
  iusace_lsp_to_lp_conversion(lsf_old, ptr_a);
  ptr_a += (m + 1);
  iusace_lsp_to_lp_conversion(lsf_new, ptr_a);

  return;
}

VOID iusace_interpolation_lsp_params(FLOAT32 *lsp_old, FLOAT32 *lsp_new, FLOAT32 *lp_flt_coff_a,
                                     WORD32 nb_subfr) {
  FLOAT32 lsp[ORDER];
  FLOAT32 factor;
  WORD32 i, k;
  FLOAT32 x_plus_y, x_minus_y;

  factor = 1.0f / (FLOAT32)nb_subfr;

  x_plus_y = 0.5f * factor;

  for (k = 0; k < nb_subfr; k++) {
    x_minus_y = 1.0f - x_plus_y;
    for (i = 0; i < ORDER; i++) {
      lsp[i] = (lsp_old[i] * x_minus_y) + (lsp_new[i] * x_plus_y);
    }
    x_plus_y += factor;

    iusace_lsp_to_lp_conversion(lsp, lp_flt_coff_a);

    lp_flt_coff_a += (ORDER + 1);
  }

  iusace_lsp_to_lp_conversion(lsp_new, lp_flt_coff_a);

  return;
}