#include "StdAfx.h"
#include "Compass.h"

const string Compass::m_sPrefix = "PTNTHPR";

Compass::Compass(void)
{
	m_sInputBuffer.reserve(COMPASS_STRINIG_BUFFER_SIZE);
	m_bNewLine = false;
	m_bValid = false;
	m_nFlag = -1;
}

Compass::~Compass(void)
{
}

#include <SYS\TIMEB.H>

void Compass::OnReceiveChar(const char ch)
{
	switch (ch)
	{
	case '$':
		m_bNewLine = true;
		m_nFlag = 0;
		break;
	case '*':
		if (m_bValid)
		{
			AnalysePTNTHPR(m_sInputBuffer);
			m_bValid = false;
		}
		m_sInputBuffer = "";
		break;
	case 13:	//cr
		break;
	case 10:	//lf
		break;
	default:
		if (m_bNewLine)
		{
			if (m_bValid)
			{
				m_sInputBuffer += ch;
			}
			else
			{
				if (m_nFlag != -1)
				{
					m_sInputBuffer += ch;
					++m_nFlag;

					if (m_nFlag == m_sPrefix.size())
					{
						if (m_sPrefix == m_sInputBuffer)
						{
							m_bValid = true;
						}
						else
						{
							m_nFlag = -1;
						}
					}
				}
			}
		}
		break;
	}
}

void Compass::AnalysePTNTHPR(const string& str)
{
	assert(!str.empty());

	HPRDATA data = {0};
	const char comma = ',';
	size_t start = 0, end = 0;

	////output for test
	//end = str.find_first_of(comma, start);
	//cout << str.substr(start, end - start) << endl;
	//start =  end + 1;

	//PTNHPR
	end = str.find_first_of(comma, start);
	start =  end + 1;

	end = str.find_first_of(comma, start);
	data.fHeading = (float)atof(str.substr(start, end - start).c_str());
	start =  end + 1;

	end = str.find_first_of(comma, start);
	assert((end - start) == 1);
	data.szHeadStatus = str[start];
	start =  end + 1;

	end = str.find_first_of(comma, start);
	data.fPitch = (float)atof(str.substr(start, end - start).c_str());
	start =  end + 1;

	end = str.find_first_of(comma, start);
	assert((end - start) == 1);
	data.szPitchStatus = str[start];
	start =  end + 1;

	end = str.find_first_of(comma, start);
	data.fRoll = (float)atof(str.substr(start, end - start).c_str());
	start =  end + 1;

	data.szRollStatus = str[start];

	UpdateDataBuffer(data);
}
void Compass::OnReceiveCh(const char ch)
{
	switch (ch)
	{
	case '#':
		m_bNewLine = true;
		m_bValid = true;
		m_nFlag = 0;
		break;
	case '*':
		if (m_bValid)
		{
			AnalyseCOUNT(m_sInputBuffer);
			m_bValid = false;
		}
		m_sInputBuffer = "";
		break;
	case 13:	//cr
		break;
	case 10:	//lf
		break;
	default:
		if (m_bNewLine)
		{
			if (m_bValid)
				m_sInputBuffer += ch;
		}
			
	}
}

void Compass::AnalyseCOUNT(const string& str)
{
	assert(!str.empty());

	HPRDATA data = {0};
	
    data.szHeadStatus = str[0];
	data.szPitchStatus = str[1];
	data.szRollStatus = str[2];
//	data.szHeadStatus = (char)atoi(str.c_str());

	UpdateDataBuffer(data);
}