#include "CCSDSHandler.h"
#include "OBSWConfig.h"

#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
#include "fsfw/objectmanager/ObjectManager.h"


#include <framework/tmtcservices/TmTcMessage.h>

#include <mission/obc/ccsdsboard/VirtualChannel.h>


VirtualChannel::VirtualChannel(uint8_t vcId, uint32_t tmQueueDepth) :
        vcId(vcId) {
    tmQueue = QueueFactory::instance()->createMessageQueue(tmQueueDepth,
            MessageQueueMessage::MAX_MESSAGE_SIZE);
}

ReturnValue_t VirtualChannel::initialize() {
    tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
    if(tmStore == nullptr) {
        sif::error << "VirtualChannel::initialize: Failed to get tm store" << std::endl;
        return RETURN_FAILED;
    }
}

ReturnValue_t VirtualChannel::performOperation() {
	ReturnValue_t status = RETURN_OK;
	TmTcMessage message;

	while(tmQueue->receiveMessage(&message) == RETURN_OK) {
	    store_address_t storeId = message.getStorageId();
	    const uint8_t* data = nullptr;
        size_t size = 0;
	    result = tmStore->getData(storeId, &data, &size);
        if (result != RETURN_OK) {
            sif::warning << "VirtualChannel::performOperation: Failed to read data from IPC store"
                    << std::endl;
            tmStore->deleteData(storeId);
            return result;
        }

        if (linkIsUp) {
            result = ptme->writeToVc(virtualChannleId, data, size);
        }

        tmStore->deleteData(storeId);

        if (result != RETURN_OK) {
            return result;
        }
	}
	return result;
}

MessageQueueId_t VirtualChannel::getReportReceptionQueue(uint8_t virtualChannel) {
	return tmQueue.getId();
}

void VirtualChannel::setPtmeObject(PtmeIF* ptme) {
    if (ptme == nullptr) {
        sif::warning << "VirtualChannel::setPtmeObject: Invalid ptme object" << std::endl;
    }
	ptme = ptme;
}

void VirtualChannel::setIdlePacketIntervalMs(uint32_t idlePacketIntervalMs) {
	timer.timeout = idlePacketIntervalMs;
}

ReturnValue_t VirtualChannel::flush() {
	TmTcMessage message;
	ReturnValue_t status = RETURN_FAILED;
	for (status = tmQueue.receiveMessage(&message); status == RETURN_OK;
			status = tmQueue.receiveMessage(&message)) {
		packetStore->deleteData(message.getStorageId());
	}
	boardHandler->resetVC(virtualChannelId);
	return status;
}