#ifdef _WINDOWS
#include "StdAfx.h"
#endif

#include "SlipSerial.h"

#include <iomanip>

enum SlipState{
	SlipNormal,
	SlipEscaped,
	SlipError,
	SlipStopped
};

#define SLIP_END 192
#define SLIP_ESC 219
#define SLIP_ESCAPED_END 220
#define SLIP_ESCAPED_ESC 221


CSlipSerial::CSlipSerial(char* port, int bitrate)
: CSerial(port,bitrate)
{
}

CSlipSerial::CSlipSerial()
: CSerial()
{
}

CSlipSerial::~CSlipSerial(void)
{
}


std::vector<unsigned char> CSlipSerial::readFrame()
{
	std::vector<unsigned char> buf;

	int state = SlipNormal;

	while ( /*bytesReady() >0 &&*/ state != SlipStopped)
	{
		int c = readByte();
		if (c == -1) 
		{
			Sleep(1);
			continue;
		}
		unsigned char data = (unsigned char) c;
		switch (state)
		{
			case SlipNormal:
				if (data == SLIP_END)
					state = SlipStopped;
				else if (data == SLIP_ESC)
					state = SlipEscaped;
				else
					buf.push_back(data);
				break;
			case SlipEscaped:
				if (data == SLIP_ESCAPED_END)
					buf.push_back(SLIP_END);
				else if (data == SLIP_ESCAPED_ESC)
					buf.push_back(SLIP_ESC);
				else
					state = SlipError;
				break;
			default: //empty default case
				break;
		}
	}

	if (state != SlipStopped || state == SlipError)  //in case we ran out of data before we recieved af SLIP_END character or there was an error
	{
		buf.clear();
	}

	return buf;
}

void CSlipSerial::writeFrame(std::vector<unsigned char> data)
{
	for (int i=0; i<data.size(); i++)
	{
		writeSlipByte( data[i] );
	}
	//send the frame termination byte
	writeByte(SLIP_END);

}

void CSlipSerial::writeFrame(unsigned char buf[], int len)
{
	for (int i=0; i<len; i++)
	{
		writeSlipByte( buf[i] );
	}
	writeByte(SLIP_END);
}

void CSlipSerial::writeSlipByte(unsigned char out)
{
	if (out == SLIP_ESC)
	{
		writeByte(SLIP_ESC);
		writeByte(SLIP_ESCAPED_ESC);
	}
	else if (out == SLIP_END)
	{
		writeByte(SLIP_ESC);
		writeByte(SLIP_ESCAPED_END);
	}
	else
		writeByte( out );
}