eive-obsw/bsp_q7s/devices/StarTrackerImageHelper.cpp

#include "fsfw/ipc/QueueFactory.h"
#include "StarTrackerImageHelper.h"

#include <fstream>
#include <filesystem>
#include <string>

StarTrackerImageHelper::StarTrackerImageHelper(object_id_t objectId) :
        SystemObject(objectId), commandActionHelper(this), actionHelper(this, nullptr) {
    commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
}

StarTrackerImageHelper::~StarTrackerImageHelper() {
}

ReturnValue_t StarTrackerImageHelper::initialize() {
    sdcMan = SdCardManager::instance();
    if (sdcMan == nullptr) {
        sif::warning << "StarTrackerImageHelper::initialize: Invaldi SD Card Manager" << std::endl;
    }
    ReturnValue_t result = SystemObject::initialize();
    if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }
    result = commandActionHelper.initialize();
    if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }
    result = actionHelper.initialize(commandQueue);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t StarTrackerImageHelper::performOperation(uint8_t operationCode) {
    readCommandQueue();
    doStateMachine();
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t StarTrackerImageHelper::executeAction(ActionId_t actionId,
        MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
    ReturnValue_t result = EXECUTION_FINISHED;

    if (state != State::IDLE) {
        return IS_BUSY;
    }

    if (size > MAX_STR_IMAGE_PATH) {
        return NAME_TOO_LONG;
    }

    switch (actionId) {
    case UPLOAD_IMAGE:
        result = prepareUploadCommand(data, size);
        if (result == RETURN_OK) {
            result = EXECUTION_FINISHED;
        }
        break;
    case DOWNLOAD_IMAGE:
        break;
    default:
        return INVALID_ACTION_ID;
    }

    return result;
}

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

MessageQueueIF* StarTrackerImageHelper::getCommandQueuePtr() {
    return commandQueue;
}

void StarTrackerImageHelper::readCommandQueue() {
    CommandMessage message;
    ReturnValue_t result;

    for (result = commandQueue->receiveMessage(&message); result == HasReturnvaluesIF::RETURN_OK;
            result = commandQueue->receiveMessage(&message)) {
        if (result != RETURN_OK) {
            continue;
        }
        result = actionHelper.handleActionMessage(&message);
        if (result == HasReturnvaluesIF::RETURN_OK) {
            continue;
        }

        result = commandActionHelper.handleReply(&message);
        if (result == HasReturnvaluesIF::RETURN_OK) {
            continue;
        }

        sif::debug << "StarTrackerImageHelper::readCommandQueue: Received inalid message"
                << std::endl;
    }
}

void StarTrackerImageHelper::doStateMachine() {
    switch (state) {
    case State::IDLE:
        break;
    case State::SEND_NEXT_UPLOAD_CMD:
        commandImageUpload();
        break;
    case State::COMMAND_EXECUTING:
    case State::UPLOAD_LAST:
        break;
    default:
        sif::debug << "StarTrackerImageHelper::doStateMachine: Invalid state" << std::endl;
        break;
    }
}

ReturnValue_t StarTrackerImageHelper::getImageLocation(const uint8_t* data, size_t size) {

    // Check if file is stored on SD card and if associated SD card is mounted
    if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_0_MOUNT_POINT)) {
        if (!isSdCardMounted(sd::SLOT_0)) {
            sif::warning << "StarTrackerImageHelper::getImageLocation: SD card 0 not mounted" << std::endl;
            return SD_NOT_MOUNTED;
        }
    }
    else if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_1_MOUNT_POINT)) {
        if (!isSdCardMounted(sd::SLOT_0)) {
            sif::warning << "StarTrackerImageHelper::getImageLocation: SD card 1 not mounted" << std::endl;
            return SD_NOT_MOUNTED;
        }
    }
    else {
        // Specified path in RAM filesystem
    }

    imageFile = std::string(reinterpret_cast<const char*>(data), size);

    // Check if file exists
    if(not std::filesystem::exists(imageFile)) {
        return FILE_NOT_EXISTS;
    }
    return RETURN_OK;
}

bool StarTrackerImageHelper::isSdCardMounted(sd::SdCard sdCard) {
    SdCardManager::SdStatePair active;
    ReturnValue_t result = sdcMan->getSdCardActiveStatus(active);
    if (result != RETURN_OK) {
        sif::debug << "StarTrackerImageHelper::isSdCardMounted: Failed to get SD card active state";
        return false;
    }
    if (sdCard == sd::SLOT_0) {
        if (active.first == sd::MOUNTED) {
            return true;
        }
        else {
            return false;
        }
    }
    else if (sdCard == sd::SLOT_1) {
        if (active.second == sd::MOUNTED) {
            return true;
        }
        else {
            return false;
        }
    }
    else {
        sif::debug << "StarTrackerImageHelper::isSdCardMounted: Unknown SD card specified" << std::endl;
    }
    return false;
}

ReturnValue_t StarTrackerImageHelper::prepareUploadCommand(const uint8_t* data, size_t size) {
    ReturnValue_t result = RETURN_OK;
    result = getImageLocation(data, size);
    if (result != RETURN_OK) {
        return result;
    }
    imageSize = std::filesystem::file_size(imageFile);
    remainingCommands = imageSize / SIZE_IMAGE_PART ;
    if (imageSize % SIZE_IMAGE_PART) {
        remainingCommands++;
    }
    commandsSent = 0;
    state = State::SEND_NEXT_UPLOAD_CMD;
    return result;
}

void StarTrackerImageHelper::stepSuccessfulReceived(ActionId_t actionId,
        uint8_t step) {
}

void StarTrackerImageHelper::stepFailedReceived(ActionId_t actionId, uint8_t step,
        ReturnValue_t returnCode) {
    switch (pendingCommand) {
    case (StarTracker::UPLOAD_IMAGE):
            if (retries < MAX_RETRIES) {
                // Repeat sending last command
                commandsSent--;
                remainingCommands++;
                commandImageUpload();
                retries++;
                state = State::COMMAND_EXECUTING;
            }
            else {
                triggerEvent(IMAGE_UPLOAD_FAILED, returnCode, commandsSent);
                retries = 0;
                state = State::IDLE;
            }
        break;
    default:
        sif::debug << "StarTrackerImageHelper::completionSuccessfulReceived: Invalid pending command"
                << std::endl;
        break;
    }
}

void StarTrackerImageHelper::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) {
}

void StarTrackerImageHelper::completionSuccessfulReceived(ActionId_t actionId) {
    switch (pendingCommand) {
    case (StarTracker::UPLOAD_IMAGE):
        if (state == State::UPLOAD_LAST) {
            triggerEvent(IMAGE_UPLOAD_FINISHED);
            pendingCommand = StarTracker::NONE;
            state = State::IDLE;
        }
        else {
            state = State::SEND_NEXT_UPLOAD_CMD;
        }
        break;
    default:
        sif::debug << "StarTrackerImageHelper::completionSuccessfulReceived: Invalid pending command"
                << std::endl;
        state = State::IDLE;
        break;
    }
}

void StarTrackerImageHelper::completionFailedReceived(ActionId_t actionId,
        ReturnValue_t returnCode) {
    switch(pendingCommand) {
    case(StarTracker::UPLOAD_IMAGE): {
        triggerEvent(IMAGE_UPLOAD_FAILED, returnCode, commandsSent);
        break;
    }
    default:
        sif::debug << "StarTrackerImageHelper::completionFailedReceived: Invalid pending command "
                << std::endl;
        break;
    }
    state = State::IDLE;
}

void StarTrackerImageHelper::commandImageUpload() {
    ReturnValue_t result = RETURN_OK;
    uint16_t dataLen = 0;
    uint8_t tmpCommandBuffer[UPLOAD_COMMAND_SIZE] = {0};
    uint32_t position = commandsSent;

    if (not std::filesystem::exists(imageFile)) {
        triggerEvent(IMAGE_FILE_NOT_EXISTS, commandsSent);
        state = State::IDLE;
        return;
    }

    std::ifstream file(imageFile, std::ifstream::binary);
    file.seekg(position, file.beg);

    if (remainingCommands == 1) {
        dataLen = imageSize - file.tellg();
    }
    else {
        dataLen = SIZE_IMAGE_PART;
    }

    size_t size = 0;
    size_t maxSize = sizeof(position);
    uint8_t* commandBufferPtr = tmpCommandBuffer;
    uint8_t** buffer = &commandBufferPtr;
    SerializeAdapter::serialize(&position, buffer, &size, maxSize,
            SerializeIF::Endianness::BIG);
    file.read(reinterpret_cast<char*>(tmpCommandBuffer), dataLen);
    file.close();

    result = commandActionHelper.commandAction(objects::START_TRACKER,
            StarTracker::UPLOAD_IMAGE, tmpCommandBuffer - size , UPLOAD_COMMAND_SIZE);

    if (result != RETURN_OK) {
        sif::warning << "StarTrackerImageHelper::commandImageUpload: Failed to send image "
                << "upload command" << std::endl;
        triggerEvent(ACTION_COMMANDING_FAILED, result, StarTracker::UPLOAD_IMAGE);
        state = State::IDLE;
        return;
    }

    remainingCommands--;
    commandsSent++;

    if (remainingCommands == 0) {
        state = State::UPLOAD_LAST;
    }
    else {
        state = State::COMMAND_EXECUTING;
    }
    pendingCommand = StarTracker::UPLOAD_IMAGE;
}