/*
 * In this module we implement our handling of BFI frame gaps
 * and invalid SID frames.
 */

#include <stdint.h>
#include <string.h>
#include "tw_gsmfr.h"
#include "pp_state.h"
#include "pp_internal.h"

static int reduce_xmaxc(uint8_t *frame)
{
	int mute_flag = 1;
	unsigned sub, xmaxc;

	for (sub = 0; sub < 4; sub++) {
		xmaxc = ((frame[sub*7+6] & 0x1F) << 1) | (frame[sub*7+7] >> 7);
		if (xmaxc > 4) {
			xmaxc -= 4;
			mute_flag = 0;
		} else
			xmaxc = 0;
		frame[sub*7+6] &= 0xE0;
		frame[sub*7+6] |= xmaxc >> 1;
		frame[sub*7+7] &= 0x7F;
		frame[sub*7+7] |= (xmaxc & 1) << 7;
	}
	return mute_flag;
}

static void random_grid_pos(struct gsmfr_preproc_state *st, uint8_t *frame)
{
	unsigned sub, Mc;

	for (sub = 0; sub < 4; sub++) {
		Mc = gsmfr_preproc_prng(st, 2);
		frame[sub*7+6] &= 0x9F;
		frame[sub*7+6] |= Mc << 5;
	}
}

static int reduce_xmaxc_sid(struct gsmfr_preproc_state *st)
{
	if (st->sid_xmaxc > 4) {
		st->sid_xmaxc -= 4;
		return 0;
	} else {
		st->sid_xmaxc = 0;
		return 1;
	}
}

void gsmfr_preproc_bfi(struct gsmfr_preproc_state *st, int taf, uint8_t *frame)
{
	int mute;

	switch (st->rx_state) {
	case NO_DATA:
		memcpy(frame, &gsmfr_preproc_silence_frame,
			GSMFR_RTP_FRAME_LEN);
		if (st->dtxd_nodata_count) {
			st->dtxd_sid_flag = 0;
			st->dtxd_nodata_count--;
		} else
			st->dtxd_sid_flag = 1;
		return;
	case SPEECH:
		memcpy(frame, &st->speech_frame, GSMFR_RTP_FRAME_LEN);
		st->rx_state = SPEECH_MUTING;
		st->dtxd_sid_flag = 0;
		return;
	case SPEECH_MUTING:
		mute = reduce_xmaxc(st->speech_frame);
		memcpy(frame, &st->speech_frame, GSMFR_RTP_FRAME_LEN);
		random_grid_pos(st, frame);
		if (mute) {
			st->rx_state = NO_DATA;
			st->dtxd_nodata_count = 0;
		}
		st->dtxd_sid_flag = 0;
		return;
	case COMFORT_NOISE:
		st->dtxd_sid_flag = 1;
		if (taf)
			st->rx_state = LOST_SID;
		gsmfr_preproc_gen_cn(st, frame);
		return;
	case LOST_SID:
		st->dtxd_sid_flag = 1;
		if (taf) {
			st->rx_state = CN_MUTING;
			reduce_xmaxc_sid(st);
			st->dtxd_sid_flag = 0;
		}
		gsmfr_preproc_gen_cn(st, frame);
		return;
	case CN_MUTING:
		st->dtxd_sid_flag = 0;
		if (reduce_xmaxc_sid(st)) {
			st->rx_state = NO_DATA;
			memcpy(frame, &gsmfr_preproc_silence_frame,
				GSMFR_RTP_FRAME_LEN);
			st->dtxd_nodata_count = 3;
		} else
			gsmfr_preproc_gen_cn(st, frame);
		return;
	}
}

void gsmfr_preproc_invalid_sid(struct gsmfr_preproc_state *st, uint8_t *frame)
{
	int mute;

	switch (st->rx_state) {
	case NO_DATA:
		memcpy(frame, &gsmfr_preproc_silence_frame,
			GSMFR_RTP_FRAME_LEN);
		if (st->dtxd_nodata_count) {
			st->dtxd_sid_flag = 0;
			st->dtxd_nodata_count--;
		} else
			st->dtxd_sid_flag = 1;
		return;
	case SPEECH:
		/*
		 * Make CN out of the last good speech frame, following the
		 * "NOTE" at the end of section 6.1.2 in TS 46.031.
		 */
		st->rx_state = COMFORT_NOISE;
		st->dtxd_sid_flag = 1;
		memcpy(st->sid_prefix, &st->speech_frame, 5);
		st->sid_xmaxc = gsmfr_preproc_xmaxc_mean(st->speech_frame);
		gsmfr_preproc_gen_cn(st, frame);
		return;
	case SPEECH_MUTING:
		/* ignore invalid SID in this state and act as if we got BFI */
		mute = reduce_xmaxc(st->speech_frame);
		memcpy(frame, &st->speech_frame, GSMFR_RTP_FRAME_LEN);
		random_grid_pos(st, frame);
		if (mute) {
			st->rx_state = NO_DATA;
			st->dtxd_nodata_count = 0;
		}
		st->dtxd_sid_flag = 0;
		return;
	case COMFORT_NOISE:
	case LOST_SID:
		st->rx_state = COMFORT_NOISE;
		st->dtxd_sid_flag = 1;
		gsmfr_preproc_gen_cn(st, frame);
		return;
	case CN_MUTING:
		st->dtxd_sid_flag = 0;
		if (reduce_xmaxc_sid(st)) {
			st->rx_state = NO_DATA;
			memcpy(frame, &gsmfr_preproc_silence_frame,
				GSMFR_RTP_FRAME_LEN);
			st->dtxd_nodata_count = 3;
		} else
			gsmfr_preproc_gen_cn(st, frame);
		return;
	}
}

void gsmfr_preproc_bfi_bits(struct gsmfr_preproc_state *st,
			    const uint8_t *bad_frame, int taf,
			    uint8_t *frame_out)
{
	if (gsmfr_preproc_sid_classify(bad_frame) == 0)
		gsmfr_preproc_bfi(st, taf, frame_out);
	else
		gsmfr_preproc_invalid_sid(st, frame_out);
}