#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, object_id_t tcDestination,
        TxRateSetterIF* txRateSetterIF) :
        SystemObject(objectId), ptmeId(ptmeId), tcDestination(tcDestination), parameterHelper(this), actionHelper(
                this, nullptr), txRateSetterIF(txRateSetterIF) {
    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;
	}

	AcceptsTelecommandsIF* tcDistributor =
            ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
    if (tcDistributor == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
	        sif::error << "CCSDSHandler::initialize: Invalid TC Distributor object" << std::endl;
#endif
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    tcDistributorQueueId = tcDistributor->getRequestQueue();

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

	result = actionHelper.initialize(commandQueue);
    if (result != 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;
        }
        result = actionHelper.handleActionMessage(&commandMessage);
        if (result == RETURN_OK) {
            return;
        }
        CommandMessage reply;
        reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
                commandMessage.getCommand());
        commandQueue->reply(&reply);
        return;
    }
}

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;
}

uint16_t CCSDSHandler::getIdentifier() {
    return 0;
}

MessageQueueId_t CCSDSHandler::getRequestQueue() {
    // Forward packets directly to TC distributor
    return tcDistributorQueueId;
}

ReturnValue_t CCSDSHandler::executeAction(ActionId_t actionId,
        MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {

    switch(actionId) {
    case SET_LOW_RATE:
        txRateSetterIF->setRate(BitRates::RATE_400KHZ);
        return EXECUTION_FINISHED;
    case SET_HIGH_RATE:
        txRateSetterIF->setRate(BitRates::RATE_2000KHZ);
        return EXECUTION_FINISHED;
    default:
        return COMMAND_NOT_IMPLEMENTED;
    }
}