/******************************************************************************
 *                                                                            *
 * 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 <math.h>
#include "ixheaac_type_def.h"
#include "iusace_bitbuffer.h"
#include "iusace_cnst.h"
#include "iusace_tns_usac.h"
#include "iusace_psy_mod.h"
#include "iusace_block_switch_const.h"
#include "iusace_rom.h"

static FLOAT32 iusace_lpc_eval_chebyshev_polyn(FLOAT32 x, FLOAT32 *coefs, WORD32 order) {
  WORD32 i;
  FLOAT32 b0, b1, b2, x2;
  x2 = 2.0f * x;
  b2 = 1.0f;
  b1 = x2 + coefs[1];
  for (i = 2; i < order; i++) {
    b0 = x2 * b1 - b2 + coefs[i];
    b2 = b1;
    b1 = b0;
  }
  return (x * b1 - b2 + 0.5f * coefs[order]);
}

VOID iusace_lpc_2_lsp_conversion(FLOAT32 *lpc, FLOAT32 *lsp, FLOAT32 *prev_lsp) {
  FLOAT32 sum_polyn[(ORDER_BY_2) + 1], diff_polyn[(ORDER_BY_2) + 1];
  FLOAT32 *p1_lpc, *p2_lpc, *p_sum_polyn, *p_diff_polyn;
  WORD32 i, j = 0, num_found_freeq = 0, is_first_polyn = 0;
  FLOAT32 x_low, y_low, x_high, y_high, x_mid, y_mid, x_lin_interp;

  p_sum_polyn = sum_polyn;
  p_diff_polyn = diff_polyn;
  *p_sum_polyn++ = 1.0f;
  *p_diff_polyn++ = 1.0f;
  sum_polyn[0] = 1.0f;
  diff_polyn[0] = 1.0f;
  p1_lpc = lpc + 1;
  p2_lpc = lpc + ORDER;
  for (i = 0; i <= ORDER_BY_2 - 1; i++) {
    *p_sum_polyn = *p1_lpc + *p2_lpc - *(p_sum_polyn - 1);
    p_sum_polyn++;
    *p_diff_polyn = *p1_lpc++ - *p2_lpc-- + *(p_diff_polyn - 1);
    p_diff_polyn++;
  }
  p_sum_polyn = sum_polyn;
  x_low = iusace_chebyshev_polyn_grid[0];
  y_low = iusace_lpc_eval_chebyshev_polyn(x_low, p_sum_polyn, ORDER_BY_2);

  while ((num_found_freeq < ORDER) && (j < CHEBYSHEV_NUM_POINTS)) {
    j++;
    x_high = x_low;
    y_high = y_low;
    x_low = iusace_chebyshev_polyn_grid[j];
    y_low = iusace_lpc_eval_chebyshev_polyn(x_low, p_sum_polyn, ORDER_BY_2);

    if (y_low * y_high <= 0.0) /* if sign change new root exists */
    {
      j--;
      for (i = 0; i < CHEBYSHEV_NUM_ITER; i++) {
        x_mid = 0.5f * (x_low + x_high);
        y_mid = iusace_lpc_eval_chebyshev_polyn(x_mid, p_sum_polyn, ORDER_BY_2);
        if (y_low * y_mid <= 0.0) {
          y_high = y_mid;
          x_high = x_mid;
        } else {
          y_low = y_mid;
          x_low = x_mid;
        }
      }

      /* linear interpolation for evaluating the root */
      x_lin_interp = x_low - y_low * (x_high - x_low) / (y_high - y_low);

      lsp[num_found_freeq] = x_lin_interp;
      num_found_freeq++;

      is_first_polyn = 1 - is_first_polyn;
      p_sum_polyn = is_first_polyn ? diff_polyn : sum_polyn;

      x_low = x_lin_interp;
      y_low = iusace_lpc_eval_chebyshev_polyn(x_low, p_sum_polyn, ORDER_BY_2);
    }
  }

  /* Check if ORDER roots found */
  /* if not use the LSPs from previous frame */
  if (num_found_freeq < ORDER) {
    for (i = 0; i < ORDER; i++) lsp[i] = prev_lsp[i];
  }
}

static VOID iusace_compute_coeff_poly_f(FLOAT32 *lsp, FLOAT32 *poly1, FLOAT32 *poly2) {
  FLOAT32 b1, b2;
  FLOAT32 *ptr_lsp;
  WORD32 i, j;

  ptr_lsp = lsp;
  poly1[0] = poly2[0] = 1.0f;

  for (i = 1; i <= ORDER_BY_2; i++) {
    b1 = -2.0f * (*ptr_lsp++);
    b2 = -2.0f * (*ptr_lsp++);
    poly1[i] = (b1 * poly1[i - 1]) + (2.0f * poly1[i - 2]);
    poly2[i] = (b2 * poly2[i - 1]) + (2.0f * poly2[i - 2]);
    for (j = i - 1; j > 0; j--) {
      poly1[j] += (b1 * poly1[j - 1]) + poly1[j - 2];
      poly2[j] += (b2 * poly2[j - 1]) + poly2[j - 2];
    }
  }
}

VOID iusace_lsp_to_lp_conversion(FLOAT32 *lsp, FLOAT32 *lp_flt_coff_a) {
  WORD32 i;
  FLOAT32 *ppoly_f1, *ppoly_f2;
  FLOAT32 *plp_flt_coff_a_bott, *plp_flt_coff_a_top;
  FLOAT32 poly1[ORDER_BY_2 + 2], poly2[ORDER_BY_2 + 2];

  poly1[0] = 0.0f;
  poly2[0] = 0.0f;

  iusace_compute_coeff_poly_f(lsp, &poly1[1], &poly2[1]);

  ppoly_f1 = poly1 + ORDER_BY_2 + 1;
  ppoly_f2 = poly2 + ORDER_BY_2 + 1;

  for (i = 0; i < ORDER_BY_2; i++) {
    ppoly_f1[0] += ppoly_f1[-1];
    ppoly_f2[0] -= ppoly_f2[-1];
    ppoly_f1--;
    ppoly_f2--;
  }

  plp_flt_coff_a_bott = lp_flt_coff_a;
  *plp_flt_coff_a_bott++ = 1.0f;
  plp_flt_coff_a_top = lp_flt_coff_a + ORDER;
  ppoly_f1 = poly1 + 2;
  ppoly_f2 = poly2 + 2;
  for (i = 0; i < ORDER_BY_2; i++) {
    *plp_flt_coff_a_bott++ = 0.5f * (*ppoly_f1 + *ppoly_f2);
    *plp_flt_coff_a_top-- = 0.5f * (*ppoly_f1++ - *ppoly_f2++);
  }
}

VOID iusace_levinson_durbin_algo(FLOAT32 *auto_corr_input, FLOAT32 *lpc) {
  WORD32 i, j;
  FLOAT32 lpc_val, sum, sigma;
  FLOAT32 reflection_coeffs[LEV_DUR_MAX_ORDER];

  lpc[0] = 1.0f;

  reflection_coeffs[0] = -auto_corr_input[1] / auto_corr_input[0];
  lpc[1] = reflection_coeffs[0];
  sigma = auto_corr_input[0] + auto_corr_input[1] * reflection_coeffs[0];

  for (i = 2; i <= ORDER; i++) {
    sum = 0.0f;
    for (j = 0; j < i; j++) sum += auto_corr_input[i - j] * lpc[j];
    reflection_coeffs[i - 1] = -sum / sigma;

    sigma = sigma * (1.0f - reflection_coeffs[i - 1] * reflection_coeffs[i - 1]);

    if (sigma <= 1.0E-09f) {
      for (j = i; j <= ORDER; j++) {
        reflection_coeffs[j - 1] = 0.0f;
        lpc[j] = 0.0f;
      }
      break;
    }

    for (j = 1; j <= (i / 2); j++) {
      lpc_val = lpc[j] + reflection_coeffs[i - 1] * lpc[i - j];
      lpc[i - j] += reflection_coeffs[i - 1] * lpc[j];
      lpc[j] = lpc_val;
    }

    lpc[i] = reflection_coeffs[i - 1];
  }
}

VOID iusace_get_weighted_lpc(FLOAT32 *lpc, FLOAT32 *weighted_lpc) {
  WORD32 i;
  for (i = 0; i <= ORDER; i++) {
    weighted_lpc[i] = iusace_gamma_table[i] * lpc[i];
  }
}

VOID iusace_lsp_2_lsf_conversion(FLOAT32 *lsp, FLOAT32 *lsf) {
  WORD32 i;
  for (i = 0; i < ORDER; i++) {
    lsf[i] = (FLOAT32)(acos(lsp[i]) * LSP_2_LSF_SCALE);
  }
}

VOID iusace_lsf_2_lsp_conversion(FLOAT32 *lsf, FLOAT32 *lsp) {
  WORD32 i;
  for (i = 0; i < ORDER; i++) lsp[i] = (FLOAT32)cos((FLOAT64)lsf[i] * (FLOAT64)PI_BY_6400);
}