// --------------------------------------------------------------
#include <string.h>
#include <math.h>
#include <stdint.h>
#include "DeSymbolizer.hpp"

DeSymbolizer::DeSymbolizer()
: Processor::Block(1, 1)
, m_dataBitCount(0)
, m_lastSymbol(0)
, m_symbolMask(0)
, m_nBitsPerSym(0)
, m_state(State::Idle)
, m_sync_dist(0)
, m_sync_dist_last(0)
, m_sync_counter(0)
{
	setNumBitsPerSymbol(8);
}

void DeSymbolizer::setNumBitsPerSymbol(size_t nBitsPerSym)
{
	m_nBitsPerSym = nBitsPerSym;
	m_symbolMask = (1 << nBitsPerSym) - 1;
	m_dataBitCount = 0;
	m_lastSymbol = 0;
}

bool DeSymbolizer::is_preamble(uint16_t data)
{
	return 0x55AA == data;
}

void DeSymbolizer::sync(symbol_t symbol)
{
	static uint16_t word;
	word <<= m_nBitsPerSym;
	word |= symbol & m_symbolMask;

	if (m_state == State::Idle)
	{
		m_sync_counter = 0;
		m_sync_dist_last = 0;
		m_state = State::Acq;
	}
	else if (m_state == State::Acq)
	{
		if (is_preamble(word))
		{
			if (m_sync_dist_last == m_sync_dist)
			{
				m_sync_counter++;
				if (m_sync_counter == SYNC_COUNTER_RELOAD)
				{
					m_sync_counter = 0;
					// Adjust data count phase
					m_dataBitCount = m_nBitsPerSym;
					m_state = State::Trk;
				}
			}
			else
			{
				m_sync_counter = 0;
			}
			m_sync_dist_last = m_sync_dist;
			word = 0;
			m_sync_dist = 0;
		}
	}
	else if (m_state == State::Trk)
	{
		if (m_sync_dist == m_sync_dist_last)
		{
			if (not is_preamble(word))
			{			
				m_state = State::Idle;
			}
			m_sync_dist = 0;
		}
	}
	m_sync_dist++;
}

size_t DeSymbolizer::sym2bytes(symbol_t symbol, uint8_t *pBytes)
{

	static uint8_t byte;
	size_t num_produced = 0;

	if (m_dataBitCount == 0)
	{
		m_dataBitCount = 8;
	}

	// Transmission order MSB first
	for (size_t i=0; i < m_nBitsPerSym; i++)
	{
		byte <<= 1;
		byte |= (symbol_t)(symbol >> (m_nBitsPerSym-1) & 1);
		symbol <<= 1;
		m_dataBitCount--;
		if (m_dataBitCount == 0)
		{
			pBytes[num_produced++] = byte;
			m_dataBitCount = 8;
		}
	}
	return num_produced;
}

void DeSymbolizer::process(symbol_t symbol_enc)
{
	uint8_t bytes[2];
	symbol_t symbol = differentialDecode(symbol_enc);
	sync(symbol);

	// Create bytes from symbol
	size_t numBytes = sym2bytes(symbol, bytes);

	// Commit bytes if in sync
	if (m_state == State::Trk)
	{
		if (numBytes)
		{
			m_buf_out->write(bytes, numBytes);
		}
	}
}

void DeSymbolizer::prepare()
{
	// Get buffers
	m_buf_in = buffer_in<symbol_t>();
	m_buf_out = buffer_out<uint8_t>();
}

void DeSymbolizer::process()
{
	// Process input
	while(m_buf_in->len())
	{
		symbol_t sym;
		m_buf_in->read(&sym, 1);
		process(sym);
	}
}

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

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

	return res;
}

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