/* ================================================================== */
/*                                                                    */ 
/*    Microsoft Speech coder     ANSI-C Source Code                   */
/*    SC1200 1200 bps speech coder                                    */
/*    Fixed Point Implementation      Version 7.0                     */
/*    Copyright (C) 2000, Microsoft Corp.                             */
/*    All rights reserved.                                            */
/*                                                                    */ 
/* ================================================================== */

/*

2.4 kbps MELP Proposed Federal Standard speech coder

Fixed-point C code, version 1.0

Copyright (c) 1998, Texas Instruments, Inc.

Texas Instruments has intellectual property rights on the MELP
algorithm.	The Texas Instruments contact for licensing issues for
commercial and non-government use is William Gordon, Director,
Government Contracts, Texas Instruments Incorporated, Semiconductor
Group (phone 972 480 7442).

The fixed-point version of the voice codec Mixed Excitation Linear
Prediction (MELP) is based on specifications on the C-language software
simulation contained in GSM 06.06 which is protected by copyright and
is the property of the European Telecommunications Standards Institute
(ETSI). This standard is available from the ETSI publication office
tel. +33 (0)4 92 94 42 58. ETSI has granted a license to United States
Department of Defense to use the C-language software simulation contained
in GSM 06.06 for the purposes of the development of a fixed-point
version of the voice codec Mixed Excitation Linear Prediction (MELP).
Requests for authorization to make other use of the GSM 06.06 or
otherwise distribute or modify them need to be addressed to the ETSI
Secretariat fax: +33 493 65 47 16.

*/

/*
	Name: melp_chn_write, melp_chn_read
	Description: Write/read MELP channel bitstream
	Inputs:
	  MELP parameter structure
	Outputs:
	  updated MELP parameter structure (channel pointers)
	Returns: void
*/

#include "sc1200.h"
#include "vq_lib.h"
#include "melp_sub.h"
#include "math_lib.h"
#include "constant.h"
#include "global.h"
#include "qnt12_cb.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "mat_lib.h"
#include "qnt12.h"
#include "msvq_cb.h"
#include "fsvq_cb.h"
#include "dsp_sub.h"
#include "fec_code.h"
#include "macro.h"

#define X0333_Q15		10923                        /* (1.0/3.0) * (1 << 15) */
#define X0667_Q15		21845                        /* (2.0/3.0) * (1 << 15) */
#define UV_PIND			0                             /* Unvoiced pitch index */
#define INVAL_PIND		1                             /* Invalid pitch index  */
#define SMOOTH			16383                                          /* Q15 */

#define ORIGINAL_BIT_ORDER	0    /* flag to use bit order of original version */
#if (ORIGINAL_BIT_ORDER)                             /* Original linear order */
static Shortword	bit_order[NUM_CH_BITS] = {
	 0,	 1,	 2,	 3,	 4,	 5,
	 6,	 7,	 8,	 9,	10, 11,
	12, 13, 14, 15, 16, 17,
	18, 19, 20, 21, 22, 23,
	24, 25, 26, 27, 28, 29,
	30, 31, 32, 33, 34, 35,
	36, 37, 38, 39, 40, 41,
	42, 43, 44, 45, 46, 47,
	48, 49, 50, 51, 52, 53
};
#else                                      /* Order based on priority of bits */
static Shortword	bit_order[NUM_CH_BITS] = {
	 0,	17,  9,	28, 34,  3,
	 4,	39,  1,	 2,	13, 38,
	14, 10, 11, 40, 15, 21,
	27, 45, 12, 26, 25, 33,
	20, 24, 23, 32, 44, 46,
	22, 31, 53, 52, 51,  7,
	 6,	19, 18, 29, 37, 30,
	36, 35, 43, 42, 16, 41,
	50, 49, 48, 47,  8,	 5
};
#endif

/* Define bit buffer */
static unsigned char	bit_buffer[NUM_CH_BITS];

static Shortword	sync_bit = 0;                                 /* sync bit */

Shortword parity(Shortword x, Shortword leng);

void melp_chn_write(struct quant_param *qpar, unsigned char chbuf[])
{
	register Shortword	i;
	unsigned char	*bit_ptr;
	Shortword	bit_cntr;


	/* FEC: code additional information in redundant indeces */
	fec_code(qpar);

	/* Fill bit buffer */
	bit_ptr = bit_buffer;
	bit_cntr = 0;

	pack_code(qpar->gain_index[1], &bit_ptr, &bit_cntr, 5, 1);

	/* Toggle and write sync bit */
	sync_bit = sub(1, sync_bit);
	pack_code(sync_bit, &bit_ptr, &bit_cntr, 1, 1);

	pack_code(qpar->gain_index[0], &bit_ptr, &bit_cntr, 3, 1);
	pack_code(qpar->pitch_index, &bit_ptr, &bit_cntr, PIT_BITS, 1);
	pack_code(qpar->jit_index[0], &bit_ptr, &bit_cntr, 1, 1);
	pack_code(qpar->bpvc_index[0], &bit_ptr, &bit_cntr, NUM_BANDS - 1, 1);

	for (i = 0; i < MSVQ_STAGES; i++)
		pack_code(qpar->msvq_index[i], &bit_ptr, &bit_cntr, msvq_bits[i], 1);

	pack_code(qpar->fsvq_index, &bit_ptr, &bit_cntr, FS_BITS, 1);

	/* Write channel output buffer */
	qpar->chptr = chbuf;
	qpar->chbit = 0;
	for (i = 0; i < bitNum24; i++){
		pack_code(bit_buffer[bit_order[i]], &qpar->chptr, &qpar->chbit, 1,
                  chwordsize);
		if (i == 0)                             /* set beginning of frame bit */
			*(qpar->chptr) |= (UShortword) 0x8000;
	}
}


BOOLEAN melp_chn_read(struct quant_param *qpar, struct melp_param *par,
					  struct melp_param *prev_par, unsigned char chbuf[])
{
	register Shortword	i;
	unsigned char	*bit_ptr;
	BOOLEAN		erase = FALSE;
	Shortword	index, bit_cntr, dontcare;


	/* Read channel output buffer into bit buffer */
	bit_ptr = bit_buffer;
	qpar->chptr = chbuf;
	qpar->chbit = 0;
	for (i = 0; i < bitNum24; i++){
		erase |= unpack_code(&(qpar->chptr), &(qpar->chbit), &index, 1,
							 chwordsize, ERASE_MASK);
		bit_buffer[bit_order[i]] = (unsigned char) index;
		bit_ptr ++;
	}

	/* Read information from  bit buffer */
	bit_ptr = bit_buffer;
	bit_cntr = 0;

	(void) unpack_code(&bit_ptr, &bit_cntr, &qpar->gain_index[1], 5, 1, 0);

	/* Read sync bit */
	(void) unpack_code(&bit_ptr, &bit_cntr, &dontcare, 1, 1, 0);
	(void) unpack_code(&bit_ptr, &bit_cntr, &qpar->gain_index[0], 3, 1, 0);
	(void) unpack_code(&bit_ptr, &bit_cntr, &(qpar->pitch_index), PIT_BITS,
					   1, 0);

	(void) unpack_code(&bit_ptr, &bit_cntr, &qpar->jit_index[0], 1, 1, 0);
	(void) unpack_code(&bit_ptr, &bit_cntr, &qpar->bpvc_index[0],
					   NUM_BANDS - 1, 1, 0);

	for (i = 0; i < MSVQ_STAGES; i++)
		(void) unpack_code(&bit_ptr, &bit_cntr, &(qpar->msvq_index[i]),
						   msvq_bits[i], 1, 0);

	(void) unpack_code(&bit_ptr, &bit_cntr, &qpar->fsvq_index, FS_BITS, 1, 0);

	/* Clear unvoiced flag */
	qpar->uv_flag[0] = FALSE;

	erase = (BOOLEAN) fec_decode(qpar, erase);

	/* Decode new frame if no erasures occurred */
	if (erase){                                      /* Erasure: frame repeat */
		*par = *prev_par;

		/* Force all subframes to equal last one */
		fill(par->gain, par->gain[NUM_GAINFR - 1], NUM_GAINFR - 1);

	} else {

		/* Decode line spectrum frequencies */
		vq_msd2(msvq_cb, par->lsf, msvq_cb_mean, qpar->msvq_index,
				msvq_levels, MSVQ_STAGES, LPC_ORD, 2);
		dontcare = FS_LEVELS;
		if (qpar->uv_flag[0])
			fill(par->fs_mag, ONE_Q13, NUM_HARM);
		else                                     /* Decode Fourier magnitudes */
			vq_msd2(fsvq_cb, par->fs_mag, (Shortword *) NULL,
					&(qpar->fsvq_index), &dontcare, 1, NUM_HARM, 0);

		/* Decode gain terms with uniform log quantizer */
		q_gain_dec(par->gain, qpar->gain_index, GN_QLO_Q8, GN_QUP_Q8,
				   GN_QLEV_M1_Q10, 5);

		/* Decode voicing information */
		par->uv_flag = qpar->uv_flag[0];

		/* Fractional pitch: Decode logarithmic pitch period */
		if (qpar->uv_flag[0])
			par->pitch = UV_PITCH_Q7;
		else {
			quant_u_dec(qpar->pitch_index, &par->pitch, PIT_QLO_Q12,
						PIT_QUP_Q12, PIT_QLEV_M1_Q8, 7);
			par->pitch = pow10_fxp(par->pitch, 7);
		}

		/* Decode jitter */
		/*	quant_u_dec(par->jit_index, &par->jitter, 0.0, MAX_JITTER, 2);    */

		if (qpar->jit_index[0] == 0)
			par->jitter = 0;
		else
			par->jitter = MAX_JITTER_Q15;

		/* Decode bandpass voicing */
		q_bpvc_dec(par->bpvc, qpar->bpvc_index[0], qpar->uv_flag[0],
				   NUM_BANDS);
	}

	/* Return erase flag */
	return(erase);
}


/****************************************************************************
**
** Function:		low_rate_chn_write
**
** Description: Write channel buffer for low rate ( 1200bps )
**
** Arguments:
**
**	quant_param par ---- The quantization structure
**
** Return value:	None
**
*****************************************************************************/
void low_rate_chn_write(struct quant_param *qpar)
{
	register Shortword	i;
	Shortword	bit_cntr, cnt;
	Shortword	uv1, uv2, cuv;
	Shortword	uv_index = 0, bp_prot1 = 0, bp_prot2 = 0, lsp_prot = 0;
	Shortword	uv_parity;
	unsigned char	*bit_ptr;


	/* FEC: code additional information in redundant indices */
	low_rate_fec_code(qpar);

	/* ====== Fill bit buffer ====== */
    bit_ptr = bit_buffer;
	bit_cntr = 0;

	/* ====== Toggle and write sync bit ====== */
	sync_bit = sub(1, sync_bit);
	pack_code(sync_bit, &bit_ptr, &bit_cntr, 1, 1);

	/* ===== Count the number of voiced frame ===== */
	cnt = 0;
	for (i = 0; i < NF; i++){
		if (!qpar->uv_flag[i])
			cnt++;
	}

	/* ====== Packing global voicing and pitch information ====== */
	if (cnt <= 1){                                         /* UUU UUV UVU VUU */
		uv_index = 0;
		bp_prot1 = 0;
		if (!qpar->uv_flag[0])
			bp_prot2 = 3;
		else if (!qpar->uv_flag[1])
			bp_prot2 = 2;
		else if (!qpar->uv_flag[2])
			bp_prot2 = 1;
		else {
			bp_prot2 = 0;
			lsp_prot = 0;
		}

		if (bp_prot2 == 0)
			qpar->pitch_index = UV_PIND;
		else
		    qpar->pitch_index =
				low_rate_pitch_enc[bp_prot2][qpar->pitch_index];
	} else if (cnt == 2){                                      /* UVV VUV VVU */
		if (qpar->uv_flag[0]){
			uv_index = 4;
			bp_prot1 = 3;
		} else if (qpar->uv_flag[1]){
			uv_index = 2;
			bp_prot1 = 2;
		} else if (qpar->uv_flag[2]){
			uv_index = 1;
			bp_prot1 = 1;
			lsp_prot = 7;
		}
	} else if (cnt == 3){                                              /* VVV */
		uv_index = (Shortword) (qpar->pitch_index / PITCH_VQ_SIZE);
		qpar->pitch_index = sub(qpar->pitch_index,
								(Shortword) (uv_index * PITCH_VQ_SIZE));
		uv_index = vvv_index_map[uv_index];
	}

	pack_code(uv_index, &bit_ptr, &bit_cntr, UV_BITS, 1);

	uv_parity = parity(uv_index, 3);
	
	pack_code(uv_parity, &bit_ptr, &bit_cntr, 1, 1);

	pack_code(qpar->pitch_index, &bit_ptr, &bit_cntr, PITCH_VQ_BITS, 1);

	/* ====== Packing LSF indices ====== */
	uv1 = qpar->uv_flag[0];
	uv2 = qpar->uv_flag[1];
	cuv = qpar->uv_flag[2];

	if ((uv1 == 1) && (uv2 == 1) && (cuv == 1)){
		pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[1][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[0][1], &bit_ptr, &bit_cntr, 4, 1);
		pack_code(qpar->lsf_index[1][1], &bit_ptr, &bit_cntr, 4, 1);
		pack_code(qpar->lsf_index[2][1], &bit_ptr, &bit_cntr, 4, 1);
		pack_code(lsp_prot,				 &bit_ptr, &bit_cntr, 3, 1);
	} else if ((uv1 == 1) && (uv2 == 1) && (cuv != 1)){
		pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[1][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 8, 1);
		pack_code(qpar->lsf_index[2][1], &bit_ptr, &bit_cntr, 6, 1);
		pack_code(qpar->lsf_index[2][2], &bit_ptr, &bit_cntr, 5, 1);
		pack_code(qpar->lsf_index[2][3], &bit_ptr, &bit_cntr, 5, 1);
	} else if ((uv1 == 1) && (uv2 != 1) && (cuv == 1)){
		pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[1][0], &bit_ptr, &bit_cntr, 8, 1);
		pack_code(qpar->lsf_index[1][1], &bit_ptr, &bit_cntr, 6, 1);
		pack_code(qpar->lsf_index[1][2], &bit_ptr, &bit_cntr, 5, 1);
		pack_code(qpar->lsf_index[1][3], &bit_ptr, &bit_cntr, 5, 1);
		pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 9, 1);
	} else if ((uv1 != 1) && (uv2 == 1) && (cuv == 1)){
		pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 8, 1);
		pack_code(qpar->lsf_index[0][1], &bit_ptr, &bit_cntr, 6, 1);
		pack_code(qpar->lsf_index[0][2], &bit_ptr, &bit_cntr, 5, 1);
		pack_code(qpar->lsf_index[0][3], &bit_ptr, &bit_cntr, 5, 1);
		pack_code(qpar->lsf_index[1][0], &bit_ptr, &bit_cntr, 9, 1);
		pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 9, 1);
	} else {
		if ((uv1 != 1) && (uv2 != 1) && (cuv == 1)){
			/* ---- Interpolation [4 inp + (8+6+6+6) res + 9 uv] ---- */
			pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 9, 1);
		} else {
			pack_code(qpar->lsf_index[0][0], &bit_ptr, &bit_cntr, 8, 1);
			pack_code(qpar->lsf_index[0][1], &bit_ptr, &bit_cntr, 6, 1);
			pack_code(qpar->lsf_index[0][2], &bit_ptr, &bit_cntr, 5, 1);
			pack_code(qpar->lsf_index[0][3], &bit_ptr, &bit_cntr, 5, 1);
		}

		pack_code(qpar->lsf_index[1][0], &bit_ptr, &bit_cntr, 4, 1);

		if ((uv1 != 1) && (uv2 != 1) && (cuv == 1)){
			pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 8, 1);
			pack_code(qpar->lsf_index[2][1], &bit_ptr, &bit_cntr, 6, 1);
			pack_code(qpar->lsf_index[2][2], &bit_ptr, &bit_cntr, 6, 1);
			pack_code(qpar->lsf_index[2][3], &bit_ptr, &bit_cntr, 6, 1);
			pack_code(lsp_prot,				 &bit_ptr, &bit_cntr, 3, 1);
		} else {
			pack_code(qpar->lsf_index[2][0], &bit_ptr, &bit_cntr, 8, 1);
			pack_code(qpar->lsf_index[2][1], &bit_ptr, &bit_cntr, 6, 1);
		}
	}

	/* ====== Packing gain index ====== */
	pack_code(qpar->gain_index[0], &bit_ptr, &bit_cntr, GAIN_VQ_BITS, 1);

	/* ====== Packing voicing indices ====== */
	for (i = 0; i < NF; i++){
		if (!qpar->uv_flag[i])
 			pack_code(qpar->bpvc_index[i], &bit_ptr, &bit_cntr, 2, 1);
	}
	if (cnt == 2)
		pack_code(bp_prot1, &bit_ptr, &bit_cntr, 2, 1);
	else if (cnt == 1){
		pack_code(bp_prot2, &bit_ptr, &bit_cntr, 2, 1);
		pack_code(bp_prot1, &bit_ptr, &bit_cntr, 2, 1);
	} else if (cnt == 0){                                              /* UUU */
  	    pack_code(qpar->bpvc_index[0], &bit_ptr, &bit_cntr, 2, 1);
	    pack_code(bp_prot2,            &bit_ptr, &bit_cntr, 2, 1);
	    pack_code(bp_prot1,            &bit_ptr, &bit_cntr, 2, 1);
	}

	/* ====== Packing Fourier magnitudes ====== */
	pack_code(qpar->fs_index, &bit_ptr, &bit_cntr, FS_BITS, 1);

	/* ====== Packing jitter index ====== */
	pack_code(qpar->jit_index[0], &bit_ptr, &bit_cntr, 1, 1);

	/* ======== Write channel output buffer ======== */
	qpar->chptr = chbuf;
	qpar->chbit = 0;
	for (i = 0; i < bitNum12; i++){
		pack_code(bit_buffer[i], &qpar->chptr, &qpar->chbit, 1, chwordsize);
		if (i == 0)
			*(qpar->chptr) |= 0x8000;           /* set beginning of frame bit */
	}
}