//------------------------------------------------------------------------------
// <copyright file="PayloadMessage.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>
// </summary>
//
// <owner></owner>
//------------------------------------------------------------------------------


#include "PayloadMessage.h"
#include "LqrSMACException.h"
#include "LqrSMACPrintfBuffer.h"
#include "nsextend.h"
#include <iostream>
#include <sstream>
#include <boost/foreach.hpp>

using namespace std;
using namespace LiquidRobotics::SMACFramework;

namespace LiquidRobotics {
namespace PayloadInterface {

PayloadMessage::PayloadMessage()
{
	Update();
}

PayloadMessage::PayloadMessage(MessageType messageType, BoardAddress sourceAddress, BoardAddress destinationAddress, TransactionID transactionID, const ByteArray& data)
: header(messageType, sourceAddress, destinationAddress, transactionID, 0)
{
	SetData(data);
}

PayloadMessage::PayloadMessage(MessageType messageType, BoardAddress sourceAddress, BoardAddress destinationAddress, TransactionID transactionID, const byte* pData, MessageLength dataLength)
: header(messageType, sourceAddress, destinationAddress, transactionID, 0)
{
	SetData(pData, dataLength);
}

PayloadMessage::PayloadMessage(const Header& _header, const ByteArray& data)
: header(_header)
{
	header.SetMessageLength(0);
	SetData(data);
}

PayloadMessage::PayloadMessage(const Header& _header, const byte* pData)
: header(_header)
{
	header.SetMessageLength(0);
	SetData(pData, _header.GetMessageLength());
}

PayloadMessage::PayloadMessage(const ByteArray& data)
{
	FromArray(data);
}

PayloadMessage::PayloadMessage(const byte* pData, MessageLength dataLength)
{
	ByteArray temp;

	temp.resize(dataLength);
	copy(pData, pData+dataLength, temp.begin());

	FromArray(temp);
}

PayloadMessage::PayloadMessage(const PayloadMessage& right)
: header(right.header)
, data(right.data)
, crc(right.crc)
{
}

PayloadMessage::~PayloadMessage()
{
}

PayloadMessage& PayloadMessage::operator=(const PayloadMessage& other)
{
	if (this != &other)
	{
		header = other.header;
		data = other.data;
		crc = other.crc;
	}

	return *this;
}

bool PayloadMessage::operator==(const PayloadMessage& other) const
{
	return (header == other.header) && (data == other.data) && (crc == other.crc);
}

void PayloadMessage::ClearData()
{
	SetData(NULL, 0);
}

const PayloadMessage::ByteArray& PayloadMessage::GetData() const
{
	return data;
}

void PayloadMessage::GetData(ByteArray& result) const
{
	result.resize(data.size());

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

PayloadMessage::MessageLength PayloadMessage::GetDataLength() const
{
	return (MessageLength) data.size();
}

void PayloadMessage::SetData(const ByteArray& data)
{
	SetData(&data.at(0), data.size());
}

void PayloadMessage::SetData(const byte* pData, unsigned dataLength)
{
	if (pData && dataLength)
	{
		data.resize(dataLength);
		std::copy(pData, pData+dataLength, data.begin());
	}
	else
		data.resize(0);

	header.SetMessageLength(data.size()+12);
	Update();
}

const PayloadMessage::Header& PayloadMessage::GetHeader() const
{
	return header;
}

PayloadMessage::MessageType PayloadMessage::GetMessageType() const
{
	return header.GetMessageType();
}

void PayloadMessage::SetMessageType(MessageType messageType)
{
	if (messageType != header.GetMessageType())
	{
		header.SetMessageType(messageType);
		Update();
	}
}

bool PayloadMessage::GetNoAckHaveMessagesFlag() const
{
	return header.GetNoAckHaveMessagesFlag();
}

void PayloadMessage::SetNoAckHaveMessagesFlag(bool set)
{
	if (header.GetNoAckHaveMessagesFlag() != !!set)
	{
		header.SetNoAckHaveMessagesFlag(set);
		Update();
	}
}


PayloadMessage::TransactionID PayloadMessage::GetTransactionID() const
{
	return header.GetTransactionID();
}

void PayloadMessage::SetTransactionID(TransactionID transactionID)
{
	if (transactionID != header.GetTransactionID())
	{
		header.SetTransactionID(transactionID);
		Update();
	}
}

PayloadMessage::BoardAddress PayloadMessage::GetSourceAddress() const
{
	return header.GetSourceAddress();
}

void PayloadMessage::SetSourceAddress(BoardAddress sourceAddress)
{
	if (sourceAddress != header.GetSourceAddress())
	{
		header.SetSourceAddress(sourceAddress);
		Update();
	}
}

PayloadMessage::BoardAddress PayloadMessage::GetDestinationAddress() const
{
	return header.GetDestinationAddress();
}

void PayloadMessage::SetDestinationAddress(BoardAddress destinationAddress)
{
	if (destinationAddress != header.GetDestinationAddress())
	{
		header.SetDestinationAddress(destinationAddress);
		Update();
	}
}

PayloadMessage::CRCType PayloadMessage::GetCRC() const
{
	return crc.Get();
}

PayloadMessage::CRCType PayloadMessage::CalculateCRC() const
{
	ByteArray headerBytes;
	header.ToArray(headerBytes);

	CRC16 crc;

	crc.Add(headerBytes);
	crc.Add(data);
	return crc.Get();
}

PayloadMessage::CRCType PayloadMessage::CalculateCRC(const byte* pData, MessageLength dataLength)
{
	CRC16 crc(pData, dataLength);
	return crc.Get();
}

void PayloadMessage::ToArray(ByteArray& result) const
{
	header.ToArray(result);

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

	CRCType crc = CalculateCRC();
	result.push_back((byte) (crc & 0xFF));
	result.push_back((byte) (crc >> 8));
}

void PayloadMessage::FromArray(const ByteArray& input)
{
	MessageLength index = 0;

	Header newHeader;
	CRC16 crcCalc;
	newHeader.FromArray(input, index, crcCalc);

	const MessageLength data2Start = index;
	const MessageLength data2Length = newHeader.GetMessageLength()-data2Start-sizeof(CRCType);
	const MessageLength remaining = input.size()-data2Start;
	const MessageLength expected = data2Length+sizeof(CRCType);

	if (remaining != expected)
	{
		stringstream sstm;

		sstm << __FILE__ << "(" << __LINE__ << ") "
		     << header << ":"
		     << " expected " << expected
		     << " bytes following header, but "
		     << remaining << " were present";

		throw InvalidArgument("data", sstm.str().c_str());
	}

	crcCalc.Add(&input.at(data2Start), data2Length);

	index += data2Length;

	CRC16 crcDummy;
	CRCType newCRC;
	Get(input, index, newCRC, crcDummy);

	if (newCRC != crcCalc.Get())
	{
		stringstream sstm;
		sstm << __FILE__ << "(" << __LINE__ << ") "
		     << header << ": CRC mismatch (calculated=" << crcCalc << " in-message=" << CRC16(newCRC) << ")";
		throw InvalidArgument("data", sstm.str().c_str());
	}

	header = newHeader;
	crc.Reset(newCRC);

	data.resize(data2Length);

	if (data2Length)
		std::copy(input.begin()+data2Start, input.begin()+data2Start+data2Length, data.begin());
}

string PayloadMessage::ToString() const
{
	stringstream sstm;

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

string PayloadMessage::ToString(MessageType messageType)
{
	stringstream sstm;

	switch(messageType)
	{
	case LRI_NAK:					sstm << "LRI_NAK";							break;
	case LRI_ACK:					sstm << "LRI_ACK";							break;
	case DATA_LINK_NAK:				sstm << "DATA_LINK_NAK";					break;
	case DATA_LINK_ACK:				sstm << "DATA_LINK_ACK";					break;
	case REQUEST_ID:				sstm << "REQUEST_ID";						break;
	case FLOAT_TELEMETRY_DATA:		sstm << "FLOAT_TELEMETRY_DATA";				break;
	case REQUEST_PIB_STATUS:		sstm << "REQUEST_PIB_STATUS";				break;
	case SEND_FWD_ICD_CMD:			sstm << "SEND_FWD_ICD_CMD";					break;
	case POWER_NOTIFICATION:		sstm << "POWER_NOTIFICATION";				break;
	case REQUEST_WAITING_PACKET:	sstm << "REQUEST_WAITING_PACKET";			break;
	case ACK_REQUESTED_PACKET:		sstm << "ACK_REQUESTED_PACKET";				break;
	default:						sstm << "MSGTYPE0x" << ultos(messageType, 16, 4) << "?"; break;
	}

	return sstm.str();
}

SHARED_LIB_PUBLIC ostream& operator<<(ostream& out, const PayloadMessage& message)
{
	const PayloadMessage::Header& header = message.GetHeader();
	const PayloadMessage::ByteArray& dataArray = message.GetData();

	out << "header = { " << header << " }"
	    << " payload = " << dataArray.ToString()
	    << " crc=0x" << ultos(message.GetCRC(), 16, 4);

	return out;
}

void PayloadMessage::Get(const ByteArray& output, MessageLength& index, ushort& data, CRC16& crcUpdate)
{
	if ((index+2) > (MessageLength) output.size())
	{
		stringstream sstm;
		sstm << __FILE__ << "(" << __LINE__ << ") Get: cannot read 16-bits at offset " << index << " in " << output.size() << "-byte array";

		throw InvalidArgument("output", sstm.str().c_str());
	}

	byte a = output[index++];
	byte b = output[index++];

	crcUpdate.Add(a);
	crcUpdate.Add(b);

	data = (ushort) b;
	data <<= 8;
	data |= (ushort) a;
}

void PayloadMessage::Put(ByteArray& output, ushort data, CRC16& crcUpdate)
{
	byte a = (byte) (data & 0xFF);
	byte b = (byte) (data >> 8);

	crcUpdate.Add(a);
	crcUpdate.Add(b);

	output.push_back(a);
	output.push_back(b);
}

void PayloadMessage::Update()
{
	crc = CalculateCRC();
}

}
}
