/*
 * This module implements the FR decoding part of trau-parse.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <tw_gsmfr.h>

/* this corresponds to the bit-lengths of the individual codec
 * parameters as indicated in Table 1.1 of TS 46.010 */
static const uint8_t gsm_fr_map[GSMFR_NUM_PARAMS] = {
	6, 6, 5, 5, 4, 4, 3, 3,
	7, 2, 2, 6, 3, 3, 3, 3,
	3, 3, 3, 3, 3, 3, 3, 3,
	3, 7, 2, 2, 6, 3, 3, 3,
	3, 3, 3, 3, 3, 3, 3, 3,
	3, 3, 7, 2, 2, 6, 3, 3,
	3, 3, 3, 3, 3, 3, 3, 3,
	3, 3, 3, 7, 2, 2, 6, 3,
	3, 3, 3, 3, 3, 3, 3, 3,
	3, 3, 3, 3
};

static unsigned
get_le(bits, nbits)
	uint8_t *bits;
	unsigned nbits;
{
	unsigned accum, mask;
	unsigned n;

	accum = 0;
	mask = 1;
	for (n = 0; n < nbits; n++) {
		if (*bits)
			accum |= mask;
		bits++;
		mask <<= 1;
	}
	return accum;
}

static void
dbits_to_params(d_bits, params)
	uint8_t *d_bits;
	int16_t *params;
{
	unsigned np, len;
	uint8_t *ip;
	int16_t *op;

	ip = d_bits;
	op = params;
	for (np = 0; np < GSMFR_NUM_PARAMS; np++) {
		len = gsm_fr_map[np];
		*op++ = get_le(ip, len);
		ip += len;
	}
}

void
print_fr_frame(d_bits)
	uint8_t d_bits;
{
	int16_t params[GSMFR_NUM_PARAMS];
	uint8_t rtp_pack[GSMFR_RTP_FRAME_LEN];
	int i, j, n, sid;

	dbits_to_params(d_bits, params);
	fputs("  FR frame:\n    ", stdout);
	n = 0;
	for (i = 0; i < 8; i++)
		printf(" %d", params[n++]);
	putchar('\n');
	for (i = 0; i < 4; i++) {
		fputs("     ", stdout);
		for (j = 0; j < 17; j++)
			printf(" %d", params[n++]);
		putchar('\n');
	}
	gsmfr_pack_from_array(params, rtp_pack);
	sid = gsmfr_preproc_sid_classify(rtp_pack);
	printf("    SID recompute: %d\n", sid);
	if (!bcmp(rtp_pack, gsmfr_decoder_homing_frame, GSMFR_RTP_FRAME_LEN))
		puts("    Matches DHF");
}