/******************************************************************************
 *                                                                            *
 * 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 "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 "ixheaace_adjust_threshold_data.h"
#include "iusace_fd_qc_util.h"
#include "iusace_config.h"
#include "iusace_arith_enc.h"
#include "iusace_block_switch_const.h"
#include "iusace_block_switch_struct_def.h"
#include "iusace_lpd_rom.h"
#include "iusace_lpd.h"

VOID iusace_acelp_encode(FLOAT32 *lp_filt_coeff, FLOAT32 *quant_lp_filt_coeff, FLOAT32 *speech_in,
                         FLOAT32 *wsig_in, FLOAT32 *synth_out, FLOAT32 *wsynth_out,
                         WORD16 acelp_core_mode, ia_usac_lpd_state_struct *lpd_state,
                         WORD32 len_subfrm, FLOAT32 norm_corr, FLOAT32 norm_corr2,
                         WORD32 ol_pitch_lag1, WORD32 ol_pitch_lag2, WORD32 pit_adj,
                         WORD32 *acelp_params, iusace_scratch_mem *pstr_scratch) {
  WORD32 i, i_subfr, num_bits, t;
  WORD32 t0, t0_min, t0_max, index, subfrm_flag;
  WORD32 t0_frac;
  FLOAT32 temp, energy, max_ener, mean_ener_code;
  FLOAT32 pitch_gain, code_gain, gain1, gain2;
  FLOAT32 tgt_cb_corr[5], tgt_cb_corr2[2];
  FLOAT32 *p_lp_filt_coeff, *p_quant_lp_filt_coeff, weighted_lpc[ORDER + 1];
  FLOAT32 imp_res[LEN_SUBFR];
  FLOAT32 code[LEN_SUBFR];
  WORD16 cb_exc[LEN_SUBFR];
  FLOAT32 error[ORDER + LEN_SUBFR + 8];
  FLOAT32 cn[LEN_SUBFR];
  FLOAT32 xn[LEN_SUBFR];
  FLOAT32 xn2[LEN_SUBFR];
  FLOAT32 dn[LEN_SUBFR];
  FLOAT32 y0[LEN_SUBFR];
  FLOAT32 y1[LEN_SUBFR];
  FLOAT32 y2[LEN_SUBFR];
  WORD32 min_pitch_lag_res1_4;
  WORD32 min_pitch_lag_res1_2;
  WORD32 min_pitch_lag_res1;
  WORD32 max_pitch_lag;
  FLOAT32 *exc_buf = pstr_scratch->p_acelp_exc_buf;
  FLOAT32 *exc;
  FLOAT32 mem_txn, mem_txnq;
  WORD32 fac_length = len_subfrm / 2;
  if (lpd_state->mode > 0) {
    for (i = 0; i < fac_length; i++) {
      acelp_params[i] = lpd_state->avq_params[i];
    }
    acelp_params += fac_length;
  }

  if (pit_adj == SR_MAX)
    exc = exc_buf + (2 * len_subfrm) + 41;
  else
    exc = exc_buf + (2 * len_subfrm);

  memset(exc_buf, 0, (2 * len_subfrm) * sizeof(exc_buf[0]));
  memcpy(exc_buf, lpd_state->acelp_exc, 2 * len_subfrm * sizeof(FLOAT32));
  memcpy(synth_out - 128, &(lpd_state->synth[ORDER]), 128 * sizeof(FLOAT32));
  memcpy(wsynth_out - 128, &(lpd_state->wsynth[1]), 128 * sizeof(FLOAT32));

  num_bits = ((iusace_acelp_core_numbits_1024[acelp_core_mode] - NBITS_MODE) / 4) - NBITS_LPC;

  if (pit_adj == 0) {
    min_pitch_lag_res1_4 = TMIN;
    min_pitch_lag_res1_2 = TFR2;
    min_pitch_lag_res1 = TFR1;
    max_pitch_lag = TMAX;
  } else {
    i = (((pit_adj * TMIN) + (FSCALE_DENOM / 2)) / FSCALE_DENOM) - TMIN;
    min_pitch_lag_res1_4 = TMIN + i;
    min_pitch_lag_res1_2 = TFR2 - i;
    min_pitch_lag_res1 = TFR1;
    max_pitch_lag = TMAX + (6 * i);
  }

  ol_pitch_lag1 *= OPL_DECIM;
  ol_pitch_lag2 *= OPL_DECIM;

  t0_min = ol_pitch_lag1 - 8;

  t = MIN(ol_pitch_lag1, ol_pitch_lag2) - 4;
  if (t0_min < t) t0_min = t;

  if (t0_min < min_pitch_lag_res1_4) {
    t0_min = min_pitch_lag_res1_4;
  }
  t0_max = t0_min + 15;
  t = MAX(ol_pitch_lag1, ol_pitch_lag2) + 4;
  if (t0_max > t) t0_max = t;

  if (t0_max > max_pitch_lag) {
    t0_max = max_pitch_lag;
    t0_min = t0_max - 15;
  }

  max_ener = 0.0;
  mean_ener_code = 0.0;
  p_quant_lp_filt_coeff = quant_lp_filt_coeff;
  for (i_subfr = 0; i_subfr < len_subfrm; i_subfr += LEN_SUBFR) {
    iusace_compute_lp_residual(p_quant_lp_filt_coeff, &speech_in[i_subfr], &exc[i_subfr],
                               LEN_SUBFR);
    energy = 0.01f;
    for (i = 0; i < LEN_SUBFR; i++) {
      energy += exc[i + i_subfr] * exc[i + i_subfr];
    }
    energy = 10.0f * (FLOAT32)log10(energy / ((FLOAT32)LEN_SUBFR));
    if (energy < 0.0) {
      energy = 0.0;
    }
    if (energy > max_ener) {
      max_ener = energy;
    }
    mean_ener_code += 0.25f * energy;
    p_quant_lp_filt_coeff += (ORDER + 1);
  }

  mean_ener_code -= 5.0f * norm_corr;
  mean_ener_code -= 5.0f * norm_corr2;

  temp = (mean_ener_code - 18.0f) / 12.0f;
  index = (WORD32)floor(temp + 0.5);
  if (index < 0) {
    index = 0;
  }
  if (index > 3) {
    index = 3;
  }
  mean_ener_code = (((FLOAT32)index) * 12.0f) + 18.0f;

  while ((mean_ener_code < (max_ener - 27.0)) && (index < 3)) {
    index++;
    mean_ener_code += 12.0;
  }
  *acelp_params = index;
  acelp_params++;

  p_lp_filt_coeff = lp_filt_coeff;
  p_quant_lp_filt_coeff = quant_lp_filt_coeff;
  for (i_subfr = 0; i_subfr < len_subfrm; i_subfr += LEN_SUBFR) {
    subfrm_flag = i_subfr;
    if ((len_subfrm == 256) && (i_subfr == (2 * LEN_SUBFR))) {
      subfrm_flag = 0;

      t0_min = ol_pitch_lag2 - 8;

      t = MIN(ol_pitch_lag1, ol_pitch_lag2) - 4;
      if (t0_min < t) t0_min = t;

      if (t0_min < min_pitch_lag_res1_4) {
        t0_min = min_pitch_lag_res1_4;
      }
      t0_max = t0_min + 15;

      t = MAX(ol_pitch_lag1, ol_pitch_lag2) + 4;
      if (t0_max > t) t0_max = t;

      if (t0_max > max_pitch_lag) {
        t0_max = max_pitch_lag;
        t0_min = t0_max - 15;
      }
    }

    memcpy(xn, &wsig_in[i_subfr], LEN_SUBFR * sizeof(FLOAT32));

    memcpy(error, &synth_out[i_subfr - ORDER], ORDER * sizeof(FLOAT32));
    memset(error + ORDER, 0, LEN_SUBFR * sizeof(FLOAT32));
    iusace_synthesis_tool_float(p_quant_lp_filt_coeff, error + ORDER, error + ORDER, LEN_SUBFR,
                                error, pstr_scratch->p_buf_synthesis_tool);
    iusace_get_weighted_lpc(p_lp_filt_coeff, weighted_lpc);
    iusace_compute_lp_residual(weighted_lpc, error + ORDER, xn2, LEN_SUBFR);

    temp = wsynth_out[i_subfr - 1];
    iusace_apply_deemph(xn2, TILT_FAC, LEN_SUBFR, &temp);
    memcpy(y0, xn2, LEN_SUBFR * sizeof(FLOAT32));

    for (i = 0; i < LEN_SUBFR; i++) {
      xn[i] -= xn2[i];
    }
    iusace_compute_lp_residual(p_quant_lp_filt_coeff, &speech_in[i_subfr], &exc[i_subfr],
                               LEN_SUBFR);

    memset(&code[0], 0, ORDER * sizeof(code[0]));
    memcpy(code + ORDER, xn, (LEN_SUBFR / 2) * sizeof(FLOAT32));
    temp = 0.0;
    iusace_apply_preemph(code + ORDER, TILT_FAC, LEN_SUBFR / 2, &temp);
    iusace_get_weighted_lpc(p_lp_filt_coeff, weighted_lpc);
    iusace_synthesis_tool_float(weighted_lpc, code + ORDER, code + ORDER, LEN_SUBFR / 2, code,
                                pstr_scratch->p_buf_synthesis_tool);
    iusace_compute_lp_residual(p_quant_lp_filt_coeff, code + ORDER, cn, LEN_SUBFR / 2);
    memcpy(cn + (LEN_SUBFR / 2), &exc[i_subfr + (LEN_SUBFR / 2)],
           (LEN_SUBFR / 2) * sizeof(FLOAT32));

    iusace_get_weighted_lpc(p_lp_filt_coeff, weighted_lpc);
    memset(imp_res, 0, LEN_SUBFR * sizeof(FLOAT32));
    memcpy(imp_res, weighted_lpc, (ORDER + 1) * sizeof(FLOAT32));
    iusace_synthesis_tool_float(p_quant_lp_filt_coeff, imp_res, imp_res, LEN_SUBFR,
                                &imp_res[ORDER + 1], pstr_scratch->p_buf_synthesis_tool);
    temp = 0.0;
    iusace_apply_deemph(imp_res, TILT_FAC, LEN_SUBFR, &temp);

    iusace_closed_loop_search(&exc[i_subfr], xn, imp_res, t0_min, t0_max, &t0_frac, subfrm_flag,
                              min_pitch_lag_res1_2, min_pitch_lag_res1, &t0);

    if (subfrm_flag == 0) {
      if (t0 < min_pitch_lag_res1_2) {
        index = t0 * 4 + t0_frac - (min_pitch_lag_res1_4 * 4);
      } else if (t0 < min_pitch_lag_res1) {
        index = t0 * 2 + (t0_frac >> 1) - (min_pitch_lag_res1_2 * 2) +
                ((min_pitch_lag_res1_2 - min_pitch_lag_res1_4) * 4);
      } else {
        index = t0 - min_pitch_lag_res1 + ((min_pitch_lag_res1_2 - min_pitch_lag_res1_4) * 4) +
                ((min_pitch_lag_res1 - min_pitch_lag_res1_2) * 2);
      }

      t0_min = t0 - 8;
      if (t0_min < min_pitch_lag_res1_4) {
        t0_min = min_pitch_lag_res1_4;
      }
      t0_max = t0_min + 15;
      if (t0_max > max_pitch_lag) {
        t0_max = max_pitch_lag;
        t0_min = t0_max - 15;
      }
    } else {
      i = t0 - t0_min;
      index = i * 4 + t0_frac;
    }
    *acelp_params = index;
    acelp_params++;

    iusace_acelp_ltpred_cb_exc(&exc[i_subfr], t0, t0_frac, LEN_SUBFR + 1);
    iusace_convolve(&exc[i_subfr], imp_res, y1);
    gain1 = iusace_acelp_tgt_cb_corr2(xn, y1, tgt_cb_corr);
    iusace_acelp_cb_target_update(xn, xn2, y1, gain1);
    energy = 0.0;
    for (i = 0; i < LEN_SUBFR; i++) {
      energy += xn2[i] * xn2[i];
    }

    for (i = 0; i < LEN_SUBFR; i++) {
      code[i] = (FLOAT32)(0.18 * exc[i - 1 + i_subfr] + 0.64 * exc[i + i_subfr] +
                          0.18 * exc[i + 1 + i_subfr]);
    }
    iusace_convolve(code, imp_res, y2);
    gain2 = iusace_acelp_tgt_cb_corr2(xn, y2, tgt_cb_corr2);

    iusace_acelp_cb_target_update(xn, xn2, y2, gain2);
    temp = 0.0;
    for (i = 0; i < LEN_SUBFR; i++) {
      temp += xn2[i] * xn2[i];
    }

    if (temp < energy) {
      *acelp_params = 0;
      memcpy(&exc[i_subfr], code, LEN_SUBFR * sizeof(FLOAT32));
      memcpy(y1, y2, LEN_SUBFR * sizeof(FLOAT32));
      pitch_gain = gain2;
      tgt_cb_corr[0] = tgt_cb_corr2[0];
      tgt_cb_corr[1] = tgt_cb_corr2[1];
    } else {
      *acelp_params = 1;
      pitch_gain = gain1;
    }
    acelp_params++;

    iusace_acelp_cb_target_update(xn, xn2, y1, pitch_gain);
    iusace_acelp_cb_target_update(cn, cn, &exc[i_subfr], pitch_gain);

    temp = 0.0;
    iusace_apply_preemph(imp_res, TILT_CODE, LEN_SUBFR, &temp);
    if (t0_frac > 2) {
      t0++;
    }

    for (i = t0; i < LEN_SUBFR; i++) {
      imp_res[i] += imp_res[i - t0] * PIT_SHARP;
    }

    iusace_acelp_tgt_ir_corr(xn2, imp_res, dn);

    if (acelp_core_mode == ACELP_CORE_MODE_9k6) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_20, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 4;
    } else if (acelp_core_mode == ACELP_CORE_MODE_11k2) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_28, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 4;
    } else if (acelp_core_mode == ACELP_CORE_MODE_12k8) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_36, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 4;
    } else if (acelp_core_mode == ACELP_CORE_MODE_14k4) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_44, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 4;
    } else if (acelp_core_mode == ACELP_CORE_MODE_16k) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_52, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 4;
    } else if (acelp_core_mode == ACELP_CORE_MODE_18k4) {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_64, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 8;
    } else {
      iusace_acelp_cb_exc(dn, cn, imp_res, cb_exc, y2, ACELP_NUM_BITS_64, acelp_params,
                          pstr_scratch->p_acelp_ir_buf);
      acelp_params += 8;
    }

    for (i = 0; i < LEN_SUBFR; i++) {
      code[i] = (FLOAT32)(cb_exc[i] / 512);
    }

    temp = 0.0;
    iusace_apply_preemph(code, TILT_CODE, LEN_SUBFR, &temp);
    for (i = t0; i < LEN_SUBFR; i++) {
      code[i] += code[i - t0] * PIT_SHARP;
    }

    iusace_acelp_tgt_cb_corr1(xn, y1, y2, tgt_cb_corr);
    iusace_acelp_quant_gain(code, &pitch_gain, &code_gain, tgt_cb_corr, mean_ener_code,
                            acelp_params);
    acelp_params++;

    temp = 0.0;
    for (i = 0; i < LEN_SUBFR; i++) {
      temp += code[i] * code[i];
    }
    temp *= code_gain * code_gain;

    for (i = 0; i < LEN_SUBFR; i++) {
      wsynth_out[i + i_subfr] = y0[i] + (pitch_gain * y1[i]) + (code_gain * y2[i]);
    }

    for (i = 0; i < LEN_SUBFR; i++) {
      exc[i + i_subfr] = pitch_gain * exc[i + i_subfr] + code_gain * code[i];
    }

    iusace_synthesis_tool_float(p_quant_lp_filt_coeff, &exc[i_subfr], &synth_out[i_subfr],
                                LEN_SUBFR, &synth_out[i_subfr - ORDER],
                                pstr_scratch->p_buf_synthesis_tool);
    p_lp_filt_coeff += (ORDER + 1);
    p_quant_lp_filt_coeff += (ORDER + 1);
  }

  memcpy(lpd_state->acelp_exc, exc - len_subfrm, 2 * len_subfrm * sizeof(FLOAT32));
  memcpy(lpd_state->synth, synth_out + len_subfrm - (ORDER + 128),
         (ORDER + 128) * sizeof(FLOAT32));
  memcpy(lpd_state->wsynth, wsynth_out + len_subfrm - (1 + 128), (1 + 128) * sizeof(FLOAT32));
  memcpy(lpd_state->lpc_coeffs_quant, p_quant_lp_filt_coeff - (2 * (ORDER + 1)),
         (2 * (ORDER + 1)) * sizeof(FLOAT32));
  memcpy(lpd_state->lpc_coeffs, p_lp_filt_coeff - (2 * (ORDER + 1)),
         (2 * (ORDER + 1)) * sizeof(FLOAT32));

  mem_txn = lpd_state->tcx_mem[128 - 1];
  mem_txnq = lpd_state->tcx_fac;

  p_quant_lp_filt_coeff = quant_lp_filt_coeff;
  for (i_subfr = 0; i_subfr < (len_subfrm - 2 * LEN_SUBFR); i_subfr += LEN_SUBFR) {
    iusace_get_weighted_lpc(p_quant_lp_filt_coeff, weighted_lpc);

    memcpy(error, &speech_in[i_subfr], LEN_SUBFR * sizeof(FLOAT32));
    iusace_apply_deemph(error, TILT_FAC, LEN_SUBFR, &mem_txn);

    memcpy(error, &synth_out[i_subfr], LEN_SUBFR * sizeof(FLOAT32));
    iusace_apply_deemph(error, TILT_FAC, LEN_SUBFR, &mem_txnq);

    p_quant_lp_filt_coeff += (ORDER + 1);
  }

  lpd_state->tcx_quant[0] = mem_txnq;
  for (i_subfr = 0; i_subfr < (2 * LEN_SUBFR); i_subfr += LEN_SUBFR) {
    iusace_get_weighted_lpc(p_quant_lp_filt_coeff, weighted_lpc);

    memcpy(&(lpd_state->tcx_mem[i_subfr]), &speech_in[i_subfr + (len_subfrm - 2 * LEN_SUBFR)],
           LEN_SUBFR * sizeof(FLOAT32));
    iusace_apply_deemph(&(lpd_state->tcx_mem[i_subfr]), TILT_FAC, LEN_SUBFR, &mem_txn);

    memcpy(&(lpd_state->tcx_quant[1 + i_subfr]),
           &synth_out[i_subfr + (len_subfrm - 2 * LEN_SUBFR)], LEN_SUBFR * sizeof(FLOAT32));
    iusace_apply_deemph(&(lpd_state->tcx_quant[1 + i_subfr]), TILT_FAC, LEN_SUBFR, &mem_txnq);
    p_quant_lp_filt_coeff += (ORDER + 1);
  }
  lpd_state->tcx_fac = mem_txnq;

  iusace_get_weighted_lpc(p_quant_lp_filt_coeff, weighted_lpc);

  memcpy(error, &synth_out[len_subfrm - ORDER], ORDER * sizeof(FLOAT32));
  for (i_subfr = (2 * LEN_SUBFR); i_subfr < (4 * LEN_SUBFR); i_subfr += LEN_SUBFR) {
    memset(error + ORDER, 0, LEN_SUBFR * sizeof(FLOAT32));

    iusace_synthesis_tool_float(p_quant_lp_filt_coeff, error + ORDER, error + ORDER, LEN_SUBFR,
                                error, pstr_scratch->p_buf_synthesis_tool);
    memcpy(&(lpd_state->tcx_quant[1 + i_subfr]), error + ORDER, LEN_SUBFR * sizeof(FLOAT32));
    iusace_apply_deemph(&(lpd_state->tcx_quant[1 + i_subfr]), TILT_FAC, LEN_SUBFR, &mem_txnq);
    memcpy(error, error + LEN_SUBFR, ORDER * sizeof(FLOAT32));
  }

  lpd_state->mode = 0;

  lpd_state->num_bits = num_bits;
}