#include "MPSoCSequenceCount.h"

MPSoCSequenceCount::MPSoCSequenceCount() {
}

MPSoCSequenceCount::~MPSoCSequenceCount() {
}

MPSoCSequenceCount::increment() {
    ReturnValue_t result = RETURN_OK;
    result = spiMutex->lockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::increment: Failed to lock mutex" << std::endl;
        return result;
    }
    sequenceCount = (sequenceCount + 1) & SEQUENCE_COUNT_MASK;
    result = spiMutex->unlockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::increment: Failed to unlock mutex" << std::endl;
        return result;
    }
}

MPSoCSequenceCount::set(uint16_t sequenceCount_) {
    ReturnValue_t result = RETURN_OK;
    result = spiMutex->lockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::set: Failed to lock mutex" << std::endl;
        return result;
    }
    sequenceCount = sequenceCount_;
    result = spiMutex->unlockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::set: Failed to unlock mutex" << std::endl;
        return result;
    }
}

MPSoCSequenceCount::reset() {
    ReturnValue_t result = RETURN_OK;
    result = spiMutex->lockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::reset: Failed to lock mutex" << std::endl;
        return result;
    }
    sequenceCount = 0;
    result = spiMutex->unlockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
        sif::warning << "MPSoCSequenceCount::reset: Failed to unlock mutex" << std::endl;
        return result;
    }
}

uint16_t MPSoCSequenceCount::get() {
    return sequenceCount;
}