/*
 * Rx front end implementation, common between the full decoder and TFO.
 */

#include <stdint.h>
#include <string.h>
#include "tw_gsmhr.h"
#include "typedefs.h"
#include "namespace.h"
#include "rxfe.h"
#include "dtx_const.h"
#include "dtx_rxfe.h"

/*
 * If the very first frame(s) we have to process out of reset (or after DHF)
 * are either BFI or invalid SID, what is our default fallback frame?
 * In the original GSM 06.06 code it is a frame of all zero params - but
 * because that code provides only a full decoder and not TFO, this oddity
 * is not readily visible.  (The PCM output from the full decoder is all
 * zeros.)  OTOH, if we feed all zeros as PCM input to a homed standard
 * encoder, we get this frame, repeating endlessly as long as all-zeros
 * PCM input continues:
 *
 * R0=00 LPC=164,171,cb Int=0 Mode=0
 * s1=00,00,00 s2=00,00,00 s3=00,00,00 s4=00,00,00
 *
 * This frame differs from all-zero params only in the LPC set, and this
 * sane-LPC silence frame is the one we shall use as our reset-default
 * fallback frame.
 */
static const Shortword default_lpc[3] = {0x164, 0x171, 0xcb};

/*
 * UFI R0 handling tables: when we get a speech frame marked with BFI=0 UFI=1,
 * we look at the R0 parameter, both in the received frame and in the saved
 * one.  Both of the following LUTs are indexed by old R0 just like in the
 * original GSM 06.06 code, but the values in them are absolute thresholds
 * for the new R0, instead of diff values used in the original code.
 */
static const Shortword ufi_r0_thresh_bfi[32] = {
	15, 16, 17, 15, 16, 17, 18, 18,
	18, 19, 19, 20, 21, 22, 22, 23,
	23, 23, 23, 24, 25, 25, 26, 26,
	26, 27, 28, 29, 30, 31, 32, 32
};

static const Shortword ufi_r0_thresh_mute[32] = {
	14, 13, 13, 12, 13, 14, 15, 14,
	15, 16, 17, 18, 18, 19, 20, 20,
	21, 21, 21, 22, 23, 24, 25, 25,
	25, 26, 27, 28, 29, 30, 30, 30
};

/* state transition tables for ECU state machine */
static const uint8_t ecu_state_trans_good[8] = {0, 0, 0, 0, 0, 0, 7, 0};
static const uint8_t ecu_state_trans_bad[8]  = {1, 2, 3, 4, 5, 6, 6, 6};

void gsmhr_rxfe_reset(struct gsmhr_rxfe_state *st)
{
	memset(st, 0, sizeof(struct gsmhr_rxfe_state));
	memcpy(st->saved_frame + 1, default_lpc, sizeof(Shortword) * 3);
	st->ecu_state = 7;
}

static Shortword input_frame_class(Shortword bfi, Shortword ufi, Shortword sid)
{
	Shortword bfi_dtx;

	bfi_dtx = bfi || ufi;
	if (sid == 2 && bfi_dtx == 0)
		return VALIDSID;
	if (sid == 0 && bfi_dtx == 0)
		return GOODSPEECH;
	if (sid == 0 && bfi_dtx != 0)
		return UNUSABLE;
	return INVALIDSID;
}

static Shortword compute_last_lag(const Shortword *prm_in)
{
	Shortword accum, delta, i;

	accum = prm_in[6];
	for (i = 0; i < 3; i++) {
		delta = prm_in[9 + i * 3] - 8;
		accum += delta;
		if (accum > 0xFF)
			accum = 0xFF;
		else if (accum < 0)
			accum = 0;
	}
	return accum;
}

static void save_speech_frame(struct gsmhr_rxfe_state *st,
				const Shortword *prm_in)
{
	memcpy(st->saved_frame, prm_in, sizeof(Shortword) * 6);
	if (prm_in[5]) {
		/* voiced modes */
		st->saved_frame[6] = compute_last_lag(prm_in);
		st->saved_frame[7] = prm_in[7];
		st->saved_frame[9] = 8;
		st->saved_frame[10] = prm_in[10];
		st->saved_frame[12] = 8;
		st->saved_frame[13] = prm_in[13];
		st->saved_frame[15] = 8;
		st->saved_frame[16] = prm_in[16];
	} else {
		/* unvoiced mode */
		st->saved_frame[6] = prm_in[6];
		st->saved_frame[7] = prm_in[7];
		st->saved_frame[9] = prm_in[9];
		st->saved_frame[10] = prm_in[10];
		st->saved_frame[12] = prm_in[12];
		st->saved_frame[13] = prm_in[13];
		st->saved_frame[15] = prm_in[15];
		st->saved_frame[16] = prm_in[16];
	}
	/* all 4 gsp0 params come from the last subframe */
	st->saved_frame[8] = prm_in[17];
	st->saved_frame[11] = prm_in[17];
	st->saved_frame[14] = prm_in[17];
	st->saved_frame[17] = prm_in[17];
}

static void speech_ecu(struct gsmhr_rxfe_state *st, const Shortword *prm_in,
			Shortword *prm_out, Shortword *mute_permit,
			Shortword *dtxd_sp)
{
	Shortword bfi = prm_in[18];
	Shortword ufi = prm_in[19];
	uint8_t prev_state = st->ecu_state;

	if (ufi && !bfi) {
		if (prm_in[0] >= ufi_r0_thresh_bfi[st->saved_frame[0]])
			bfi = 1;
		else if (prm_in[0] >= ufi_r0_thresh_mute[st->saved_frame[0]]) {
			if (mute_permit)
				*mute_permit = 1;
		}
	}
	if (bfi)
		st->ecu_state = ecu_state_trans_bad[prev_state];
	else
		st->ecu_state = ecu_state_trans_good[prev_state];
	switch (st->ecu_state) {
	case 0:
		memcpy(prm_out, prm_in, sizeof(Shortword) * GSMHR_NUM_PARAMS);
		save_speech_frame(st, prm_in);
		return;
	/* all remaining cases return the saved frame, differ only in R0 */
	case 1:
	case 2:
		/* no muting yet */
		break;
	case 3:
	case 4:
	case 5:
		st->saved_frame[0] -= 2;
		if (st->saved_frame[0] < 0)
			st->saved_frame[0] = 0;
		break;
	case 6:
	case 7:
		st->saved_frame[0] = 0;
		break;
	}
	memcpy(prm_out, st->saved_frame, sizeof(Shortword) * GSMHR_NUM_PARAMS);
	if (bfi < 2) {
		if (st->saved_frame[5] == 0 && prm_in[5] == 0) {
			/* both unvoiced */
			prm_out[6] = prm_in[6];
			prm_out[7] = prm_in[7];
			prm_out[9] = prm_in[9];
			prm_out[10] = prm_in[10];
			prm_out[12] = prm_in[12];
			prm_out[13] = prm_in[13];
			prm_out[15] = prm_in[15];
			prm_out[16] = prm_in[16];
		} else if (st->saved_frame[5] != 0 && prm_in[5] != 0) {
			/* both voiced */
			prm_out[7] = prm_in[7];
			prm_out[10] = prm_in[10];
			prm_out[13] = prm_in[13];
			prm_out[16] = prm_in[16];
		}
	}
	if (dtxd_sp && prev_state >= 6)
		*dtxd_sp = 0;
}

static void save_speech_gs(struct gsmhr_rxfe_state *st,
			   const Shortword *sp_param)
{
	Shortword vmode = sp_param[5];
	uint8_t ptr = st->gs_history_ptr;
	int i;

	for (i = 0; i < N_SUB; i++) {
		st->gs_history[ptr] = ppLr_gsTable[vmode][sp_param[8 + i * 3]];
		ptr++;
		if (ptr >= GS_HISTORY_SIZE)
			ptr = 0;
	}
	st->gs_history_ptr = ptr;
}

static void init_cn_gen(struct gsmhr_rxfe_state *st)
{
	Longword L_RxDTXGs;

	st->cn_prng = PN_INIT_SEED;
	avgGsHistQntz(st->gs_history, &L_RxDTXGs);
	st->gs_cn_out = gsQuant(L_RxDTXGs, 0);
	st->in_dtx = 1;
	st->dtx_bfi_count = 0;
	st->dtx_muting = 0;
}

static void cn_output(struct gsmhr_rxfe_state *st, Shortword *prm_out)
{
	int i;

	memcpy(prm_out, st->cn_r0_lpc, sizeof(Shortword) * 4);
	prm_out[4] = 1;		/* soft interpolation */
	prm_out[5] = 0;		/* unvoiced mode */
	for (i = 0; i < N_SUB; i++) {
		prm_out[6 + i * 3] = getPnBits(7, &st->cn_prng);
		prm_out[7 + i * 3] = getPnBits(7, &st->cn_prng);
		prm_out[8 + i * 3] = st->gs_cn_out;
	}
}

void rxfe_main(struct gsmhr_rxfe_state *st, const Shortword *prm_in,
		Shortword *prm_out, int fast_cn_muting,
		Shortword *deco_mode_out, Shortword *mute_permit,
		Shortword *dtxd_sp)
{
	Shortword frame_class, deco_mode;

	frame_class = input_frame_class(prm_in[18], prm_in[19], prm_in[20]);
	if (!st->in_dtx) {
		/* speech decoding mode */
		if (frame_class == VALIDSID || frame_class == INVALIDSID)
			deco_mode = CNIFIRSTSID;
		else
			deco_mode = SPEECH;
	} else {
		/* comfort noise insertion mode */
		if (frame_class == VALIDSID || frame_class == INVALIDSID)
			deco_mode = CNICONT;
		else if (frame_class == UNUSABLE)
			deco_mode = CNIBFI;
		else
			deco_mode = SPEECH;
	}
	/* Muting permission for the full decoder will be set only in one
	 * special case of speech UFI - default it to 0.
	 */
	if (mute_permit)
		*mute_permit = 0;
	/* now real per-mode processing */
	switch (deco_mode) {
	case SPEECH:
		/* Except for the special case of prolonged BFI that
		 * led to total muting, TFO with DTXd gets SP=1.
		 */
		if (dtxd_sp)
			*dtxd_sp = 1;
		st->in_dtx = 0;
		speech_ecu(st, prm_in, prm_out, mute_permit, dtxd_sp);
		save_speech_gs(st, prm_out);
		break;
	case CNIFIRSTSID:
		if (dtxd_sp)
			*dtxd_sp = 0;
		if (frame_class == VALIDSID)
			memcpy(st->cn_r0_lpc, prm_in, sizeof(Shortword) * 4);
		else
			memcpy(st->cn_r0_lpc, st->saved_frame,
				sizeof(Shortword) * 4);
		init_cn_gen(st);
		cn_output(st, prm_out);
		break;
	case CNICONT:
		if (dtxd_sp)
			*dtxd_sp = 0;
		if (frame_class == VALIDSID)
			memcpy(st->cn_r0_lpc, prm_in, sizeof(Shortword) * 4);
		else
			deco_mode = CNIBFI;	/* for interpolation */
		st->dtx_bfi_count = 0;
		st->dtx_muting = 0;
		cn_output(st, prm_out);
		break;
	case CNIBFI:
		if (dtxd_sp)
			*dtxd_sp = 0;
		if (st->dtx_muting) {
			if (fast_cn_muting)
				st->cn_r0_lpc[0] = 0;
			else {
				st->cn_r0_lpc[0] -= 2;
				if (st->cn_r0_lpc[0] < 0)
					st->cn_r0_lpc[0] = 0;
			}
		}
		st->dtx_bfi_count++;
		if (st->dtx_bfi_count >= (prm_in[21] ? 25 : 36))
			st->dtx_muting = 1;
		cn_output(st, prm_out);
		break;
	}
	if (deco_mode_out)
		*deco_mode_out = deco_mode;
}