/*****************************************************************************/
/* This software module was originally developed by                          */
/*   Naoki Iwakami (NTT)                                                     */
/* and edited by                                                             */
/*   Naoki Iwakami (NTT) on 1997-07-17,                                      */
/* in the course of development of the                                       */
/* MPEG-2 NBC/MPEG-4 Audio standard ISO/IEC 13818-7, 14496-1,2 and 3.        */
/* This software module is an implementation of a part of one or more        */
/* MPEG-2 NBC/MPEG-4 Audio tools as specified by the MPEG-2 NBC/MPEG-4 Audio */
/* standard. ISO/IEC  gives users of the MPEG-2 NBC/MPEG-4 Audio standards   */
/* free license to this software module or modifications thereof for use in  */
/* hardware or software products claiming conformance to the MPEG-2 NBC/     */
/* MPEG-4 Audio  standards. Those intending to use this software module in   */
/* hardware or software products are advised that this use may infringe      */
/* existing patents. The original developer of this software module and      */
/* his/her company, the subsequent editors and their companies, and ISO/IEC  */
/* have no liability for use of this software module or modifications        */
/* thereof in an implementation. Copyright is not released for non           */
/* MPEG-2 NBC/MPEG-4 Audio conforming products. The original developer       */
/* retains full right to use the code for his/her  own purpose, assign or    */
/* donate the code to a third party and to inhibit third party from using    */
/* the code for non MPEG-2 NBC/MPEG-4 Audio conforming products.             */
/* This copyright notice must be included in all copies or derivative works. */
/* Copyright (c)1996.                                                        */
/*****************************************************************************/


#include "ntt_conf.h"
#include "ntt_encode.h"
#include "all.h"

void ntt_tf_quantize_spectrum(double spectrum[],          /* Input : spectrum*/
			      double lpc_spectrum[],      /* Input : LPC spectrum*/
			      double bark_env[],          /* Input : Bark-scale envelope*/
			      double pitch_sequence[],    /* Input : periodic peak components*/
			      double gain[],              /* Input : gain factor*/
			      double perceptual_weight[], /* Input : perceptual weight*/
			      ntt_INDEX *index)           /* In/Out : VQ code indices */
{
  /*--- Variables ---*/
  int    ismp, nfr, nsf, n_can, vq_bits;
  double add_signal[ntt_T_FR_MAX];
  double *sp_cv0, *sp_cv1;
  int    cb_len_max;

  /*--- Parameter settings ---*/
  switch (index->w_type){
  case ONLY_LONG_WINDOW:
  case LONG_SHORT_WINDOW:
  case SHORT_LONG_WINDOW:
  case LONG_MEDIUM_WINDOW:
  case MEDIUM_LONG_WINDOW:
    /* available bits */
    vq_bits = ntt_VQTOOL_BITS;
    /* codebooks */
    sp_cv0 = (double *)ntt_codev0;
    sp_cv1 = (double *)ntt_codev1;
    cb_len_max = ntt_CB_LEN_READ + ntt_CB_LEN_MGN;
    /* number of pre-selection candidates */
    n_can = ntt_N_CAN;
    /* frame size */
    nfr = ntt_N_FR;
    nsf = ntt_N_SUP;
    /* additional signal */
    for (ismp=0; ismp<ntt_N_FR*ntt_N_SUP; ismp++){
      add_signal[ismp] = pitch_sequence[ismp] / lpc_spectrum[ismp];
    }
    break;
    case ONLY_SHORT_WINDOW:
    /* available bits */
    vq_bits = ntt_VQTOOL_BITS_S;
    /* codebooks */
    sp_cv0 = (double *)ntt_codev0s; sp_cv1 = (double *)ntt_codev1s;
    cb_len_max = ntt_CB_LEN_READ_S + ntt_CB_LEN_MGN;
    /* number of pre-selection candidates */
    n_can = ntt_N_CAN_S;
    /* number of subframes in a frame */
    nfr = ntt_N_FR_S;
    nsf = ntt_N_SUP * ntt_N_SHRT;
    /* additional signal */
    ntt_zerod(ntt_N_FR*ntt_N_SUP, add_signal);
    break;
  default:
    fprintf(stderr, "ntt_sencode(): %d: Window mode error.\n", index->w_type);
    exit(1);
  }

  /*--- Vector quantization process ---*/
  ntt_vq_pn(nfr, nsf, vq_bits, n_can,
	    sp_cv0, sp_cv1, cb_len_max,
	    spectrum, lpc_spectrum, bark_env, add_signal, gain,
	    perceptual_weight,
	    index->wvq);
}

void ntt_div_vec(int nfr,                         /* Param. : block length */
		 int nsf,                         /* Param. : number of sub frames */
		 int cb_len,                      /* Param. : codebook length */
		 int cb_len0,                     /* Param.*/
		 int idiv,                        /* Param. */
		 int ndiv,                        /* Param. : number of interleave division */
		 double target[],                 /* Input */
		 double d_target[],               /* Output */
		 double weight[],                 /* Input */
		 double d_weight[],               /* Output */
		 double add_signal[],             /* Input */
		 double d_add_signal[],           /* Output */
		 double perceptual_weight[],      /* Input */
		 double d_perceptual_weight[])    /* Output */
{
  /*--- Variables ---*/
  int  icv, ismp, itmp;

  /*--- Parameter settings ---*/

  for (icv=0; icv<cb_len; icv++) {
    /*--- Set interleave ---*/
    if ((icv<cb_len0-1) &&
	((ndiv%nsf==0 && nsf>1) || ((ndiv&0x1)==0 && (nsf&0x1)==0)))
      itmp = ((idiv + icv) % ndiv) + icv * ndiv;
    else itmp = idiv + icv * ndiv;
    ismp = itmp / nsf + ((itmp % nsf) * nfr);

    /*--- Vector division ---*/
    d_target[icv] = target[ismp];
    d_weight[icv] = weight[ismp];
    d_add_signal[icv] = add_signal[ismp];
    d_perceptual_weight[icv] = perceptual_weight[ismp];
  }
}

void ntt_vq_main_select(/* Parameters */
			int    cb_len,       /* code book length */
			int    cb_len_max,   /* maximum code vector length */
			double *codev0,      /* code book0 */
			double *codev1,      /* code book1 */
			int    n_can0,
			int    n_can1,
			int    can_ind0[],
			int    can_ind1[],
			int    can_sign0[],
			int    can_sign1[],
                        /* Input */
			double d_target[],
			double d_weight[],
			double d_add_signal[],
			double d_perceptual_weight[],
			/* Output */
			int    *index_wvq0,
			int    *index_wvq1)
{
  /*--- Variables ---*/
  int    ismp, i_can, j_can;
  int    index0, index1, i_can0, i_can1;
  double dist_min, dist;
  double *codev0_p, *codev1_p;
  double pw2, sign0, sign1, reconst;

  /*--- Best codevector search ---*/
  dist_min = 1.e100;
  for (i_can=0; i_can<n_can0; i_can++){
    codev0_p = &(codev0[can_ind0[i_can]*cb_len_max]);
    sign0 = 1. - (2. * can_sign0[i_can]);
    for (j_can=0; j_can<n_can1; j_can++){
      codev1_p = &(codev1[can_ind1[j_can]*cb_len_max]);
      sign1 = 1. - (2. * can_sign1[j_can]);
      dist = 0.;
      for (ismp=0; ismp<cb_len; ismp++){
	pw2 = d_perceptual_weight[ismp] * d_perceptual_weight[ismp];

	reconst = d_add_signal[ismp]
	  + 0.5 * d_weight[ismp] *
	    (sign0 * codev0_p[ismp] + sign1 * codev1_p[ismp]);

	dist += pw2 * (d_target[ismp] - reconst) * (d_target[ismp] - reconst);
      }
      if (dist < dist_min){
	dist_min = dist;
	i_can0 = i_can;
	i_can1 = j_can;
	index0 = can_ind0[i_can0];
	index1 = can_ind1[i_can1];
      }
    }
  }

  /*--- Make output indexes ---*/
  *index_wvq0 = index0;
  *index_wvq1 = index1;
  if(can_sign0[i_can0] == 1) 
    *index_wvq0 += ((0x1)<<(ntt_MAXBIT_SHAPE));
  if(can_sign1[i_can1] == 1) 
    *index_wvq1 +=  ((0x1)<<(ntt_MAXBIT_SHAPE));
}

void ntt_vq_pn(/* Parameters */
	       int    nfr,            /* block length */
	       int    nsf,            /* number of sub frames */
	       int    available_bits, /* available bits for VQ */
	       int    n_can,          /* number of pre-selection candidates */
	       double *codev0,        /* code book0 */
	       double *codev1,        /* code book1 */
	       int    cb_len_max,     /* maximum code vector length */
               /* Input */
	       double target[],
	       double lpc_spectrum[], /* LPC spectrum */
	       double bark_env[],     /* Bark-scale envelope */
	       double add_signal[],   /* added signal */
	       double gain[],
	       double perceptual_weight[],
	       /* Output */
	       int    index_wvq[])
{
  /*--- Variables ---*/
  int    ismp, isf, idiv, ndiv, top, block_size;
  int    bits, pol_bits0, pol_bits1, cb_len, cb_len0;
  int    can_ind0[ntt_N_CAN_MAX], can_ind1[ntt_N_CAN_MAX];
  int    can_sign0[ntt_N_CAN_MAX], can_sign1[ntt_N_CAN_MAX];
  double weight[ntt_T_FR_MAX], d_weight[ntt_CB_LEN_MAX];
  double d_add_signal[ntt_CB_LEN_MAX];
  double d_target[ntt_CB_LEN_MAX];
  double d_perceptual_weight[ntt_CB_LEN_MAX];
  
  /*--- Parameter settings ---*/
  block_size = nfr * nsf;
  ndiv    = (int)((available_bits+ntt_MAXBIT*2-1)/(ntt_MAXBIT*2));
  cb_len0 =  (int)((block_size + ndiv - 1) / ndiv);
  
  /*--- Make weighting factor ---*/
  for (isf=0; isf<nsf; isf++){
    top = isf * nfr;
    for (ismp=0; ismp<nfr; ismp++){
      weight[ismp+top]
	= gain[isf] * bark_env[ismp+top] / lpc_spectrum[ismp+top];
    }
  }
  
  /*--- Sub-vector division loop ---*/
  for (idiv=0; idiv<ndiv; idiv++){
    /*--- set codebook lengths and sizes ---*/
    cb_len = (int)((block_size + ndiv - 1 - idiv) / ndiv);
    bits = (available_bits+ndiv-1-idiv)/ndiv;
    pol_bits0 = ((bits+1)/2)-ntt_MAXBIT_SHAPE;
    pol_bits1 = (bits/2)-ntt_MAXBIT_SHAPE;
    
    /*--- vector division ---*/
    ntt_div_vec(nfr, nsf, cb_len, cb_len0, idiv, ndiv,
		target, d_target,
		weight, d_weight,
		add_signal, d_add_signal,
		perceptual_weight, d_perceptual_weight);
    
    /*--- pre selection ---*/
    ntt_vq_pre_select(cb_len, cb_len_max, pol_bits0, codev0,
		      d_target, d_weight, d_add_signal, d_perceptual_weight,
		      n_can, can_ind0, can_sign0);
    ntt_vq_pre_select(cb_len, cb_len_max, pol_bits1, codev1,
		      d_target, d_weight, d_add_signal, d_perceptual_weight,
		      n_can, can_ind1, can_sign1);
    
    /*--- main selection ---*/
    ntt_vq_main_select(cb_len, cb_len_max,
		       codev0, codev1,
		       n_can, n_can, can_ind0, can_ind1,
		       can_sign0, can_sign1,
		       d_target, d_weight, d_add_signal, d_perceptual_weight,
		       &index_wvq[idiv], &index_wvq[idiv+ndiv]);
    
  
  }

}

#define OFFSET_GAIN 1.3

void ntt_vq_pre_select(/* Parameters */
		       int    cb_len,         /* code book length */
		       int    cb_len_max,     /* maximum code vector length */
		       int    pol_bits,
		       double *codev,         /* code book */
                       /* Input */
		       double d_target[],
		       double d_weight[],
		       double d_add_signal[],
		       double d_perceptual_weight[],
		       int    n_can,
		       /* Output */
		       int    can_ind[],
		       int    can_sign[])
{
  /*--- Variables ---*/
  double xxp, xyp, xxn, xyn;
  double recp, recn;
  double pw2;
  double *p_code;
  int    icb, ismp;
  double max_t[ntt_N_CAN_MAX];
  double code_targ_tmp_p, code_targ_tmp_n, dtmp;
  double code_targ[ntt_CB_SIZE];
  int    code_sign[ntt_CB_SIZE];
  int    i_can, j_can, tb_top, spl_pnt;

  /*--- Distance calculation ---*/
  for (icb=0; icb<ntt_CB_SIZE; icb++){
    xxp=xyp=xxn=xyn = 0.0;
    code_targ_tmp_p = code_targ_tmp_n = 0.;
    p_code = &(codev[icb*cb_len_max]);
    for (ismp=0; ismp<cb_len; ismp++){
      pw2 = d_perceptual_weight[ismp] * d_perceptual_weight[ismp];

      recp =(d_add_signal[ismp] + p_code[ismp] * d_weight[ismp]) * OFFSET_GAIN;
      xxp = recp * recp;
      xyp = recp * d_target[ismp];
      code_targ_tmp_p += pw2 * (4.0 * xyp - xxp);

      recn =(d_add_signal[ismp] - p_code[ismp] * d_weight[ismp]) * OFFSET_GAIN;
      xxn = recn * recn;
      xyn = recn * d_target[ismp];
      code_targ_tmp_n += pw2 * (4.0 * xyn - xxn);
    }
    if ((pol_bits==1) && (code_targ_tmp_n>code_targ_tmp_p)){
      code_targ[icb] = code_targ_tmp_n;
      code_sign[icb] = 1;
    }
    else{
      code_targ[icb] = code_targ_tmp_p;
      code_sign[icb] = 0;
    }
  }

  /*--- Pre-selection search ---*/
  if (n_can < ntt_CB_SIZE){
    max_t[0] = -1.e30;
    can_ind[0] = 0;
    tb_top = 0;
    for (icb=0; icb<ntt_CB_SIZE; icb++){
      dtmp = code_targ[icb];
      if (dtmp>max_t[tb_top]){
	i_can = tb_top; j_can = 0;
	while(i_can>j_can){
	  spl_pnt = (i_can-j_can)/2 + j_can;
	  if (max_t[spl_pnt]>dtmp){
	    j_can = spl_pnt + 1;
	  }
	  else{
	    i_can = spl_pnt;
	  }
	}
	tb_top = ntt_min(tb_top+1, n_can-1);
	for (j_can=tb_top; j_can>i_can; j_can--){
	  max_t[j_can] = max_t[j_can-1];
	  can_ind[j_can] = can_ind[j_can-1];
	}
	max_t[i_can] = dtmp;
	can_ind[i_can] = icb;
      }
    }

    /*--- Make output ---*/
    for (i_can=0; i_can<n_can; i_can++){
      can_sign[i_can] = code_sign[can_ind[i_can]];
    }
  }
  else{
    for (i_can=0; i_can<n_can; i_can++){
      can_ind[i_can] = i_can;
      can_sign[i_can] = code_sign[i_can];
    }
  }
}
