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

#include <linux/obc/PdecHandler.h>

#include "CCSDSHandler.h"

CCSDSHandler::CCSDSHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
        TxRateSetterIF* txRateSetterIF, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData) :
        SystemObject(objectId), ptmeId(ptmeId), tcDestination(tcDestination), parameterHelper(this), actionHelper(
                this, nullptr), txRateSetterIF(txRateSetterIF), gpioIF(gpioIF), enTxClock(
                enTxClock), enTxData(enTxData) {
    commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
    eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 2);
}

CCSDSHandler::~CCSDSHandler() {
}

ReturnValue_t CCSDSHandler::performOperation(uint8_t operationCode) {
    checkEvents();
    readCommandQueue();
    handleTelemetry();
    handleTelecommands();
    checkTxTimer();
    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);
    }

    EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
                objects::EVENT_MANAGER);
    if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl;
#endif
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }
    result = manager->registerListener(eventQueue->getId());
    if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event "
                "listener" << std::endl;
#endif
        return ObjectManagerIF::CHILD_INIT_FAILED;;
    }
    result = manager->subscribeToEventRange(eventQueue->getId(),
            event::getEventId(PdecHandler::CARRIER_LOCK),
            event::getEventId(PdecHandler::BIT_LOCK_PDEC));
    if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "CCSDSHandler::initialize: Failed to subscribe to events from PDEC "
                "handler" << std::endl;
#endif
        return result;
    }
	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;
    }
    case EN_TRANSMITTER: {
        enableTransmit();
        return EXECUTION_FINISHED;
    }
    case DIS_TRANSMITTER: {
        disableTransmit();
        return EXECUTION_FINISHED;
    }
    default:
        return COMMAND_NOT_IMPLEMENTED;
    }
}

void CCSDSHandler::checkEvents() {
    EventMessage event;
    for (ReturnValue_t result = eventQueue->receiveMessage(&event);
            result == RETURN_OK; result = eventQueue->receiveMessage(&event)) {
        switch (event.getMessageId()) {
        case EventMessage::EVENT_MESSAGE:
            handleEvent(&event);
            break;
        default:
            sif::debug << "CCSDSHandler::checkEvents: Did not subscribe to this event message"
                << std::endl;
            break;
        }
    }
}


void CCSDSHandler::handleEvent(EventMessage* eventMessage) {
    Event event = eventMessage->getEvent();
    switch(event){
    case PdecHandler::BIT_LOCK_PDEC: {
        handleBitLockEvent();
        break;
    }
    case PdecHandler::CARRIER_LOCK: {
        handleCarrierLockEvent();
        break;
    }
    default:
        sif::debug << "CCSDSHandler::handleEvent: Did not subscribe to this event"
            << std::endl;
        break;
    }
}

void CCSDSHandler::handleBitLockEvent() {
    if(transmitterCountdown.isBusy()) {
        // Transmitter already enabled
        return;
    }
    enableTransmit();
}

void CCSDSHandler::handleCarrierLockEvent() {
    if (!enableTxWhenCarrierLock) {
        return;
    }
    enableTransmit();
}

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

void CCSDSHandler::enableTransmit() {
    if(transmitterCountdown.isBusy()) {
        // Transmitter already enabled
        return;
    }
    transmitterCountdown.setTimeout(TRANSMITTER_TIMEOUT);
#if BOARD_TE0720 == 0
    gpioIF->pullHigh(enTxClock);
    gpioIF->pullHigh(enTxData);
#endif /* BOARD_TE0720 == 0 */
    linkState = UP;
    // Set link state of all virtual channels to link up
    forwardLinkstate();
}

void CCSDSHandler::checkTxTimer() {
    if(linkState == DOWN) {
        return;
    }
    if (transmitterCountdown.hasTimedOut()) {
        disableTransmit();
    }
}

void CCSDSHandler::disableTransmit() {
#if BOARD_TE0720 == 0
        gpioIF->pullLow(enTxClock);
        gpioIF->pullLow(enTxData);
#endif /* BOARD_TE0720 == 0 */
    linkState = DOWN;
    forwardLinkstate();
    transmitterCountdown.setTimeout(0);
}