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

#define DESYMBOLIZER_DEBUG printf

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 = 0;
	for(size_t k=0; k < nBitsPerSym; k++)
	{
		m_symbolMask <<= 1;
		m_symbolMask |= 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;
		DESYMBOLIZER_DEBUG("Idle -> ACQ\n");
		m_state = State::Acq;
	}
	else if (m_state == State::Acq)
	{
		if (is_preamble(word))
		{
			DESYMBOLIZER_DEBUG("Acquired bit sync. Last bit sync was %zu symbols ago\n", m_sync_dist);
			if (m_sync_dist_last == m_sync_dist)
			{
				m_sync_counter++;
				DESYMBOLIZER_DEBUG("Received sync at %zu %zu times -> OK\n", m_sync_dist, m_sync_counter);
				if (m_sync_counter == SYNC_COUNTER_RELOAD)
				{
					m_sync_counter = 0;
					m_dataBitCount = 0;
					DESYMBOLIZER_DEBUG("ACQ -> TRK\n");
					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))
			{			
				DESYMBOLIZER_DEBUG("Missed sync at %zu. Found %04X instead -> NOK\n", m_sync_dist, word);
				DESYMBOLIZER_DEBUG("TRK -> Idle\n");
				m_state = State::Idle;
			}
			else
			{
//				printf("We tracked sync at %zu -> OK\n", m_sync_dist);
			}
			m_sync_dist = 0;
		}
		else
		{
			if (is_preamble(word))
			{			
				DESYMBOLIZER_DEBUG("Found sync at %zu -> IGN\n", m_sync_dist);
			}
		}
	}
	m_sync_dist++;
}

uint8_t DeSymbolizer::sym2byte(symbol_t symbol)
{
	static uint8_t byte;
	byte <<= m_nBitsPerSym;
	byte |= symbol & m_symbolMask;

	return byte;
}

void DeSymbolizer::process(symbol_t symbol_enc)
{

	symbol_t symbol = differentialDecode(symbol_enc);
	sync(symbol);

	// Create bytes from symbol
	uint8_t byte = sym2byte(symbol);

	// Commit bytes if in sync
	if (m_state == State::Trk)
	{
		if (m_dataBitCount == 0)
		{
//			printf("Write %02X\n", byte);
			m_buf_out->write(&byte, 1);
		}
		m_dataBitCount = (m_dataBitCount + m_nBitsPerSym) & 7;
	}
}

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);
	}
}

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

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

	return res;
}

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