// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <crc/crc.h>
#include "symbol.h"
#include "frame.h"

// --------------------------------------------------------------
uint32_t Scramble(uint8_t *pSrcDst, uint32_t state, uint32_t poly, uint32_t len)
{
	uint32_t i;

	while(len)
	{
		*pSrcDst ^= (uint8_t)state;
		pSrcDst++;
		len--;

		for (i=0; i < 8; i++)
		{
			if (state & 0x0001)
				state = (state >> 1) ^ poly;
			else
				state = (state >> 1);
		}

	}
	return state;
}

uint32_t FrameCalcNumStreamBytes(uint32_t len_raw)
{
	uint32_t len, num_frames;
	num_frames = (uint32_t)ceil((float)len_raw / FRAME_NUM_BYTES_PER_FRAME);
	len = num_frames*sizeof(frame_t);

	if (len < sizeof(frame_t))
		len = sizeof(frame_t);

	return len;
}

uint32_t FrameFormat(uint8_t *pRaw, uint8_t *pFormatted, uint32_t len_raw, uint32_t *pPrn_state, uint16_t frame_type)
{
	frame_t frame;

	uint32_t bytes_remain, len_formatted, len;
	uint32_t prn_state;

	if (len_raw == 0)
		return 0;

	prn_state = *pPrn_state;

	// Set preamble
	frame.hdr.preamble[0] = FRAME_PREAMBLE_IV;
	frame.hdr.preamble[1] = ~(frame.hdr.preamble[0]);

	bytes_remain = len_raw;
	len_formatted = 0;
	while(bytes_remain)
	{
		len = FRAME_NUM_BYTES_PER_FRAME;
		if (bytes_remain < FRAME_NUM_BYTES_PER_FRAME)
			len = bytes_remain;

		memset(frame.data, 0, FRAME_NUM_BYTES_PER_FRAME);
		memcpy(frame.data, pRaw, len);
		frame.hdr.crc16 = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, frame.data, FRAME_NUM_BYTES_PER_FRAME);
		frame.hdr.type = frame_type;
		frame.hdr.prn_state = prn_state;
		frame.hdr.length = (uint16_t)len;
		prn_state = Scramble(frame.data, prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
		prn_state = Scramble((uint8_t*)&frame.hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.crc16));
		prn_state = Scramble((uint8_t*)&frame.hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.type));
		prn_state = Scramble((uint8_t*)&frame.hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.length));

		pRaw += len;
		bytes_remain -= len;

		memcpy(pFormatted, &frame, sizeof(frame_t));
		pFormatted += sizeof(frame_t);
		len_formatted += sizeof(frame_t);

	}
	*pPrn_state = prn_state;
	return len_formatted;
}

uint32_t FrameDeformat(uint8_t *pFormatted, uint8_t *pRaw, uint32_t len_formatted, uint16_t frame_type_mask)
{
	frame_hdr_t hdr;
	uint32_t bytes_remain, len_raw, sync_state, frame_sync, i;
	uint16_t crc16_ist;
	uint8_t temp[FRAME_NUM_BYTES_PER_FRAME];
	uint32_t prn_state;

	if (len_formatted == 0)
		return 0;

	i = 0;
	bytes_remain = len_formatted;
	len_raw = 0;
	while(bytes_remain)
	{

		sync_state = 0;
		do
		{
			// Find frame boundaries
			switch(sync_state)
			{
				case 0:
				if (pFormatted[i] == FRAME_PREAMBLE_IV)
				{
					sync_state = 1;
				}
				break;

				case 1:
				if ((uint8_t)(~pFormatted[i]) == pFormatted[i-1])
				{
					sync_state = 2;
				}
				else
				{
					sync_state = 0;
				}
				break;
			}
			i++;
			bytes_remain--;
			frame_sync = (sync_state == 2);

		} while(bytes_remain && !frame_sync);

		memcpy(&hdr, &pFormatted[i-2], sizeof(frame_hdr_t));
		memcpy(temp, &pFormatted[i-2+sizeof(frame_hdr_t)], FRAME_NUM_BYTES_PER_FRAME);
		
		prn_state = Scramble(temp, hdr.prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
		prn_state = Scramble((uint8_t*)&hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.crc16));
		prn_state = Scramble((uint8_t*)&hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.type));
		Scramble((uint8_t*)&hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.length));

		crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, temp, FRAME_NUM_BYTES_PER_FRAME);

		if (crc16_ist == hdr.crc16)
		{
			
			if ((hdr.type & frame_type_mask) == frame_type_mask)
			{
				memcpy(&pRaw[len_raw], temp, hdr.length);
				len_raw += hdr.length;
			}
			i += FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2;
			bytes_remain -= (FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2);
		}

		if ((int32_t)bytes_remain < sizeof(frame_t))
			bytes_remain = 0;
	}
	return len_raw;
}

// Transmission order MSB first
uint32_t FrameSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen)
{

	uint32_t bits_remain, i, j;
	uint8_t src, dst;

	if (srclen == 0)
		return 0;

	if (nBitsPerSym > 8)
		return (uint32_t)-1;

	j = 0;
	src = *pSrc;
	bits_remain = 8;
	while(srclen)
	{
		dst = 0;
		for (i=0; i < nBitsPerSym; i++)
		{
			dst <<= 1;
			dst |= (0x01 & (src >> 7));
			src <<= 1;

			bits_remain--;
			if (bits_remain == 0)
			{
				pSrc++;
				bits_remain = 8;
				src = *pSrc;
				srclen--;
			}

		}
		pDst[j++] = dst;
	}
	return j;
}

uint32_t FrameDeSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen, uint16_t frame_type_mask)
{

	uint32_t size, err, bits_remain, i, j, byte_sync, srcRemain, bitStart, numCorrupt;
	uint8_t src, dst, *pBitStart;
	uint16_t dst16, t;
	uint8_t frame[sizeof(frame_t)], data[FRAME_NUM_BYTES_PER_FRAME];

	if (srclen == 0)
		return 0;

	if (nBitsPerSym > 8)
		return (uint32_t)-1;

	src = *pSrc;

	bits_remain = nBitsPerSym;
	bitStart = bits_remain;
	pBitStart = pSrc;
	numCorrupt = 0;

	t = (FRAME_PREAMBLE_IV << 8) | (0xFF & ~FRAME_PREAMBLE_IV);
	size = 0;
	do
	{
		j = 0;
		dst16 = 0;
		byte_sync = 0;
		do
		{
			bits_remain--;
			dst16 <<= 1;
			dst16 |= (0x01 & (src >> (bits_remain)));

			if (bits_remain == 0)
			{
				pSrc++;
				bits_remain = nBitsPerSym;
				src = *pSrc;
				srclen--;
				if (!srclen)
					break;
			}
			
			// Find frame boundaries
			if (dst16 == t)
			{
				pBitStart = pSrc;
				bitStart = bits_remain;
				srcRemain = srclen;
				byte_sync = 1;
				frame[j++] = 0xFF & (t >> 8);
				frame[j++] = 0xFF & t;

			}

		} while(srclen && !byte_sync);
		if (!srclen)
			break;

		for(j; j < sizeof(frame_t); j++)
		{
			dst = 0;
			for (i=0; i < 8; i++)
			{
				bits_remain--;
				dst <<= 1;
				dst |= (0x01 & (src >> (bits_remain)));

				if (bits_remain == 0)
				{
					pSrc++;
					bits_remain = nBitsPerSym;
					src = *pSrc;
					srclen--;
					if (!srclen)
						break;
				}

			}
			frame[j] = dst;
			if (!srclen)
				break;

		}
		if (!srclen)
			break;

		err = FrameDeformat(frame, data, sizeof(frame_t), frame_type_mask);
		if (err > 0)
		{
			memcpy(&pDst[size], frame, sizeof(frame_t));
			size += sizeof(frame_t);

		}
		else
		{
			if (size > 0)
				numCorrupt++;
			bits_remain = bitStart;
			pSrc = pBitStart;
			srclen = srcRemain;
		}

	} while(srclen);

//	printf("Number of corrupted frames = %d\n", numCorrupt);	
	return size;
}

// Differential M-PSK symbol mapping
uint32_t FrameSymbolsMap(uint8_t *pBits, cpx_t *pSym, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
	uint32_t i;
	sym_map_t sym_mapper;

	if (num_syms == 0)
		return 0;

	SymMapInit(&sym_mapper, nBitsPerSym, type);

	for(i=0; i < num_syms; i++)
	{
		pSym[i] = SymMapEncode(&sym_mapper, pBits[i]);
	}
	
	return num_syms;
}

// Differential M-PSK symbol de-mapping
uint32_t FrameSymbolsDemap(cpx_t *pSym, uint8_t *pBits, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
	uint32_t i;
	sym_map_t sym_demapper;
	sym_err_t sym_err;

	if (num_syms == 0)
		return 0;

	memset(&sym_err, 0, sizeof(sym_err_t));

	SymMapInit(&sym_demapper, nBitsPerSym, type);

	for(i=0; i < num_syms; i++)
	{
		pBits[i] = SymMapDecode(&sym_demapper, pSym[i], &sym_err);
	}

	return num_syms;
}

// --------------------------------------------------------------