#include "OBSWConfig.h"
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/tmtcpacket/pus/tm.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <mission/utility/TmFunnel.h>

object_id_t TmFunnel::downlinkDestination = objects::NO_OBJECT;
object_id_t TmFunnel::storageDestination = objects::NO_OBJECT;

TmFunnel::TmFunnel(object_id_t objectId, uint32_t messageDepth):
        SystemObject(objectId), messageDepth(messageDepth) {
	tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
	        MessageQueueMessage::MAX_MESSAGE_SIZE);
	storageQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
	        MessageQueueMessage::MAX_MESSAGE_SIZE);
}

TmFunnel::~TmFunnel() {
}

MessageQueueId_t TmFunnel::getReportReceptionQueue(uint8_t virtualChannel) {
	return tmQueue->getId();
}

ReturnValue_t TmFunnel::performOperation(uint8_t operationCode) {
	TmTcMessage currentMessage;
	ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
	while(status == HasReturnvaluesIF::RETURN_OK)
	{
		status = handlePacket(&currentMessage);
		if(status != HasReturnvaluesIF::RETURN_OK){
			break;
		}
		status = tmQueue->receiveMessage(&currentMessage);
	}

	if (status == MessageQueueIF::EMPTY) {
		return HasReturnvaluesIF::RETURN_OK;
	}
	else {
		return status;
	}
}

ReturnValue_t TmFunnel::handlePacket(TmTcMessage* message) {
	uint8_t* packetData = nullptr;
	size_t size = 0;
	ReturnValue_t result = tmPool->modifyData(message->getStorageId(),
	        &packetData, &size);
	if(result != HasReturnvaluesIF::RETURN_OK){
		return result;
	}
	TmPacketPusC packet(packetData);
	packet.setPacketSequenceCount(this->sourceSequenceCount);
	sourceSequenceCount++;
	sourceSequenceCount = sourceSequenceCount %
	        SpacePacketBase::LIMIT_SEQUENCE_COUNT;
	packet.setErrorControl();

	result = tmQueue->sendToDefault(message);
	if(result != HasReturnvaluesIF::RETURN_OK){
		tmPool->deleteData(message->getStorageId());
		sif::error << "TmFunnel::handlePacket: Error sending to downlink "
				"handler" << std::endl;
		return result;
	}

	if(storageDestination != objects::NO_OBJECT) {
		result = storageQueue->sendToDefault(message);
		if(result != HasReturnvaluesIF::RETURN_OK){
			tmPool->deleteData(message->getStorageId());
			sif::error << "TmFunnel::handlePacket: Error sending to storage "
					"handler" << std::endl;
			return result;
		}
	}
	return result;
}

ReturnValue_t TmFunnel::initialize() {

	tmPool = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
	if(tmPool == nullptr) {
		sif::error << "TmFunnel::initialize: TM store not set."
				<< std::endl;
		sif::error << "Make sure the tm store is set up properly"
				" and implements StorageManagerIF" << std::endl;
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

	AcceptsTelemetryIF* tmTarget =
	        ObjectManager::instance()->get<AcceptsTelemetryIF>(downlinkDestination);
	if(tmTarget == nullptr){
		sif::error << "TmFunnel::initialize: Downlink Destination not set."
				<< std::endl;
		sif::error << "Make sure the downlink destination object is set up "
				"properly and implements AcceptsTelemetryIF" << std::endl;
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

#if OBSW_USE_PTME_IP_CORE == 1
	// Live TM will be sent via the virtual channel 0
	tmQueue->setDefaultDestination(tmTarget->getReportReceptionQueue(0));
#else
	tmQueue->setDefaultDestination(tmTarget->getReportReceptionQueue());
#endif /* OBSW_USE_CCSDS_IP_CORES == 1 */

	// Storage destination is optional.
	if(storageDestination == objects::NO_OBJECT) {
	    return SystemObject::initialize();
	}

	AcceptsTelemetryIF* storageTarget =
	        ObjectManager::instance()->get<AcceptsTelemetryIF>(storageDestination);
	if(storageTarget != nullptr) {
		storageQueue->setDefaultDestination(
		        storageTarget->getReportReceptionQueue());
	}

	return SystemObject::initialize();
}