/******************************************************************************
 *                                                                            *
 * 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 <stdlib.h>
#include "ixheaac_type_def.h"
#include "iusace_avq_enc.h"

static VOID iusace_gosset_compute_rank_and_sign(WORD32 *x, WORD32 *rank, WORD32 *sign_code) {
  WORD32 xs[8], a[8], q, d[8], w[8], A, B, idx, tmp, abs_i, abs_j;
  WORD32 i, j, k;
  for (i = 0; i < 8; i++) {
    xs[i] = x[i];
  }
  for (k = 0; k < 7; k++) {
    j = k;
    for (i = k + 1; i < 8; i++) {
      abs_j = abs(xs[j]);
      abs_i = abs(xs[i]);
      if (abs_i >= abs_j) {
        if (abs_i > xs[j]) {
          j = i;
        }
      }
    }
    if (j > k) {
      tmp = xs[k];
      xs[k] = xs[j];
      xs[j] = tmp;
    }
  }
  *sign_code = 0;
  for (i = 0; i < 8; i++) {
    if (xs[i] < 0) {
      *sign_code += iusace_pow2_table[i];
    }
  }
  a[0] = xs[0];
  q = 1;
  for (i = 1; i < 8; i++) {
    if (xs[i] != xs[i - 1]) {
      a[q] = xs[i];
      q++;
    }
  }
  for (i = 0; i < 8; i++) {
    for (j = 0; j < q; j++) {
      if (x[i] == a[j]) {
        d[i] = j;
        break;
      }
    }
  }
  *rank = 0;
  for (j = 0; j < q; j++) {
    w[j] = 0;
  }
  B = 1;
  for (i = 7; i >= 0; i--) {
    idx = d[i];
    w[idx]++;
    B *= w[idx];
    A = 0;
    for (j = 0; j < idx; j++) {
      A += w[j];
    }
    if (A > 0) {
      *rank += A * iusace_factorial_table[i] / B;
    }
  }
}

static VOID iusace_gosset_compute_base_idx(WORD32 *x, WORD32 ka, WORD32 *idx) {
  WORD32 rank, offset, code, i, ks;
  iusace_gosset_compute_rank_and_sign(x, &rank, &code);
  ks = -1;
  for (i = iusace_iso_code_index_table[ka]; i < LEN_SIGN_LEADER; i++) {
    if (code == iusace_iso_code_data_table[i]) {
      ks = i;
      break;
    }
  }
  if (ks == -1) {
    ks = 0;
  }
  offset = iusace_signed_leader_is[ks];
  *idx = offset + rank;
  return;
}

static WORD32 iusace_find_absolute_leader(WORD32 *y) {
  WORD32 i, nb, pos, ka;
  WORD64 s, C[8], id;
  for (i = 0; i < 8; i++) {
    C[i] = (WORD64)y[i] * y[i];
  }
  s = 0;
  for (i = 0; i < 8; i++) {
    s += C[i];
  }
  s >>= 3;
  ka = LEN_ABS_LEADER + 1;
  if (s == 0) {
    ka = LEN_ABS_LEADER;
  } else {
    if (s <= NB_SPHERE) {
      id = 0;
      for (i = 0; i < 8; i++) {
        id += C[i] * C[i];
      }
      id = id >> 3;
      nb = iusace_da_num_bits[s - 1];
      pos = iusace_da_pos[s - 1];
      for (i = 0; i < nb; i++) {
        if (id == (long)iusace_da_id[pos]) {
          ka = pos;
          break;
        }
        pos++;
      }
    }
  }
  return (ka);
}

static VOID iusace_nearest_neighbor_2d(FLOAT32 *x, WORD32 *y) {
  WORD32 i, j;
  WORD64 sum;
  FLOAT32 diff[8], em;
  sum = 0;
  for (i = 0; i < 8; i++) {
    if (x[i] < 0) {
      y[i] = -2 * (((WORD32)(1.0 - x[i])) >> 1);
    } else {
      y[i] = 2 * (((WORD32)(1.0 + x[i])) >> 1);
    }
    sum += y[i];
  }
  if (sum % 4) {
    FLOAT32 s;
    em = 0;
    j = 0;
    for (i = 0; i < 8; i++) {
      diff[i] = x[i] - y[i];
      s = (FLOAT32)fabs(diff[i]);
      if (em < s) {
        em = s;
        j = i;
      }
    }
    if (diff[j] < 0) {
      y[j] -= 2;
    } else {
      y[j] += 2;
    }
  }
  return;
}

VOID iusace_find_nearest_neighbor(FLOAT32 *bk, WORD32 *ck) {
  WORD32 i, y1[8], y2[8];
  FLOAT32 e1k, e2k, x1[8], tmp;

  iusace_nearest_neighbor_2d(bk, y1);

  for (i = 0; i < 8; i++) {
    x1[i] = bk[i] - 1.0f;
  }
  iusace_nearest_neighbor_2d(x1, y2);
  for (i = 0; i < 8; i++) {
    y2[i] += 1;
  }
  /* Compute e1k = (Bk - y1k)^2 and e2k = (Bk – y2k)^2 */
  e1k = e2k = 0.0;
  for (i = 0; i < 8; i++) {
    tmp = bk[i] - y1[i];
    e1k += tmp * tmp;
    tmp = bk[i] - y2[i];
    e2k += tmp * tmp;
  }

  /* Select best lattice point */
  if (e1k < e2k) {
    for (i = 0; i < 8; i++) {
      ck[i] = y1[i];
    }
  } else {
    for (i = 0; i < 8; i++) {
      ck[i] = y2[i];
    }
  }
  return;
}

static VOID iusace_vononoi_idx(WORD32 *kv, WORD32 m, WORD32 *y) {
  WORD32 i, v[8], tmp, sum, *ptr1, *ptr2;
  FLOAT32 z[8];
  for (i = 0; i < 8; i++) {
    y[i] = kv[7];
  }
  z[7] = (FLOAT32)y[7] / m;
  sum = 0;
  for (i = 6; i >= 1; i--) {
    tmp = kv[i] << 1;
    sum += tmp;
    y[i] += tmp;
    z[i] = (FLOAT32)y[i] / m;
  }
  y[0] += (4 * kv[0] + sum);
  z[0] = (FLOAT32)(y[0] - 2) / m;
  iusace_find_nearest_neighbor(z, v);
  ptr1 = y;
  ptr2 = v;
  for (i = 0; i < 8; i++) {
    *ptr1++ -= m * *ptr2++;
  }
}

static VOID iusace_compute_coord(WORD32 *y, WORD32 *k) {
  WORD32 i, tmp, sum;
  k[7] = y[7];
  tmp = y[7];
  sum = 5 * y[7];
  for (i = 6; i >= 1; i--) {
    k[i] = (y[i] - tmp) >> 1;
    sum -= y[i];
  }
  k[0] = (y[0] + sum) >> 2;
}

VOID iusace_apply_voronoi_ext(WORD32 *x, WORD32 *n, WORD32 *idx, WORD32 *k) {
  WORD32 ka, c[8];
  WORD32 i, r, m, v[8], c_tmp[8], k_mod[8], k_tmp[8], iter, ka_tmp, n_tmp, mask;
  FLOAT32 sphere;
  ka = iusace_find_absolute_leader(x);
  *n = iusace_da_nq[ka];
  if (*n <= 4) {
    for (i = 0; i < 8; i++) {
      c[i] = x[i];
    }
  } else {
    sphere = 0.0;
    for (i = 0; i < 8; i++) {
      sphere += (FLOAT32)x[i] * (FLOAT32)x[i];
    }
    sphere *= 0.125;
    r = 1;
    sphere *= 0.25;
    while (sphere > 11.0) {
      r++;
      sphere *= 0.25;
    }
    iusace_compute_coord(x, k_mod);
    m = 1 << r;
    mask = m - 1;
    for (iter = 0; iter < 2; iter++) {
      for (i = 0; i < 8; i++) {
        k_tmp[i] = k_mod[i] & mask;
      }
      iusace_vononoi_idx(k_tmp, m, v);
      for (i = 0; i < 8; i++) {
        c_tmp[i] = (x[i] - v[i]) / m;
      }
      ka_tmp = iusace_find_absolute_leader(c_tmp);
      n_tmp = iusace_da_nq[ka_tmp];
      if (n_tmp > 4) {
        r++;
        m = m << 1;
        mask = ((mask << 1) + 1);
      } else {
        if (n_tmp < 3) {
          n_tmp = 3;
        }
        ka = ka_tmp;
        *n = n_tmp + 2 * r;
        for (i = 0; i < 8; i++) {
          k[i] = k_tmp[i];
        }
        for (i = 0; i < 8; i++) {
          c[i] = c_tmp[i];
        }
        r--;
        m = m >> 1;
        mask = mask >> 1;
      }
    }
  }

  if (*n > 0) {
    iusace_gosset_compute_base_idx(c, ka, idx);
  }
  return;
}

VOID iusace_alg_vec_quant(FLOAT32 *ptr_input, WORD32 *ptr_out, WORD32 *ptr_lpc_idx) {
  WORD32 i, l, nq, pos, c[8], kv[8];
  FLOAT32 x1[8];
  WORD32 lpc_index;

  pos = 2;
  for (l = 0; l < 2; l++) {
    for (i = 0; i < 8; i++) {
      x1[i] = ptr_input[l * 8 + i];
      kv[i] = 0;
    }

    iusace_find_nearest_neighbor(x1, c);

    iusace_apply_voronoi_ext(c, &nq, &lpc_index, kv);

    for (i = 0; i < 8; i++) {
      ptr_out[l * 8 + i] = c[i];
    }

    ptr_lpc_idx[l] = nq;

    if (nq > 0) {
      ptr_lpc_idx[pos++] = lpc_index;
      for (i = 0; i < 8; i++) {
        ptr_lpc_idx[pos++] = kv[i];
      }
    }
  }

  return;
}