// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <crc/crc.h>
#include <radio/symbol.h>
#include "Frame.hpp"

// --------------------------------------------------------------
// Scrambler
// --------------------------------------------------------------
Scrambler::Scrambler(uint32_t state, uint32_t poly)
	: m_state(state)
	, m_poly(poly)
{
}

Scrambler::~Scrambler()
{
}

uint32_t Scrambler::stateGet()
{
	return m_state;
}

void Scrambler::stateSet(uint32_t state)
{
	m_state = state;
}

uint8_t Scrambler::process(uint8_t src)
{
	uint32_t i;
	uint8_t res;

	res = (uint8_t)(src ^ m_state);

	for (i=0; i < 8; i++)
	{
		if (m_state & 0x0001)
		{
			m_state = (m_state >> 1) ^ m_poly;
		}
		else
		{
			m_state = (m_state >> 1);
		}
	}
	return res;
}

void Scrambler::process(uint8_t *pSrcDst, uint32_t len)
{
	while(len--)
	{
		*pSrcDst = process(*pSrcDst);
		pSrcDst++;
	}
}

// --------------------------------------------------------------
// Formatter
// --------------------------------------------------------------
Formatter::Formatter(IFormatter *pFrameHandler)
	: m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
	, m_pFrameHandler(pFrameHandler)
	, m_dataCount(0)
	, m_idleCount(0)
{
	resetStats();
	memset(&m_frame, 0, sizeof(frame_t));

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

Formatter::~Formatter()
{
}

void Formatter::idle()
{
	if (m_dataCount)
	{
		// Commit partial DATA frame
		commit(FRAME_TYPE_DATA, m_dataCount);
		m_dataCount = 0;
	}
	else
	{
		// IDLE frame not full
		if (m_idleCount < FRAME_NUM_BYTES_PER_FRAME)
		{
			m_frame.data[m_idleCount++] = 0;
		}

		if (m_idleCount == FRAME_NUM_BYTES_PER_FRAME)
		{
			// Commit full IDLE frame of size FRAME_NUM_BYTES_PER_FRAME
			commit(FRAME_TYPE_IDLE, m_idleCount);
			m_idleCount = 0;
		}
	}
}

void Formatter::process(uint8_t data)
{
	if (m_idleCount)
	{
		// Commit partial IDLE frame
		commit(FRAME_TYPE_IDLE, m_idleCount);
		m_idleCount = 0;
	}
	else
	{
		// DATA frame not full
		if (m_dataCount < FRAME_NUM_BYTES_PER_FRAME)
		{
			m_frame.data[m_dataCount++] = data;
		}

		if (m_dataCount == FRAME_NUM_BYTES_PER_FRAME)
		{
			// Commit full DATA frame of size FRAME_NUM_BYTES_PER_FRAME
			commit(FRAME_TYPE_DATA, m_dataCount);
			m_dataCount = 0;
		}
	}
}

void Formatter::process(void *pData, uint32_t len)
{
	uint8_t *pByte;

	pByte = (uint8_t*)pData;
	
	while(len--)
	{
		process(*(pByte++));
	}
}

frame_statistics_t Formatter::getStats()
{
	return m_frameStats;
}

void Formatter::resetStats()
{
	memset(&m_frameStats, 0, sizeof(frame_statistics_t));
}

void Formatter::commit(frame_type frameType, uint32_t dataCount)
{
	// Commit frame
	m_frame.hdr.length = (uint16_t)dataCount;
	m_frame.hdr.type = (uint16_t)frameType;
	m_frame.hdr.crc16 = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
	m_frame.hdr.prn_state = m_scrambler.stateGet();

	m_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
	m_scrambler.process((uint8_t*)&m_frame.hdr.crc16, sizeof(m_frame.hdr.crc16));
	m_scrambler.process((uint8_t*)&m_frame.hdr.type, sizeof(m_frame.hdr.type));
	m_scrambler.process((uint8_t*)&m_frame.hdr.length, sizeof(m_frame.hdr.length));

	m_pFrameHandler->onFrame((void*)&m_frame, sizeof(frame_t));
}

// --------------------------------------------------------------
// Deformatter
// --------------------------------------------------------------
DeFormatter::DeFormatter(IDeFormatter *pDataHandler)
	: m_pDataHandler(pDataHandler)
	, m_scrambler(FRAME_SCRAMBLER_SEED, FRAME_SCRAMBLER_POLY)
	, m_dataCount(0)
	, m_enable(0)
{
	resetStats();
}

DeFormatter::~DeFormatter()
{
}

void DeFormatter::process(uint8_t data)
{
	uint8_t *pData;

	pData = (uint8_t*)&m_frame;
	pData += 2;
	
	if (!m_enable)
		return;

	pData[m_dataCount++] = data;

	if (m_dataCount == (sizeof(frame_t)-2))
	{
		m_scrambler.stateSet(m_frame.hdr.prn_state);
		m_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
		m_scrambler.process((uint8_t*)&m_frame.hdr.crc16, sizeof(m_frame.hdr.crc16));
		m_scrambler.process((uint8_t*)&m_frame.hdr.type, sizeof(m_frame.hdr.type));
		m_scrambler.process((uint8_t*)&m_frame.hdr.length, sizeof(m_frame.hdr.length));
		uint16_t crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_frame.data, FRAME_NUM_BYTES_PER_FRAME);
		if (crc16_ist == m_frame.hdr.crc16)
		{
			// Frame is valid
			if (m_frame.hdr.type == FRAME_TYPE_DATA)
			{
				m_frameStats.numDataBytes += m_frame.hdr.length;
				m_pDataHandler->onData(m_frame.data, m_frame.hdr.length);
			}
		}
		m_frameStats.numDataFrame += (uint32_t)(m_frame.hdr.type == FRAME_TYPE_DATA);
		m_frameStats.numIdleFrame += (uint32_t)(m_frame.hdr.type == FRAME_TYPE_IDLE);
		m_frameStats.numInvalidFrame += (uint32_t)(crc16_ist != m_frame.hdr.crc16);
		m_frameStats.numTotalFrame++;
		m_dataCount = 0;
		m_enable = 0;
	}
}

frame_statistics_t DeFormatter::getStats()
{
	return m_frameStats;
}

void DeFormatter::resetStats()
{
	memset(&m_frameStats, 0, sizeof(frame_statistics_t));
}

void DeFormatter::onReset()
{
	m_dataCount = 0;
	m_enable = 1;
};

void DeFormatter::onData(data_t data)
{
	process((uint8_t)data);
}

Symbolizer::Symbolizer(ISymbolizer *pSymbolHandler)
	: m_pSymbolHandler(pSymbolHandler)
	, m_symbolBitCount(0)
	, m_lastSymbol(0)
	, m_symbolMask(0)
{
	setNumBitsPerSymbol(8);
}

Symbolizer::~Symbolizer()
{
}

void Symbolizer::setNumBitsPerSymbol(uint32_t nBitsPerSym)
{
	m_symbolBitCount = 0;
	m_nBitsPerSym = nBitsPerSym;
	m_lastSymbol = 0;
	m_symbolMask = 0;
	for(uint32_t k=0; k < nBitsPerSym; k++)
	{
		m_symbolMask <<= 1;
		m_symbolMask |= 1;
	}
}

void Symbolizer::process(data_t src)
{
	uint32_t i;
	static symbol_t dst;

	// Speed up
	if (m_nBitsPerSym == SIZEOF_DATA_BITS)
	{
		m_pSymbolHandler->onSymbol(differentialEncode((symbol_t)src));
		return;
	}

	// Transmission order MSB first
	for (i=0; i < SIZEOF_DATA_BITS; i++)
	{
		dst <<= 1;
		dst |= (symbol_t)((src >> (SIZEOF_DATA_BITS-1) & 1) != 0);
		src <<= 1;
		m_symbolBitCount++;
		if (m_symbolBitCount == m_nBitsPerSym)
		{
			m_pSymbolHandler->onSymbol(differentialEncode(dst));
			m_symbolBitCount = 0;
			dst = 0;
		}
	}
}

void Symbolizer::process(void *pSrc, uint32_t len)
{
	data_t *pData;

	pData = (data_t*)pSrc;
	
	while(len--)
	{
		process(*(pData++));
	}
}

symbol_t Symbolizer::differentialEncode(symbol_t symbol)
{
	// Differential encode
	m_lastSymbol = (symbol_t)(m_symbolMask & (symbol + m_lastSymbol));

	return m_lastSymbol;
}

void Symbolizer::onFrame(void *pFrame, uint32_t size)
{
	data_t *pData = (data_t*)pFrame;

	while(size)
	{
		process(*(pData++));
		size -= sizeof(data_t);
	}
}

DeSymbolizer::DeSymbolizer(IData *pDataHandler)
	: m_pDataHandler(pDataHandler)
	, m_dataBitCount(0)
	, m_lastSymbol(0)
	, m_symbolMask(0)
	, m_nBitsPerSym(0)
{
	setNumBitsPerSymbol(8);
}

DeSymbolizer::~DeSymbolizer()
{
}

void DeSymbolizer::setNumBitsPerSymbol(uint32_t nBitsPerSym)
{
	m_nBitsPerSym = nBitsPerSym;
	m_symbolMask = 0;
	for(uint32_t k=0; k < nBitsPerSym; k++)
	{
		m_symbolMask <<= 1;
		m_symbolMask |= 1;
	}
	m_bitCount = 0;
	m_dataBitCount = 0;
	m_lastSymbol = 0;
}

void DeSymbolizer::process(symbol_t src)
{
	uint32_t i, j;
	static data_t dst;
	static uint16_t preamble;
	uint16_t preamble_soll;
	symbol_t symSync;
	const uint32_t frameLengthInBits = 8*sizeof(frame_t);
	src = differentialDecode(src);
	symSync = src;

	preamble_soll = ((uint16_t)FRAME_PREAMBLE_IV << 8) | (~(uint16_t)FRAME_PREAMBLE_IV & 0xFF);

	j = 0;
	for (i=0; i < m_nBitsPerSym; i++)
	{
		preamble <<= 1;
		preamble |= (symbol_t)((symSync >> (m_nBitsPerSym-1) & 1) != 0);
		symSync <<= 1;
		m_bitCount++;
		if (preamble == preamble_soll)
		{
			preamble = 0;
			// We have data sync
			if (m_bitCount == frameLengthInBits)
			{
				m_dataBitCount = 0;
				m_pDataHandler->onReset();
				j = i + 1;
				src = symSync;
			}
			if (m_bitCount >= frameLengthInBits)
			{
				m_bitCount = 0;
			}
		}
	}

	// Transmission order MSB first
	for (i=j; i < m_nBitsPerSym; i++)
	{
		dst <<= 1;
		dst |= (symbol_t)((src >> (m_nBitsPerSym-1) & 1) != 0);
		src <<= 1;
		m_dataBitCount++;
		if (m_dataBitCount == SIZEOF_DATA_BITS)
		{
			m_pDataHandler->onData(dst);
			m_dataBitCount = 0;
			dst = 0;
		}
	}
}

symbol_t DeSymbolizer::differentialDecode(symbol_t symbol)
{
	symbol_t res;

	// Differential decode
	res = (symbol_t)(m_symbolMask & (symbol - m_lastSymbol));
	m_lastSymbol = symbol;

	return res;
}

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