//------------------------------------------------------------------------------------
// <copyright file="FCCMessage_CommHeaderMessageGeneric.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 CommHeaderMessageGeneric class, which represents a message
//   based on a COMM_HEADER, and which provides methods streaming the data.
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------------

#include "FCCMessage_CommHeaderMessageGeneric.h"
#include "DoThrow.h"
#include "LqrSMACPrintfBuffer.h"
#include "nsextend.h"
#include <sstream>
#include <boost/foreach.hpp>
#include <string.h>

using namespace std;
using namespace LiquidRobotics::SMACFramework;

//#define TEST_CRC

template<typename T>
void AppendSimpleData(ByteArray& ba, const T& t)
{
	ba.insert(ba.end(), (const ByteArray::byte*) &t, sizeof(t));
}

namespace LiquidRobotics {
namespace PayloadInterface {
namespace FCCMessage {

IMPLEMENT_INNER_EXCEPTION(CommHeaderMessageGeneric, BadCRC, runtime_error);
IMPLEMENT_INNER_EXCEPTION(CommHeaderMessageGeneric, NotACommand, runtime_error);


CommHeaderMessageGeneric::CommHeaderMessageGeneric(unsigned _simpleTypeSize, bool _withCRC, unsigned extraBytes)
: simpleTypeSize(_simpleTypeSize-(_withCRC ? sizeof(CRCType) : 0))
, withCRC(_withCRC)
, isFSReport(false)
{
	data.resize(BaseSize(extraBytes));
	Zero();
}

CommHeaderMessageGeneric::CommHeaderMessageGeneric(unsigned _simpleTypeSize, bool _withCRC, const ByteArray& byteData, unsigned offset)
: simpleTypeSize(_simpleTypeSize-(_withCRC ? sizeof(CRCType) : 0))
, withCRC(_withCRC)
, isFSReport(false)
{
	FromArray(byteData, offset);
}

CommHeaderMessageGeneric::CommHeaderMessageGeneric(unsigned _simpleTypeSize, bool _withCRC, const byte* pData, unsigned dataLength)
: simpleTypeSize(_simpleTypeSize-(_withCRC ? sizeof(CRCType) : 0))
, withCRC(_withCRC)
, isFSReport(false)
{
	FromArray(pData, dataLength);
}

CommHeaderMessageGeneric::CommHeaderMessageGeneric(unsigned _simpleTypeSize, bool _withCRC, uint8_t _type, uint32_t _packetID, const CommHeaderConversionBase& _data)
: simpleTypeSize(_simpleTypeSize-(_withCRC ? sizeof(CRCType) : 0))
, withCRC(_withCRC)
, isFSReport(false)
{
	pConversionBaseData.reset(_data.Duplicate());
	_data.ToArray(data);

	header.type = _type;
	header.packetID = _packetID;
	header.size = data.size();
}

CommHeaderMessageGeneric::CommHeaderMessageGeneric(unsigned _simpleTypeSize, bool _withCRC, uint8_t _type, uint32_t _packetID, const void* _pData, unsigned extraBytes)
: simpleTypeSize(_simpleTypeSize-(_withCRC ? sizeof(CRCType) : 0))
, withCRC(_withCRC)
, isFSReport(false)
{
	if (!_pData)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "_pData is NULL");

	header.type = _type;
	header.packetID = _packetID;
	header.size = simpleTypeSize +extraBytes;

	data.resize(header.size);
	ZeroData();

	if (header.size)
		memcpy(&data.at(0), _pData, header.size);
}

CommHeaderMessageGeneric::CommHeaderMessageGeneric(const CommHeaderMessageGeneric& right)
: simpleTypeSize(right.simpleTypeSize)
, withCRC(right.withCRC)
, isFSReport(right.isFSReport)
{
	memcpy(&header, &right.header, sizeof(header));

	if (right.pConversionBaseData)
	{
		pConversionBaseData.reset(right.pConversionBaseData->Duplicate());
		pConversionBaseData->ToArray(data);
	}
	else
	{
		data.resize(right.data.size());

		if (data.size())
			std::copy(right.data.begin(), right.data.end(), data.begin());
	}
}

CommHeaderMessageGeneric::~CommHeaderMessageGeneric()
{
}

unsigned CommHeaderMessageGeneric::BaseSize(unsigned extraBytes) const
{
	return simpleTypeSize + extraBytes;
}

unsigned CommHeaderMessageGeneric::HeaderSize() const
{
	return isFSReport ? sizeof(header.type) : sizeof(header);
}

uint8_t CommHeaderMessageGeneric::ContainedType(const byte* pData, unsigned dataLength)
{
	return ContainedHeader(pData, dataLength).type;
}

FCCMessage::COMM_HEADER CommHeaderMessageGeneric::ContainedHeader(const byte* pData, unsigned dataLength)
{
	if (!pData)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "pData is NULL");

	if (dataLength < sizeof(COMM_HEADER))
		DoThrow<invalid_argument>(__FILE__, __LINE__, "byte array is too short to contain a header");

	return (const COMM_HEADER&) *pData;
}

FCCMessage::CMD_FMT_COMMON CommHeaderMessageGeneric::ContainedCmd(const byte* pData, unsigned dataLength)
{
	if (!pData)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "pData is NULL");

	if (dataLength < (sizeof(COMM_HEADER)+sizeof(CMD_FMT_COMMON)))
		DoThrow<NotACommand>(__FILE__, __LINE__, "byte array is too short to contain a command");

	const COMM_HEADER& header = (const COMM_HEADER&) pData[0];
	const CMD_FMT_COMMON& cmdFormat = (const CMD_FMT_COMMON&) pData[sizeof(COMM_HEADER)];

	if (header.type != FCCMessage::RCV_CMD)
		DoThrow<NotACommand>(__FILE__, __LINE__, "the packet type does not indicate a command");

	return cmdFormat;
}

CommHeaderConversionBase* CommHeaderMessageGeneric::Duplicate() const
{
	return new CommHeaderMessageGeneric(*this);
}

void CommHeaderMessageGeneric::FromArray(const ByteArray& _data, unsigned offset)
{
	if (offset >= _data.size())
		DoThrow<invalid_argument>(__FILE__, __LINE__, "offset points past the end of the array");

	FromArray(&_data.at(offset), _data.size()-offset);
}

void CommHeaderMessageGeneric::FromArray(const byte* _pData, unsigned dataLength)
{
	if (!_pData)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "pData is NULL");

	const unsigned zeroBaseSize = BaseSize(0);
	const unsigned headerSize = HeaderSize();

	if ((headerSize+zeroBaseSize+sizeof(CRCType)) > dataLength)
		DoThrow<invalid_argument>(__FILE__, __LINE__, "byte array does not contain data to initialize type");

	memcpy(&header, _pData, headerSize);

	if (isFSReport)
		data.resize(dataLength-headerSize);
	else
	{
		if ((header.size+sizeof(CRCType)) < simpleTypeSize)
			DoThrow<invalid_argument>(__FILE__, __LINE__, "the header indicates a size to small for the type");

		if ((header.size+sizeof(CRCType)) > dataLength)
			DoThrow<invalid_argument>(__FILE__, __LINE__, "the header indicates a size larger than the byte array");

		data.resize(header.size);
	}

	if (data.size())
		memcpy(&data.at(0), &_pData[headerSize], data.size());

	const CRCType crc = *((const uint16_t*) &_pData[isFSReport ? (dataLength-sizeof(CRCType)) : (sizeof(header)+header.size)]);
	const CRCType computedCRC = ComputeCRC();

	if (crc != computedCRC)
	{
		stringstream msg;
		msg << "CRC in the message (0x" << utos(crc, 16, 4) << ") does not match computed CRC (0x" << utos(computedCRC, 16, 4) << ")";
		string msg2 = msg.str();
		DoThrow<BadCRC>(__FILE__, __LINE__, msg2);
	}
}

void CommHeaderMessageGeneric::ToArray(ByteArray& result, bool includeCRC) const
{
	result.clear();

	ByteArray newData;

	if (pConversionBaseData)
		pConversionBaseData->ToArray(newData);

	const unsigned headerSize = HeaderSize();

	result.reserve(headerSize+data.size()+(includeCRC ? sizeof(CRCType) : 0));
	result.insert_type(result.end(), header, headerSize);

	if (!isFSReport)
		result.as<COMM_HEADER>().size = data.size();

	if (data.size())
		result.insert(result.end(), data.begin(), data.end());

	if (includeCRC)
	{
		CRC16 crc;
		crc.Add(data);

		CRCType crcVal = crc.Get();
		result.insert_type(result.end(), crcVal);
	}
}

ostream& CommHeaderMessageGeneric::ToStream(ostream& out) const
{
	ByteArray temp;
	const ByteArray* pData;

	if (pConversionBaseData)
	{
		pConversionBaseData->ToArray(temp);
		pData = &temp;
	}
	else
		pData = &data;

	if (isFSReport)
	{
		out << "type={ ";
		PacketType_ToStream(out, header.type);
		out << " }";
	}
	else
		out << "header={ " << header << " }";

	out << " data=" << pData->size() << " bytes { " << *pData << " }"
	    << " simpleTypeSize=" << simpleTypeSize
	    << " withCRC=" << (withCRC ? "true" : "false");

	return out;
}

string CommHeaderMessageGeneric::ToString() const
{
	stringstream stm;

	stm << *this;
	return stm.str();
}

CommHeaderMessageGeneric::CRCType CommHeaderMessageGeneric::ComputeCRC() const
{
	CRC16 crc;
	ByteArray temp;

	if (pConversionBaseData)
	{
		COMM_HEADER tempHeader = header;

		pConversionBaseData->ToArray(temp);

		if (isFSReport)
			AppendSimpleData(temp, tempHeader.type);
		else
		{
			tempHeader.size = temp.size();
			AppendSimpleData(temp, tempHeader);
		}
	}
	else
	{
		temp.reserve(sizeof(header)+data.size());

		if (isFSReport)
			AppendSimpleData(temp, header.type);
		else
			AppendSimpleData(temp, header);

		temp.insert(temp.end(), data.begin(), data.end());
	}

#ifdef TEST_CRC
	cout << "Data to CRC:" << endl
	     << temp.ToStringSplitView() << endl;
#endif

	crc.Add(temp);
	const CRCType result = crc.Get();

#ifdef TEST_CRC
	cout << "Result = " << ultos(result, 16, 2*sizeof(result)) << endl;
#endif

	return result;
}

void CommHeaderMessageGeneric::ZeroHeader()
{
	memset(&header, '\0', sizeof(header));
}

void CommHeaderMessageGeneric::ZeroData()
{
	if (data.size())
		memset(&data.at(0), '\0', data.size());
}

void CommHeaderMessageGeneric::Zero()
{
	ZeroHeader();
	ZeroData();
}

std::ostream& operator<<(std::ostream& out, const CommHeaderMessageGeneric& message)
{
	return message.ToStream(out);
}

}
}
}
