// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <stdint.h>
#include <algorithm>
#include <stdio.h>

#include <crc/crc.h>
#include "Formatter.hpp"

// --------------------------------------------------------------
// 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.preamble.data[0] = FRAME_PREAMBLE_IV;
	m_frame.preamble.data[1] = ~(m_frame.preamble.data[0]);
}

Formatter::~Formatter()
{
}

void Formatter::process_data(uint8_t *pData, uint32_t len)
{
	if (m_idleCount)
	{
		// Idle in progress: Commit partial IDLE frame
		commit(FRAME_TYPE_IDLE, m_idleCount);
		m_idleCount = 0;
	}

	size_t remain = len;
	while(remain)
	{
		size_t frame_remain = FRAME_NUM_BYTES_PER_FRAME - m_dataCount;
		size_t to_write = std::min(remain, frame_remain);
		memcpy(&m_frame.data[m_dataCount], pData, to_write);
		m_dataCount += to_write;
		remain -= to_write;
		pData += to_write;

		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_idle(uint32_t len)
{
	if (m_dataCount)
	{
		// Commit partial DATA frame
		commit(FRAME_TYPE_DATA, m_dataCount);
		m_dataCount = 0;
	}

	size_t remain = len;
	while(remain)
	{
		size_t frame_remain = FRAME_NUM_BYTES_PER_FRAME - m_dataCount;
		size_t to_write = std::min(remain, frame_remain);
		memset(&m_frame.data[m_dataCount], 0, to_write);
		m_dataCount += to_write;
		remain -= to_write;

		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(void *pData, uint32_t len)
{
	if (pData)
	{
		// Data available: Process data
		process_data((uint8_t*)pData, len);	
	}
	else
	{
		// Data not available: Process Idle
		process_idle(len);
	}
}

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((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_scrambler.process(m_frame.data, FRAME_NUM_BYTES_PER_FRAME);

	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_state(Idle)
{
	resetStats();
}

DeFormatter::~DeFormatter()
{
}

void DeFormatter::process(uint8_t data)
{
	if (m_state == Idle)
	{
		return;
	}

	uint8_t *pData;
	if (m_state == Header)
	{
		pData = (uint8_t*)&m_Hdr;
		pData[m_dataCount++] = data;
		if (m_dataCount == (sizeof(frame_hdr_t)))
		{
			m_scrambler.stateSet(m_Hdr.prn_state);
			m_scrambler.process((uint8_t*)&m_Hdr.crc16, sizeof(m_Hdr.crc16));
			m_scrambler.process((uint8_t*)&m_Hdr.type, sizeof(m_Hdr.type));
			m_scrambler.process((uint8_t*)&m_Hdr.length, sizeof(m_Hdr.length));
			m_dataCount = 0; 
			m_state = Data;
		}
	}
	else if (m_state == Data)
	{
		pData = (uint8_t*)&m_data;
		pData[m_dataCount++] = data;
		if (m_dataCount == m_Hdr.length)
		{
			m_scrambler.process(m_data, FRAME_NUM_BYTES_PER_FRAME);
			uint16_t crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, m_data, FRAME_NUM_BYTES_PER_FRAME);
			if (crc16_ist == m_Hdr.crc16)
			{
				// Frame is valid
				if (m_Hdr.type == FRAME_TYPE_DATA)
				{
					m_frameStats.numDataBytes += m_Hdr.length;
					m_pDataHandler->onData(m_data, m_Hdr.length);
				}
			}
			m_frameStats.numDataFrame += (uint32_t)(m_Hdr.type == FRAME_TYPE_DATA);
			m_frameStats.numIdleFrame += (uint32_t)(m_Hdr.type == FRAME_TYPE_IDLE);
			m_frameStats.numInvalidFrame += (uint32_t)(crc16_ist != m_Hdr.crc16);
			m_frameStats.numTotalFrame++;
			m_dataCount = 0;
			m_state = Idle;
		}
	}
}

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_state = Header;
};

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

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