/* -*- c++ -*- */
/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#include "common.h"
#include <gsm_run_bb.h>
#include <gr_io_signature.h>
#include "sch.h"
#include "cch.h"
#include "data_out.h"

struct _opt opt;

/* Estiamted earliest SCH start after FCH 142 bit's 0 */
#define GSMSP_EST_SCH_START_AFTER_FCH	(3 + 8 + 156 * 7)

/* SCH training sequence */
#if 1
const unsigned char syncbits[] = {
0x01, 0x00, 0x01, 0x01, 0x01, 0x00, 0x00, 0x01,
0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x01,
0x00, 0x00, 0x01, 0x00, 0x01, 0x01, 0x00, 0x01,
0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01,
0x00, 0x01, 0x01, 0x01, 0x00, 0x01, 0x01, 0x00,
0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x01, 0x01 };
#endif

static unsigned char syncbitsdiffencoded[sizeof syncbits];

static ssize_t g_req_size;

static void diff_encode(unsigned char *out, const unsigned char *in, size_t len);
static void diff_decode(unsigned char *out, const unsigned char *in, size_t len);
static int memdiff(const unsigned char *s1, const unsigned char *s2, size_t n);
/*
 * Create a new instance of howto_square2_ff and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
gsm_run_bb_sptr 
gsm_make_run_bb ()
{
  return gsm_run_bb_sptr (new gsm_run_bb ());
}

/*
 * The private constructor
 */
gsm_run_bb::gsm_run_bb ()
  : gr_sync_block ("run_bb",
		   gr_make_io_signature (1, 1, sizeof (unsigned char)),
		   gr_make_io_signature (0, 0, 0))
{
	set_output_multiple(156);
	bits_processed = 0;
	iPhase = 1;
	ChangeSearchMode(EModeFindFCCH);
	diff_encode(syncbitsdiffencoded, syncbits, sizeof syncbits);
	//HEXDUMPF(syncbitsdiffencoded, sizeof syncbitsdiffencoded, "syncbitsdiffencoded:\n");
	interleave_init(&opt.ictx, 456, 114);
}

/*
 * Our virtual destructor.
 */
gsm_run_bb::~gsm_run_bb ()
{
  // nothing else required in this example
	interleave_deinit(&opt.ictx);
}

void
gsm_run_bb::forecast(int noutput_items, gr_vector_int &ninput_items_required)
{
	unsigned ninputs = ninput_items_required.size();
	for (unsigned int i = 0; i < ninputs; i++)
		ninput_items_required[i] = noutput_items;
}

/*
 * Find SCH in inbuf[]. Return the offset of the SCH *start*.
 */
int
gsm_run_bb::find_sch()
{
	int ret;
	const unsigned char *end;

	end = iPtr + 3 + 39 + sizeof syncbitsdiffencoded + 39 + 8;

	DEBUGF("earliest SCH can start: %d\n", bits_processed + iPtr - iStart);

	while (iPtr + 39 + 3 + sizeof syncbitsdiffencoded < end)
	{
		ret = memdiff(iPtr + 39 + 3, syncbitsdiffencoded, sizeof (syncbitsdiffencoded));
		//DEBUGF("memdiff start: %d, ret = %d\n", bits_processed + iPtr - iStart + 39 + 3, ret);
		if (ret <= 5)
		{
			DEBUGF("Possible SCH start: %d (bit error %d)\n", bits_processed + iPtr - iStart, ret);
			//iPtr = iPtr - 39 - 3;
			return 0;
		}
		iPtr++;
	}

	return -1;	/* SCH not found */
}

int
gsm_run_bb::process_sch()
{
	int fn = 0, bsic = 0;
	int ret;
	unsigned char burst[156];

	//DEBUGF("process_sch at %d\n", bits_processed + iPtr - iStart);
	diff_decode(burst, iPtr, 156);
	ret = decode_sch(burst, &fn, &bsic);
	DEBUGF("SCH ret %d, fn %d, bsic %d\n", ret, fn, bsic);
	if (ret == 0)
		iFN = fn;

	return ret;
}

/*
 * Search for an exact match!
 * FIXME: later on accepts FCH burst with a few bit errors.
 */
int
gsm_run_bb::find_fch()
{
	while (iPtr < iEnd)
	{
	//	DEBUGF("zero count %d\n", fcch_zero_count);
		if (*iPtr == iFchLastBit)
		{

			fcch_zero_count++;
			if (fcch_zero_count >= 142)
			{
				iPtr++;
				return 0;
			}
		} else {
			fcch_zero_count = 0;
			iFchLastBit = *iPtr;
		}
		iPtr++;
	}

	return -1; /* Need more data */
}

void
gsm_run_bb::ChangeSearchMode(TMode new_mode)
{
	switch (new_mode)
	{
	case EModeFindFCCH:
		ts_n = 0;
		g_req_size = 1;
		iFchLastBit = 0;
		fcch_zero_count = 0;
		break;
	case EModeProcessL2:
		memset(timeslots, 0, sizeof timeslots);
	//	memset(timeslots[0].data, 0x41, sizeof timeslots[0].data);
		timeslots[0].n_max_frames = 51;
		timeslots[0].config = TS_CONFIG_CCH;
		for (int i = 1; i < 2; i++)
		{
			timeslots[i].n_max_frames = 51;
			timeslots[i].config = TS_CONFIG_SDCCH8;
		}
	case EModeProcessSCH:
	case EModeFindSCH:
		ts_n = 0;
	default:
		g_req_size = 156;
	}

	mode = new_mode;
}

/*
 * encode:
 * - last = 1
 * - *out = !(in[0] ^ last)
 * decode:
 * - last = 1
 * => ret 0 seen 16 times. all FCCH with Phase: 1
 * decode:
 * - last = 0
 * => ret 0 seen 2 times. all FCCH with Phase: 1
 */
static void
diff_decode(unsigned char *out, const unsigned char *in, size_t len)
{
	const unsigned char *end = in + len;
	unsigned char last;	/* = 1; */

	/*
	 * The first 3 decoded bits must be 0 because they are the tail bits.
	 * If one of these bits is wrong then the error is propagated.
	 * To prevent this we just always set the first 3 bits to 0.
	 */
#if 0
	int ret;
	ret = memdiff(in, (const unsigned char *)"\x00\x01\x01", 3);
	if (ret != 0)
	{
		HEXDUMPF(in, 3, "BIT ERROR in TAIL BITS (%d)\n", ret);
	}
#endif
#if 1
	in += 3;
	last = 0;
	memset(out, 0, 3);
	out += 3;
#endif

	while (in < end)
	{
		*out = !*in ^ last;
		last = *out;
		in++;
		out++;
	}
}

/*
 * Init value is always 1. (after GSM 05.04?)
 */
static void
diff_encode(unsigned char *out, const unsigned char *in, size_t len)
{
	const unsigned char *end = in + len;
	unsigned char last = 1;

	while (in < end)
	{
		*out = !(in[0] ^ last);
		last = in[0];
		in++;
		out++;
	}
}

int 
gsm_run_bb::work (int noutput_items,
			gr_vector_const_void_star &input_items,
			gr_vector_void_star &/*output_items*/)
{
	int ret;

	iStart = (const unsigned char *)input_items[0];
	iPtr = iStart;
	iEnd = iStart + noutput_items;

	while (iEnd - iPtr >= g_req_size)
	{
		ts_n = (ts_n + 1) % 8;
		if (ts_n == 0)
			iFN++;
		switch (mode)
		{
		case EModeFindFCCH:
			ret = find_fch();
			if (ret != 0)
				break;
			DEBUGF("ts %d: Found FCH start at %d (Phase: %d)\n", ts_n, bits_processed + (iPtr - iStart) - 142, iFchLastBit);
			iPtr += 3 + 8; 	/* Tail + guard */
			iPhase = iFchLastBit;
			ChangeSearchMode(EModeFindSCH);
			break;
		case EModeFindSCH:
			if (ts_n != 0)
			{
				iPtr += g_req_size;
				break;
			}
			ret = find_sch();
			if (ret != 0)
				ChangeSearchMode(EModeFindFCCH);
			else
				ChangeSearchMode(EModeProcessSCH);
			break;
		case EModeProcessSCH:
			if (process_sch() != 0)
			{
				ChangeSearchMode(EModeFindFCCH);
			} else {
				iPtr += g_req_size;
				ChangeSearchMode(EModeProcessL2);
			}
			//ChangeSearchMode(EModeFindFCCH);
			break;
		default:
			/* Check if handling this timeslot...*/
			/* At the moment only handle cch channel */
			if (timeslots[ts_n].config == TS_CONFIG_UNKNOWN)
			{
				iPtr += g_req_size;
				break;
			}
#if 1
			/* 1, 11, 21, 31, 41 but not 0, 10, 20, 30, 40, 50, . */
			if ((timeslots[ts_n].config == TS_CONFIG_CCH) && ((iFN % 51 != 0) && ((iFN % 51 + 9) % 10 == 0)))
			{
				ret = memdiff(iPtr + 39 + 3, syncbitsdiffencoded, sizeof (syncbitsdiffencoded));
				if (ret >= 5)
				{
					HEXDUMPF(iPtr + 39 + 3, sizeof syncbitsdiffencoded, "ERR: Out of sync!\n");
					ChangeSearchMode(EModeFindFCCH);
					break;
				}
				ChangeSearchMode(EModeProcessSCH);
				break;
			}
#endif

			burst_ptr = timeslots[ts_n].data[iFN % timeslots[ts_n].n_max_frames];
			diff_decode(burst_ptr, iPtr, 156);
			if (handle_l2() != 0)
			{
				ChangeSearchMode(EModeFindFCCH);
				break;
			}
			iPtr += g_req_size;
			break;
		}

		/* NOTHING HERE */
	}

	bits_processed += iPtr - iStart;
	return iPtr - iStart;
}

int
gsm_run_bb::handle_l2()
{
	unsigned char *data;
	unsigned int len;
	unsigned char b[4][58 * 2];
	unsigned char ready = 0;
	//DEBUGF("(ts %d, iFN %% %d = %d\n", ts_n, timeslots[ts_n].n_max_frames, iFN % 51);

	/* FIXME: if ts_n == 0 then check if burst 1, 11, 21, 31, ... etc
	 * are still same bsic and if frame number matches...
	 */
	if (timeslots[ts_n].config == TS_CONFIG_CCH)
	{
		/* 2..5, 12..15, 22..25, 23..35, 42..45 */
		/* 6..9, 16..19, 26..29, 36..39, 46..49 */
		/* But not 0, 1, 50, ... */
		if ((iFN % 51 != 0) && ( (((iFN % 51) + 5) % 10 == 0) || (((iFN % 51) + 1) % 10 == 0) ) )
			ready = 1;
		//ready = 0; /* TESTING */
	} else if (timeslots[ts_n].config == TS_CONFIG_SDCCH8) {
		/* 0..3, 4..7, 8..11, 12..15, .. but not 48..50 */
		if (((iFN % 51) <= 47) &&  (((iFN % 51) + 1) % 4 == 0))
			ready = 1;
	}

	if (ready == 0)
		return 0;

	int tdma_n = (iFN % 51) - 3;
	for (int i = 0; i < 4; i++)
	{
		memcpy(b[i], timeslots[ts_n].data[tdma_n + i] + 3, 58);
		memcpy(b[i] + 58, timeslots[ts_n].data[tdma_n + i] + 3 + 58 + 26, 58);
	}
	data = decode_cch(b[0], b[1], b[2], b[3], &len);
	if (data == NULL)
	{
		//DEBUGF("ERR TS %d decoding at %d\n", ts_n, bits_processed + iPtr - iStart);
		return 0;
	}
	DEBUGF("OK TS %d decoding at %d, len %d\n", ts_n, bits_processed + iPtr - iStart, len);
	//HEXDUMPF(data, len, "DATA (%d)\n", len);
#if 0 
	if (1)
	{
		unsigned char data[]={0x17, 0x06, 0x00, 0xe0, 0xa7, 0x80, 0xe0 ,0x0e ,0x12 ,0x04 ,0xd2 ,0x49 ,0xd2 ,0xda ,0x2b ,0x2b ,0x2b ,0x2b ,0x2b ,0x2b ,0x2b ,0x2b ,0x2b};
		l2_data_out(1, 0, data, 23);
		exit(0);
	}
#endif
	if (timeslots[ts_n].config == TS_CONFIG_SDCCH8)
	{
		l2_data_out_B(iFN, data, len);
	} else {
		l2_data_out_Bbis(iFN, data, len);
	}

	return 0;
}

/*
 * Return number of bytes s1 differes from s2.
 * Return 0 if s1 and s2 are equal.
 */
static int
memdiff(const unsigned char *s1, const unsigned char *s2, size_t n)
{
	int diff = 0;

	for (size_t i = 0; i < n; i++)
		if (s1[i] != s2[i])
			diff++;

	return diff;
}