#include "TestEchoComIF.h"
#include "TestCookie.h"

#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/tmtcservices/CommandingServiceBase.h>
#include <fsfw/tmtcpacket/pus/TmPacketStored.h>


TestEchoComIF::TestEchoComIF(object_id_t objectId):
	SystemObject(objectId) {
}

TestEchoComIF::~TestEchoComIF() {}

ReturnValue_t TestEchoComIF::initializeInterface(CookieIF * cookie) {
	DummyCookie* dummyCookie = dynamic_cast<DummyCookie*>(cookie);
	if(dummyCookie == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
		sif::warning << "TestEchoComIF::initializeInterface: Invalid cookie!" << std::endl;
#else
		sif::printWarning("TestEchoComIF::initializeInterface: Invalid cookie!\n");
#endif
		return NULLPOINTER;
	}

	auto resultPair = replyMap.emplace(
	        dummyCookie->getAddress(), ReplyBuffer(dummyCookie->getReplyMaxLen()));
	if(not resultPair.second) {
	    return HasReturnvaluesIF::RETURN_FAILED;
	}
	return RETURN_OK;
}

ReturnValue_t TestEchoComIF::sendMessage(CookieIF *cookie,
		const uint8_t * sendData, size_t sendLen) {
    DummyCookie* dummyCookie = dynamic_cast<DummyCookie*>(cookie);
    if(dummyCookie == nullptr) {
        return NULLPOINTER;
    }

    ReplyBuffer& replyBuffer = replyMap.find(dummyCookie->getAddress())->second;
	if(sendLen > replyBuffer.capacity()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
		sif::warning << "TestEchoComIF::sendMessage: Send length " << sendLen << " larger than "
		        "current reply buffer length!" << std::endl;
#else
		sif::printWarning("TestEchoComIF::sendMessage: Send length %d larger than current "
		        "reply buffer length!\n", sendLen);
#endif
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	replyBuffer.resize(sendLen);
	memcpy(replyBuffer.data(), sendData, sendLen);
	return RETURN_OK;
}

ReturnValue_t TestEchoComIF::getSendSuccess(CookieIF *cookie) {
	return RETURN_OK;
}

ReturnValue_t TestEchoComIF::requestReceiveMessage(CookieIF *cookie,
		size_t requestLen) {
	return RETURN_OK;
}

ReturnValue_t TestEchoComIF::readReceivedMessage(CookieIF *cookie,
		uint8_t **buffer, size_t *size) {
    DummyCookie* dummyCookie = dynamic_cast<DummyCookie*>(cookie);
    if(dummyCookie == nullptr) {
        return NULLPOINTER;
    }

    ReplyBuffer& replyBuffer = replyMap.find(dummyCookie->getAddress())->second;
	*buffer = replyBuffer.data();
	*size = replyBuffer.size();

	dummyReplyCounter ++;
	if(dummyReplyCounter == 10) {
		// add anything that needs to be read periodically by dummy handler
		dummyReplyCounter = 0;
	}
	return RETURN_OK;

}