#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/ipc/QueueFactory.h"

#include "CCSDSHandler.h"

CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId) :
		SystemObject(objectId), ptmeId(ptmeId), parameterHelper(this) {
    commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
}

CCSDSHandler::~CCSDSHandler() {
}

ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) {
    readCommandQueue();
    handleTelemetry();
    handleTelecommands();
    return RETURN_OK;
}

void CCSDSHandler::handleTelemetry() {
    VirtualChannelMapIter iter;
    for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
        iter->second->performOperation();
    }
}

void CCSDSHandler::handleTelecommands() {

}

ReturnValue_t CCSDSHandler::initialize() {
    ReturnValue_t result = RETURN_OK;
	PtmeIF* ptme = ObjectManager::instance()->get<PtmeIF>(ptmeId);
	if (ptme == nullptr) {
	    sif::warning << "Invalid PTME object" << std::endl;
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

	result = parameterHelper.initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	VirtualChannelMapIter iter;
    for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
        result = iter->second->initialize();
        if (result != RETURN_OK) {
            return result;
        }
        iter->second->setPtmeObject(ptme);
    }

	return result;
}

void CCSDSHandler::readCommandQueue(void) {
	CommandMessage commandMessage;
	ReturnValue_t result = RETURN_FAILED;

	result = commandQueue->receiveMessage(&commandMessage);
    if (result == RETURN_OK) {
        result = parameterHelper.handleParameterMessage(&commandMessage);
        if (result == RETURN_OK) {
            return;
        }
        CommandMessage reply;
        reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
                commandMessage.getCommand());
        commandQueue->reply(&reply);
    }
}

MessageQueueId_t CCSDSHandler::getCommandQueue() const {
	return commandQueue->getId();
}

void CCSDSHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
    if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) {
        sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl;
        return;
    }

    if (virtualChannel == nullptr) {
        sif::warning << "CCSDSHandler::addVirtualChannel: Invalid virtual channel interface" << std::endl;
        return;
    }

    auto status = virtualChannelMap.emplace(vcId, virtualChannel);
    if (status.second == false) {
        sif::warning << "CCSDSHandler::addVirtualChannel: Failed to add virtual channel to "
                "virtual channel map" << std::endl;
        return;
    }
}

MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) {
	if (virtualChannel < common::NUMBER_OF_VIRTUAL_CHANNELS) {
	    VirtualChannelMapIter iter = virtualChannelMap.find(virtualChannel);
	    if (iter != virtualChannelMap.end()) {
	        return iter->second->getReportReceptionQueue();
	    }
		else {
		    sif::warning << "CCSDSHandler::getReportReceptionQueue: Virtual channel with ID "
		            << static_cast<unsigned int>(virtualChannel) << " not in virtual channel map"
		            << std::endl;
		    return MessageQueueIF::NO_QUEUE;
		}
	} else {
		sif::debug << "CCSDSHandler::getReportReceptionQueue: Invalid virtual channel requested";

	}
	return MessageQueueIF::NO_QUEUE;
}

ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
        ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
        uint16_t startAtIndex) {
    return RETURN_OK;
}