/*

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.

*/

/* ===================================== */
/* pit_lib.c: pitch analysis subroutines */
/* ===================================== */

#include "sc1200.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "mat_lib.h"
#include "math_lib.h"
#include "dsp_sub.h"
#include "pit_lib.h"
#include "constant.h"
#include "global.h"
#include "coeff.h"

#define PDECAY_Q15			31129                         /* 0.95 * (1 << 15) */
#define PDECAY_PITCH_Q7 	320            /* (0.05*DEFAULT_PITCH) * (1 << 7) */
#define NUM_MULT			8
#define SHORT_PITCH			3840                             /* 30 * (1 << 7) */
#define MAXFRAC				16384                          /* 2.0 * (1 << 13) */
#define MINFRAC				-8192                         /* -1.0 * (1 << 13) */
#define X05_Q13				4096                           /* 0.5 * (1 << 13) */

/* Added 1 to variables which appear in comparison statements to make it      */
/* bit-exact as tested version                                                */
#define UVMAX				(9011 + 1)                    /* 0.55 * (1 << 14) */
#define PCORR_THR			(9830 + 1)                     /* 0.6 * (1 << 14) */
#define PDOUBLE1			96                             /* 0.75 * (1 << 7) */
#define PDOUBLE2			64                              /* 0.5 * (1 << 7) */
#define PDOUBLE3			115                             /* 0.9 * (1 << 7) */
#define PDOUBLE4			89                              /* 0.7 * (1 << 7) */
#define LONG_PITCH			12800                        /* 100.0 * (1 << 7)) */
#define LPF_ORD_SOS			2

/* Prototypes */

static Shortword	double_chk(Shortword sig_in[], Shortword *pcorr,
							   Shortword pitch, Shortword pdouble,
							   Shortword pmin, Shortword pmax,
							   Shortword pmin_q7, Shortword pmax_q7,
							   Shortword lmin);

static void		double_ver(Shortword sig_in[], Shortword *pcorr,
						   Shortword pitch, Shortword pmin, Shortword pmax,
						   Shortword pmin_q7, Shortword pmax_q7,
						   Shortword lmin);


/*	double_chk.c: check for pitch doubling and also verify pitch multiple for */
/*                short pitches.                                              */
/*                                                                            */
/* Q values                                                                   */
/*     sig_in - Q0, pcorr - Q14, pitch - Q7, pdouble - Q7                     */

static Shortword	double_chk(Shortword sig_in[], Shortword *pcorr,
							   Shortword pitch, Shortword pdouble,
							   Shortword pmin, Shortword pmax,
							   Shortword pmin_q7, Shortword pmax_q7,
							   Shortword lmin)
{
	Shortword	mult, corr, thresh, temp_pit;
	Shortword	temp1, temp2;
	Longword	L_temp;


	pitch = frac_pch(sig_in, pcorr, pitch, 0, pmin, pmax, pmin_q7, pmax_q7,
					 lmin);

	/* compute threshold Q14*Q7>>8 */
	/* extra right shift to compensate left shift of L_mult */
	L_temp = L_mult(*pcorr, pdouble);
	L_temp = L_shr(L_temp, 8);
	thresh = extract_l(L_temp);                                        /* Q14 */

	/* Check pitch submultiples from shortest to longest */
	for (mult = NUM_MULT; mult >= 2; mult--){

		/* temp_pit = pitch / mult */
		temp1 = 0;
		temp2 = shl(mult, 11);                                         /* Q11 */
		temp_pit = pitch;
		while (temp_pit > temp2){
			temp_pit = shr(temp_pit, 1);
			temp1 = add(temp1, 1);
		}
		/* Q7*Q15/Q11 -> Q11 */
		temp2 = divide_s(temp_pit, temp2);
		temp1 = sub(4, temp1);
		/* temp_pit=pitch/mult in Q7 */
		temp_pit = shr(temp2, temp1);

		if (temp_pit >= pmin_q7){
			temp_pit = frac_pch(sig_in, &corr, temp_pit, 0, pmin, pmax,
								pmin_q7, pmax_q7, lmin);
			double_ver(sig_in, &corr, temp_pit, pmin, pmax, pmin_q7,
					   pmax_q7, lmin);

			/* stop if submultiple greater than threshold */
			if (corr > thresh){
				/* refine estimate one more time since previous window */
				/* may be off center slightly and temp_pit has moved */
				pitch = frac_pch(sig_in, pcorr, temp_pit, 0, pmin, pmax,
								 pmin_q7, pmax_q7, lmin);
				break;
			}
		}
	}

	/* Verify pitch multiples for short pitches */
	double_ver(sig_in, pcorr, pitch, pmin, pmax, pmin_q7, pmax_q7, lmin);

	/* Return full floating point pitch value and correlation*/
	return(pitch);
}


/* double_ver.c: verify pitch multiple for short pitches.                     */
/*                                                                            */
/* Q values                                                                   */
/*      pitch - Q7, pcorr - Q14                                               */

static void		double_ver(Shortword sig_in[], Shortword *pcorr,
						   Shortword pitch, Shortword pmin, Shortword pmax,
						   Shortword pmin_q7, Shortword pmax_q7,
						   Shortword lmin)
{
	Shortword	multiple;
	Shortword	corr, temp_pit;


	/* Verify pitch multiples for short pitches */
	multiple = 1;
	while (extract_l(L_shr(L_mult(pitch, multiple), 1)) < SHORT_PITCH){
		multiple = add(multiple, 1);
	}

	if (multiple > 1){
		temp_pit = extract_l(L_shr(L_mult(pitch, multiple), 1));
		temp_pit = frac_pch(sig_in, &corr, temp_pit, 0, pmin, pmax,
							pmin_q7, pmax_q7, lmin);

		/* use smaller of two correlation values */
		if (corr < *pcorr){
			*pcorr = corr;
		}
	}
}


/* f_pitch_scale.c: Scale pitch signal buffer for best precision              */

Shortword f_pitch_scale(Shortword sig_out[], Shortword sig_in[],
						Shortword length)
{
	register Shortword i;
	Shortword	scale;
	Shortword	*temp_buf;
	Longword	corr;
	Longword	L_temp, L_sum, L_margin;

	/* Compute signal buffer scale factor */
	scale = 0;

	/*	corr = L_v_magsq(sig_in, length, 0, 1); */
	L_sum = 0;
	L_margin = LW_MAX;
	temp_buf = sig_in;
	for (i = 0; i < length; i++){
		L_temp = L_mult(*temp_buf, *temp_buf);
		if (L_temp <= L_margin){
			L_sum = L_add(L_sum, L_temp);
			L_margin = L_sub(L_margin, L_temp);
		} else {
			L_margin = LW_MIN;
			break;
		}
		temp_buf ++;
	}
	corr = L_sum;

	if (L_margin == LW_MIN){

		/* allocate scratch buffer */
		temp_buf = v_get(length);

		/* saturation: right shift input signal and try again */
		scale = 5;
		v_equ_shr(temp_buf, sig_in, scale, length);
		corr = L_v_magsq(temp_buf, length, 0, 1);

		/* could add delta to compensate possible truncation error */

		/* free scratch buffer */
		v_free(temp_buf);
	}

	scale = sub(scale, shr(norm_l(corr), 1));

	/* Scale signal buffer */
	v_equ_shr(sig_out, sig_in, scale, length);

	/* return scale factor */
	return(scale);
}


/* find_pitch.c: Determine pitch value.                                       */
/*                                                                            */
/* Q values:                                                                  */
/*      sig_in - Q0, ipitch - Q0, *pcorr - Q14                                */
/*                                                                            */
/* WARNING: this function assumes the input buffer has been normalized by     */
/*          f_pitch_scale().                                                  */

Shortword find_pitch(Shortword sig_in[], Shortword *pcorr, Shortword lower,
					 Shortword upper, Shortword length)
{
	register Shortword	i;
	Shortword	cbegin, ipitch, even_flag;
	Shortword	s_corr, shift1a, shift1b, shift2, shift;
	Longword	c0_0, cT_T, corr;
	Longword	denom, max_denom, num, max_num;


	/* Find beginning of correlation window centered on signal */
	ipitch = lower;
	max_num = 0;
	max_denom = 1;
	even_flag = 1;
	/* cbegin = -((length+upper)/2) */
	cbegin = negate(shr(add(length, upper), 1));

	c0_0 = L_v_magsq(&sig_in[cbegin], length, 0, 1);
	cT_T = L_v_magsq(&sig_in[cbegin + upper], length, 0, 1);

	for (i = upper; i >= lower; i--){

		/* calculate normalized crosscorrelation */
		corr = L_v_inner(&sig_in[cbegin], &sig_in[cbegin + i], length, 0, 0, 1);

		/* calculate normalization for numerator and denominator */
		shift1a = norm_s(extract_h(c0_0));
		shift1b = norm_s(extract_h(cT_T));
		shift = add(shift1a, shift1b);
		shift2 = shr(shift, 1);               /* shift2 = half of total shift */
		if (shl(shift2, 1) != shift)
			shift1a = sub(shift1a, 1);

		/* check if current maximum value */
		if (corr > 0){
			s_corr = extract_h(L_shl(corr, shift2));
			num = extract_h(L_mult(s_corr, s_corr));
		} else
			num = 0;
		denom = extract_h(L_mult(extract_h(L_shl(c0_0, shift1a)),
								 extract_h(L_shl(cT_T, shift1b))));
		if (denom < 1)
			denom = 1;

		if (L_mult(extract_l(num), extract_l(max_denom)) >
			L_mult(extract_l(max_num), extract_l(denom))){
			max_denom = denom;
			max_num = num;
			ipitch = i;
		}

		/* update for next iteration */
		if (even_flag){
			even_flag = 0;
			c0_0 = L_msu(c0_0, sig_in[cbegin], sig_in[cbegin]);
			c0_0 = L_mac(c0_0, sig_in[cbegin + length],
						 sig_in[cbegin + length]);
			cbegin = add(cbegin, 1);
		} else {
			even_flag = 1;
			cT_T = L_msu(cT_T, sig_in[cbegin + i - 1 + length],
						 sig_in[cbegin + i - 1 + length]);
			cT_T = L_mac(cT_T, sig_in[cbegin + i - 1], sig_in[cbegin + i - 1]);
		}

	}

	/* Return pitch value and correlation*/
	*pcorr = shr(sqrt_fxp(divide_s(extract_l(max_num), extract_l(max_denom)),
								   15), 1);

	return(ipitch);
}


/* Name: frac_pch.c                                                           */
/* Description: Determine fractional pitch.                                   */
/* Inputs:                                                                    */
/*    sig_in - input signal                                                   */
/*    fpitch - initial floating point pitch estimate                          */
/*    range - range for local integer pitch search (0=none)                   */
/*    pmin - minimum allowed pitch value                                      */
/*    pmax - maximum allowed pitch value                                      */
/*    lmin - minimum correlation length                                       */
/* Outputs:                                                                   */
/*    pcorr - correlation at fractional pitch value                           */
/* Returns: fpitch - fractional pitch value                                   */
/*                                                                            */
/* Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.        */
/*                                                                            */