//--------------------------------------------------------------------------------
// <copyright file="HDLCParser.cpp" company="LiquidRobotics">
//
// Copyright (c) 2014 Liquid Robotics, Inc.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// This software is associated with a release.  Liquid Robotics makes no
// representation that the design, interfaces or component distribution will
// remain unchanged in future releases.
//
// </copyright>
//
// <summary>
//   Implementation of the HDLCParser class, which provides utilities for encoding
//   and decoding bytes in HDLC message format (used for sending messages across
//   the payload port)
// </summary>
//
// <owner>Mike Cookson</owner>
//--------------------------------------------------------------------------------

#include "HDLCParser.h"
#include "DoThrow.h"
#include <stdexcept>

using namespace std;

namespace LiquidRobotics {
namespace PayloadInterface {

const HDLCParser::byte HDLCParser::ESC = 0x7D;
const HDLCParser::byte HDLCParser::ESCMask = 0x20;
const HDLCParser::byte HDLCParser::SOF = 0x7E;

HDLCParser::HDLCParser(ByteArray& _block, ByteArray::size_type _maxSize)
: block(_block)
, inBlock(false)
, inEscape(false)
, succeeded(false)
, blockLength(0)
, maxSize(_maxSize)
{
	block.reserve(maxSize);
}

HDLCParser::~HDLCParser()
{
}

// returns the number of bytes used
HDLCParser::ByteArray::size_type HDLCParser::Feed(const ByteArray& input, ByteArray::size_type offset)
{
	// restart if already successful
	if (succeeded)
		Reset();

	const ByteArray::size_type offset0 = offset;

	while(!succeeded && (offset < input.size()))
	{
		ByteArray::byte data = input[offset++];
		bool pushB = false;

		// anytime a SOF is received, that means the start of the block; SOF itself is
		// escaped if it appears as valid block data
		if (data == SOF)
		{
			inEscape = false;
			inBlock = true;
		}

		// otherwise, if in the middle of receiving ...
		else if (inBlock)
		{
			// if a previous character started an escape sequence ...
			if (inEscape)
			{
				// this byte clears escape mode
				inEscape = false;

				// an escaped ESC character results in ESC data
				if (data == (ESC ^ ESCMask))
				{
					data = ESC;
					pushB = true;
				}

				// an escaped SOF character results in SOF data
				else if (data == (SOF ^ ESCMask))
				{
					data = SOF;
					pushB = true;
				}

				// any other data is invalid and restarts the block receive
				else
					Reset();
			}

			// otherwise (not in a block), an escape character starts an escape sequence
			else if (data == ESC)
				inEscape = true;

			// all other bytes are considered valid data
			else
				pushB = true;
		}

		// if saving data ...
		if (pushB)
		{
			// error: the block received is too long
			if (block.size() >= maxSize)
				Reset();
			else
			{
				// store the data in the result and save the size of the result so far
				block.push_back(data);

				const ByteArray::size_type resultSize = block.size();

				// if the current size is 2, then the length of the block was just received: save it
				// in blockLength, restart block receive if the length is out of range
				if (resultSize == 2)
				{
					blockLength = LengthFromBlock(block);

					if ((blockLength < 2) || (blockLength > maxSize))
						Reset();
				}

				// stop if the right number of bytes is now received
				if ((resultSize >= 2) && (resultSize == blockLength))
					succeeded = true;
			}
		}
	}

	return offset-offset0;
}

void HDLCParser::Reset()
{
	block.clear();
	block.reserve(maxSize);
	inBlock = inEscape = succeeded = false;
	blockLength = 0;
}

unsigned HDLCParser::LengthFromBlock(const ByteArray& bytes)
{
	if (bytes.size() < 2)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "bytes length must be >= 2");

	unsigned length = bytes[1];
	length <<= 8;
	length |= bytes[0];

	return length;
}


}
}
