/* $Id$ */

#include <cmath>
#include <cassert>
#include <cstring>
#include "fsk.h"
#include "util.h"

CFskEncoder::CFskEncoder(): m_index(0), m_angle(0.0), m_generateLeadIn(true), m_generateLeadOut(false)
{
	EnqueuePacket(TYPE_START, NULL, 0);
}

void CFskEncoder::Feed(const char *data, unsigned size)
{
	EnqueuePacket(TYPE_DATA, data, size);
}

void CFskEncoder::Terminate()
{
	EnqueuePacket(TYPE_END, NULL, 0);
	m_generateLeadOut = true;
}

void CFskEncoder::EnqueuePacket(EType type, const char *data, unsigned size)
{
	assert(size <= 255);

	struct SHeader
	{
		unsigned char sync;
		unsigned char index;
		unsigned char type;
		unsigned char size;
		unsigned char crclo;
		unsigned char crchi;
	} __attribute__((packed));

	SHeader hdr;

	hdr.sync = 'S';
	hdr.index = m_index++;
	hdr.type = type;
	hdr.size = size;
	hdr.crclo = 0;
	hdr.crchi = 0;

	std::vector<unsigned char> v;
	v.resize(sizeof(hdr) + size);
	memcpy(&v[0], &hdr, sizeof(hdr));

	if (size)
		memcpy(&v[sizeof(hdr)], data, size);

	unsigned short crc(Util::Crc16(&v[0], v.size()));
	hdr.crclo = crc & 0xFF;
	hdr.crchi = crc >> 8;
	memcpy(&v[0], &hdr, sizeof(hdr));

	const unsigned sz(m_data.size());
	m_data.resize(sz + v.size());
	memcpy(&m_data[sz], &v[0], v.size());
}

void CFskEncoder::Write(FILE *fp)
{
	if (m_generateLeadIn)
	{
		GenerateLead(fp);
		m_generateLeadIn = false;
	}

	for (std::vector<unsigned char>::const_iterator it(m_data.begin()); it != m_data.end(); ++it)
	{
		bool parity(false);

		for (int i(0); i < 8; ++i)
		{
			if (*it & (1 << i))
			{
				parity = !parity;
				GenerateTone(fp, false);
				GenerateTone(fp, true);
			}
			else
			{
				GenerateTone(fp, true);
				GenerateTone(fp, false);
			}
		}

		GenerateTone(fp, !parity);
		GenerateTone(fp, parity);
	}

	if (m_generateLeadOut)
	{
		GenerateLead(fp);
		m_generateLeadOut = false;
	}

	m_data.clear();
}

void CFskEncoder::GenerateLead(FILE *fp)
{
	for (int i(0); i < 1200; ++i)
		GenerateTone(fp, true);
}

void CFskEncoder::GenerateTone(FILE *fp, bool isMark)
{
	const double phaseChange(2 * M_PI / (44100. / (isMark ? 1200. : 2200.)));

	// 44100 / 1200 = 36.75
	for (int i(0); i < 37; ++i)
	{
		// 0 - 65534 actually
		const signed value(sin(m_angle) * 32767. + 32767);
		char buf[2];

		buf[0] = value & 0xFF;
		buf[1] = value >> 8;

		fwrite(buf, sizeof(buf), 1, fp);

		// angle should be that 
		m_angle += phaseChange;

		if (m_angle > 2 * M_PI)
			m_angle -= 2 * M_PI;
	}
}
