meshBuffers.h

Go to the documentation of this file.

 
#ifndef INC_MESHBUFFERS_H_
#define INC_MESHBUFFERS_H_
 
#include <meshPacket.h>
 
template <typename T, uint8_t BufferSize>
class MicroFIFO : public Printable {
public:
 
    const T& front() const {
        return buffer[startPos];
    }
 
    T& front() {
        return buffer[startPos];
    }
 
    const T& back() const {
        uint8_t index = 0;
        if (endPos == 0) index = BufferSize - 1;
        else index = endPos - 1;
        return buffer[index];
    }
 
    T& back() {
        uint8_t index = 0;
        if (endPos == 0) index = BufferSize - 1;
        else index = endPos - 1;
        return buffer[index];
    }
 
    bool get(T& item, uint8_t index) const {
        T* ptr = getRowPtr(index);
        if (ptr != NULL) {
            item = *ptr;
            return true;
        }
        else return false;
    }
 
    const T& operator[](int index) const
    {
        const T* ptr = getRowPtr(index);
        return *ptr;
    }
 
    T& operator[](int index)
    {
        T* ptr = getRowPtr(index);
        return *ptr;
    }
 
    void push(T const& item) {
        buffer[endPos] = item;
        endPos = (endPos + 1) % BufferSize;
 
        if (usedSize >= BufferSize) {
            startPos = (startPos + 1) % BufferSize; //Buffer overflow - remove first item to store last item
            lostItems++;
        }
        else {
            usedSize++;
        }
    }
 
    void push(T&& item) {
        buffer[endPos] = std::move(item);
        endPos = (endPos + 1) % BufferSize;
 
        if (usedSize >= BufferSize) {
            startPos = (startPos + 1) % BufferSize; //Buffer overflow - remove first item to store last item
            lostItems++;
        }
        else {
            usedSize++;
        }
    }
 
    T&& popMove() {
        uint8_t index = startPos;
        startPos = (startPos + 1) % BufferSize;
        usedSize--;
        return std::move(buffer[index]);
    }
 
    bool pop() {
        if(!empty()){
            startPos = (startPos + 1) % BufferSize;
            usedSize--;
            return true;
        }
        else{
            return false;
        }
    }
 
    inline bool empty() const {
        return usedSize == 0;
    }
 
    inline uint8_t count() const {
        return usedSize;
    }
 
    const uint8_t size() const {
        return BufferSize;
    }
 
    inline uint8_t lostItemsCount() const {
        return lostItems;
    }
 
    inline void restartLostItemsCounter() {
        lostItems = 0;
    }
 
    bool removeAt(uint8_t index) {
        if (index >= count()) return false;
        if (index < count() / 2) {
            //Moving items up from start until removed item position.
            for (uint8_t i = index; i > 0; i--) {
                buffer[getRawIndex(i)] = std::move(buffer[getRawIndex(i - 1)]);
            }
            startPos = (startPos + 1) % BufferSize;
        }
        else {
            //Moving items down from end until removed item position.
            for (uint8_t i = index+1; i < count(); i++) {
                buffer[getRawIndex(i - 1)] = std::move(buffer[getRawIndex(i)]);
            }
            if (endPos == 0) {
                endPos = BufferSize - 1;
            }
            else {
                endPos--;
            }
        }
        usedSize--;
        return true;
    }
 
    uint8_t removeMulti(uint8_t startPos_, uint8_t itmCount) {
        if (startPos_ >= count() || itmCount == 0) return 0;
        itmCount--; //To not count item at startPos during calculation
        bool lowerHalf = (startPos_ + itmCount / 2) < count() / 2;
        uint8_t i = startPos_ + itmCount; //i is now index of last item
        if (i >= count()) { //End was reached
            i = count() - 1;
            itmCount = i - startPos_;
        }
        itmCount++; //Get it back, where it was before
        if (lowerHalf) {
            //Moving items up from start until removed item position.
            i--;
            for (; i > itmCount; i--) {
                buffer[getRawIndex(i)] = std::move(buffer[getRawIndex(i - itmCount)]);
            }
            startPos = (startPos + itmCount) % BufferSize;
        }
        else {
            //Moving items down from end until removed item position.
            for (; i < count(); i++) {
                buffer[getRawIndex(i - itmCount)] = std::move(buffer[getRawIndex(i)]);
            }
            if (endPos < itmCount) {
                endPos = BufferSize - (itmCount - endPos);
            }
            else {
                endPos -= itmCount;
            }
        }
        usedSize -= itmCount;
        return itmCount;
    }
 
    uint8_t removeIf(bool(*dec)(const T&)) {
        if (count() == 0) return 0;
        uint8_t removeCnt = 0;
        for (uint8_t readPos = 0; readPos < count(); readPos++) {
            T& item = buffer[getRawIndex(readPos)];
            if (dec(item)) {
                //Remove packet
                removeCnt++;
            }
            else if (removeCnt > 0) {
                //Move packets down
                buffer[getRawIndex(readPos - removeCnt)] = std::move(item);
            }
        }
        if (endPos < removeCnt) {
            endPos = BufferSize - (removeCnt - endPos);
        }
        else {
            endPos -= removeCnt;
        }
        usedSize -= removeCnt;
        return removeCnt;
    }
 
    void clear() {
        for(uint8_t i = 0; i < BufferSize; i++){
            buffer[0].~T(); //Calling destructor to free allocated memory if any
        }
        startPos = 0;
        endPos = 0;
        usedSize = 0;
        lostItems = 0;
    }
 
    size_t getRawDataSize() const {
        if (std::is_base_of<String, T>::value) { //special code for strings
            size_t ret = 14;
            for (uint8_t i = 0; i < usedSize; i++) {
                const String* item = (String*)getRowPtr(i);
                ret += item->length() + 4;
            }
            return ret;
        }
        else if (std::is_base_of<RawPrintable, T>::value) { //special code for raw printables
            size_t ret = 14;
            for (uint8_t i = 0; i < usedSize; i++) {
                const RawPrintable* rp = (RawPrintable*)getRowPtr(i);
                ret += rp->getRawSize();
            }
            return ret;
        }
        else {
            return 14 + usedSize * sizeof(T);
        }
    }
 
    size_t virtual printRaw(OSTREAM& stream, const char* customTag) {
        crc.begin();
        crc.setInputReverse(true);
        crc.setOutputReverse(true);
        crc.setPolynomial32();
        crc.reset(0xFFFFFFFFUL);
 
        size_t ret = 0;
        ret = stream.print("MFF:"); //Micro FIFO
        union WTB {
            uint32_t word;
            uint8_t buffer[4];
        };
 
        char ctag[4] = { 0 };
        for (uint8_t i = 0; i < 4 && customTag[i] != 0; i++) ctag[i] = customTag[i];
        ret += stream.write(ctag, 4);
        crc.append((uint8_t*)ctag, 4);
 
        ret += stream.write(BufferSize);
        crc.append(BufferSize);
 
        if (std::is_base_of<String, T>::value || std::is_base_of<RawPrintable, T>::value) { //special code for strings and raw printables
            ret += stream.write((uint8_t)255);
            crc.append((uint8_t)255);
        }
        else {
            ret += stream.write((uint8_t)sizeof(T));
            crc.append((uint8_t)sizeof(T));
        }
 
        ret += stream.write(usedSize);
        crc.append(usedSize);
 
        for (uint8_t i = 0; i < usedSize; i++) { //Printing all used rows
            const T* item = getRowPtr(i);
 
            if (std::is_base_of<String, T>::value) { //special code for strings
                String txt = (String&)*item;
                uint32_t len = (uint32_t)txt.length();
                stream.write((uint8_t*)&len, 4); //Length of string - 4 bytes
                crc.append((uint8_t*)&len, 4);
                uint32_t bytesCount = (uint32_t)stream.print(txt);
                crc.append((uint8_t*)txt.c_str(), bytesCount);
                ret += bytesCount;
            }
            else if (std::is_base_of<RawPrintable, T>::value) { //special code for raw printables
                const RawPrintable* rp = item;
                ret += rp->printRaw(&stream, [](uint8_t* buffer, size_t length) {
                    crc.append(buffer, (uint32_t)length);
                    });
            }
            else { //Others types
                uint32_t bytesCount = (uint32_t)stream.write((uint8_t*)item, bytesCount);
                crc.append((uint8_t*)item, bytesCount);
                ret += bytesCount;
            }
        }
 
        uint32_t CRC_val = crc.getLastCRC(0xFFFFFFFFUL);
        ret += stream.write((uint8_t*)&CRC_val, 4); //Append CRC-32 at the end of message
 
        crc.end();
 
        return ret;
    }
 
    virtual size_t print(OSTREAM& stream, bool showIndexes = true) const {
        size_t ret = 0;
        if (showIndexes) {
            ret += stream.print("Index|");
        }
 
        //Check if T contains InlinePrintable methods
        if (std::is_base_of<InlinePrintable, T>::value) {
            //Header printing
            if (usedSize == 0) {
                const T prHeader = T();
                const InlinePrintable& printable = prHeader;
                ret += printable.printHeader(&stream);
            }
            else {
                const InlinePrintable& printable = buffer[startPos];
                ret += printable.printHeader(&stream);
            }
 
            //Dash print
            size_t dashCount = ret;
            for (size_t i = 0; i < dashCount; i++) {
                ret += stream.print('-');
            }
            ret += stream.println();
 
            //Rows print
            if (usedSize == 0) {
                ret += stream.println("Empty");
            }
            else
            {
                for (int i = 0; i < usedSize; i++) {
                    if (showIndexes) {
                        char txtBuffer[10] = { 0 };
                        ITOA(i, txtBuffer, sizeof(txtBuffer) / sizeof(char), 10);
                        alignText(txtBuffer, 5, ' ', T_LEFT);
                        ret += stream.print(txtBuffer);
                        ret += stream.print('|');
                    }
 
                    const InlinePrintable* printable = getRowPtr(i);
                    ret += printable->printLine(&stream);
                }
            }
        }
        else {
            //Header print
            ret += stream.println("Value");
 
            //Dash print
            size_t dashCount = ret;
            for (size_t i = 0; i < dashCount; i++) {
                ret += stream.print('-');
            }
            ret += stream.println();
 
            //Rows print
            if (usedSize == 0) {
                ret += stream.println("Empty");
            }
            else
            {
                for (int i = 0; i < usedSize; i++) {
                    if (showIndexes) {
                        char txtBuffer[10] = { 0 };
                        ITOA(i, txtBuffer, sizeof(txtBuffer) / sizeof(char), 10);
                        alignText(txtBuffer, 5, ' ', T_LEFT);
                        ret += stream.print(txtBuffer);
                        ret += stream.print('|');
                    }
 
                    T item = *getRowPtr(i);
                    ret += stream.println(item);
                }
            }
            ret += stream.println();
        }
        return ret;
    }
 
    virtual size_t sizeOf() const {
        if (std::is_base_of<String, T>::value) { //special code for strings
            size_t ret = 0;
            for (uint8_t i = 0; i < usedSize; i++) {
                const String* item = (String*)getRowPtr(i);
                ret += item->length();
 
            }
            return sizeof(*this) + ret;
        }
        else if (std::is_base_of<RawPrintable, T>::value) { //special code for raw printables
            size_t ret = 0;
            for (uint8_t i = 0; i < usedSize; i++) {
                const RawPrintable* rp = (RawPrintable*)getRowPtr(i);
                ret += rp->sizeOf() - sizeof(T);
            }
            return sizeof(*this) + ret;
        }
        return sizeof(*this);
    }
 
    size_t printTo(Print& p) const override
    {
        return print(p);
    }
 
protected:
    inline int16_t getRawIndex(uint8_t index) const {
        if (index >= BufferSize) return -1;
        uint16_t rawIndex = (startPos + index) % BufferSize;
        return rawIndex;
    }
 
    const T* getRowPtr(uint8_t index) const {
        int16_t rawIndex = getRawIndex(index);
        if (rawIndex >= 0) {
            return &buffer[rawIndex];
        }
        return NULL;
    }
 
    T* getRowPtr(uint8_t index) {
        int16_t rawIndex = getRawIndex(index);
        if (rawIndex >= 0) {
            return &buffer[rawIndex];
        }
        return NULL;
    }
 
    bool isRawIndexOccupied(uint8_t rawIndex) const {
        if (usedSize >= BufferSize) {
            return true; //when buffer is full, it is occupied
        }
        else if (startPos == endPos) {
            return false; //Empty buffer
        }
        else if (startPos < endPos) {
            return rawIndex >= startPos && rawIndex < endPos;
        }
        else {
            return rawIndex >= startPos || rawIndex < endPos;
        }
    }
 
    T buffer[BufferSize];
    uint8_t startPos = 0;
    uint8_t endPos = 0;
    uint8_t usedSize = 0;
    uint8_t lostItems = 0;
};
 
 
template <uint8_t BufferSize>
class MeshPacketFIFO : public MicroFIFO<MeshPacket, BufferSize> {
public:
    bool existsID(uint8_t ID, uint8_t source) const {
        for (uint8_t i = 0; i < MicroFIFO<MeshPacket, BufferSize>::count(); i++) {
            const MeshPacket& packet = MicroFIFO<MeshPacket, BufferSize>::operator[](i);
            if (packet.PacketHeader.ID == ID && packet.PacketHeader.Source == source) {
                return true;
            }
        }
        return false;
    }
 
    int16_t getIndexOfPacket(uint8_t ID, uint8_t source) const {
        for (uint8_t i = 0; i < MicroFIFO<MeshPacket, BufferSize>::count(); i++) {
            const MeshPacket& packet = MicroFIFO<MeshPacket, BufferSize>::operator[](i);
            if (packet.PacketHeader.ID == ID && packet.PacketHeader.Source == source) {
                return i;
            }
        }
        return -1;
    }
 
 
    uint8_t removeByDestination(uint8_t destination) {
        if (MicroFIFO<MeshPacket, BufferSize>::count() == 0) return 0;
        uint8_t removeCnt = 0;
        for (uint8_t readPos = 0; readPos < MicroFIFO<MeshPacket, BufferSize>::count(); readPos++) {
            MeshPacket& readPacket = MicroFIFO<MeshPacket, BufferSize>::buffer[MicroFIFO<MeshPacket, BufferSize>::getRawIndex(readPos)];
            if (readPacket.PacketHeader.Destination == destination) {
                //Remove packet
                removeCnt++;
            }
            else if(removeCnt > 0) {
                //Move packets down
                MicroFIFO<MeshPacket, BufferSize>::buffer[MicroFIFO<MeshPacket, BufferSize>::getRawIndex(readPos - removeCnt)] = std::move(readPacket);
            }
        }
        if (MicroFIFO<MeshPacket, BufferSize>::endPos < removeCnt) {
            MicroFIFO<MeshPacket, BufferSize>::endPos = BufferSize - (removeCnt - MicroFIFO<MeshPacket, BufferSize>::endPos);
        }
        else {
            MicroFIFO<MeshPacket, BufferSize>::endPos -= removeCnt;
        }
        MicroFIFO<MeshPacket, BufferSize>::usedSize -= removeCnt;
        return removeCnt;
    }
};
 
template <uint8_t ROUTE_BufferSize, uint8_t FLOOD_BufferSize>
class MeshPacketPriorityFIFO : public Printable {
 
public:
    const MeshPacket& front() const {
        if (routeBuffer.count()) {
            return routeBuffer.front();
        }
        else {
            return floodBuffer.front();
        }
    }
 
    MeshPacket& front() {
        if (routeBuffer.count()) {
            return routeBuffer.front();
        }
        else {
            return floodBuffer.front();
        }
    }
 
    const MeshPacket& back() const {
        if (routeBuffer.count()) {
            return routeBuffer.back();
        }
        else {
            return floodBuffer.back();
        }
    }
 
    MeshPacket& back() {
        if (routeBuffer.count()) {
            return routeBuffer.back();
        }
        else {
            return floodBuffer.back();
        }
    }
 
    bool get(MeshPacket& item, uint8_t index) const {
        if (routeBuffer.count() > index) {
            return routeBuffer.get(item, index);
        }
        index -= routeBuffer.count();
        if (floodBuffer.count() > index) {
            return floodBuffer.get(item, index);
        }
        return false;
    }
 
    const MeshPacket& operator[](int index) const
    {
        if (routeBuffer.count() > index) {
            return routeBuffer[index];
        }
        index -= routeBuffer.count();
        return floodBuffer[index];
    }
 
    MeshPacket& operator[](int index)
    {
        if (routeBuffer.count() > index) {
            return routeBuffer[index];
        }
        index -= routeBuffer.count();
        return floodBuffer[index];
    }
 
    void push(MeshPacket const& item) {
        if (item.PacketHeader.FrameControl.RoutingEnabled) {
            routeBuffer.push(item);
        }
        else {
            floodBuffer.push(item);
        }
    }
 
    void push(MeshPacket&& item) {
        if (item.PacketHeader.FrameControl.RoutingEnabled) {
            routeBuffer.push(std::move(item));
        }
        else {
            floodBuffer.push(std::move(item));
        }
    }
 
    MeshPacket&& popMove() {
        if (routeBuffer.count()) {
            return routeBuffer.popMove();
        }
        else {
            return floodBuffer.popMove();
        }
    }
 
    bool pop() {
        if (routeBuffer.count()) {
            return routeBuffer.pop();
        }
        else {
            return floodBuffer.pop();
        }
    }
 
    bool removeAt(uint8_t index) {
        if (index < routeBuffer.count()) {
            return routeBuffer.removeAt(index);
        }
        else {
            return floodBuffer.removeAt(index - routeBuffer.count());
        }
    }
 
    uint8_t removeMulti(uint8_t startPos, uint8_t itmCount) {
        uint8_t remCnt = 0;
        uint8_t oldRBCount = routeBuffer.count();
        if (startPos < routeBuffer.count()) {
            remCnt = routeBuffer.removeMulti(startPos, itmCount);
            if (remCnt == 0) return 0;
            startPos = 0;
            itmCount -= remCnt;
        }
        else {
            startPos -= oldRBCount;
        }
 
        if(itmCount > 0) {
            return floodBuffer.removeMulti(startPos, itmCount);
        }
        return 0;
    }
 
    uint8_t removeIf(bool(*dec)(const MeshPacket&)) {
        if (count() == 0) return 0;
        uint8_t cnt = routeBuffer.removeIf(dec);
        return cnt + floodBuffer.removeIf(dec);
    }
 
    uint8_t removeByDestination(uint8_t destination) {
        uint8_t remCnt = floodBuffer.removeByDestination(destination);
        return remCnt + routeBuffer.removeByDestination(destination);
    }
 
    bool existsID(uint8_t ID, uint8_t source) const {
        for (uint8_t i = 0; i < floodBuffer.count(); i++) {
            if (floodBuffer[i].PacketHeader.ID == ID && floodBuffer[i].PacketHeader.Source == source) {
                return true;
            }
        }
        for (uint8_t i = 0; i < routeBuffer.count(); i++) {
            if (routeBuffer[i].PacketHeader.ID == ID && routeBuffer[i].PacketHeader.Source == source) {
                return true;
            }
        }
        return false;
    }
 
    int16_t getIndexOfPacket(uint8_t ID, uint8_t source) const {
        int16_t index = floodBuffer.getIndexOfPacket(ID, source);
        if (index >= 0) return index + routeBuffer.count();
        return routeBuffer.getIndexOfPacket(ID, source);
    }
 
    inline bool empty() const {
        return count() == 0;
    }
 
    inline uint8_t count() const {
        return routeBuffer.count() + floodBuffer.count();
    }
 
    inline uint8_t highCount() const {
        return routeBuffer.count();
    }
 
    inline uint8_t lowCount() const {
        return floodBuffer.count();
    }
 
    inline uint8_t size() const {
        return routeBuffer.size() + floodBuffer.size();
    }
 
    inline uint8_t highSize() const {
        return routeBuffer.size();
    }
 
    inline uint8_t lowSize() const {
        return floodBuffer.size();
    }
 
    inline uint8_t lostItemsCount() const {
        return routeBuffer.lostItemsCount() + floodBuffer.lostItemsCount();
    }
 
    inline void restartLostItemsCounter() {
        routeBuffer.restartLostItemsCounter();
        floodBuffer.restartLostItemsCounter();
    }
 
    inline void clear() {
        routeBuffer.clear();
        floodBuffer.clear();
    }
 
    MeshPacketFIFO<ROUTE_BufferSize>& highFIFO() {
        return routeBuffer;
    }
 
    const MeshPacketFIFO<ROUTE_BufferSize>& highFIFO() const {
        return routeBuffer;
    }
 
    MeshPacketFIFO<ROUTE_BufferSize>& lowFIFO() {
        return floodBuffer;
    }
 
    const MeshPacketFIFO<ROUTE_BufferSize>& lowFIFO() const {
        return floodBuffer;
    }
 
    size_t getRawDataSize() const {
        return routeBuffer.getRawDataSize();
    }
 
    size_t virtual printRaw(OSTREAM& stream, const char* customTag) {
        crc.begin();
        crc.setInputReverse(true);
        crc.setOutputReverse(true);
        crc.setPolynomial32();
        crc.reset(0xFFFFFFFFUL);
 
        size_t ret = 0;
        ret = stream.print("MFF:"); //Micro FIFO
        union WTB {
            uint32_t word;
            uint8_t buffer[4];
        };
 
        char ctag[4] = { 0 };
        for (uint8_t i = 0; i < 4 && customTag[i] != 0; i++) ctag[i] = customTag[i];
        ret += stream.write(ctag, 4);
        crc.append((uint8_t*)ctag, 4);
 
        ret += stream.write(size());
        crc.append(size());
 
        ret += stream.write((uint8_t)255);
        crc.append((uint8_t)255);
 
        ret += stream.write(count());
        crc.append(count());
 
        uint8_t index = 0;
        uint8_t len_p0 = routeBuffer.count();
        for (uint8_t i = 0; i < count(); i++) { //Printing all used rows
            const RawPrintable* rp = NULL;
 
            if (index < len_p0) { //High priority
                index = i;
                rp = &routeBuffer[index];
            }
            else { //Low priority
                index = i - len_p0;
                rp = &floodBuffer[index];
            }
 
            ret += rp->printRaw(&stream, [](uint8_t* buffer, size_t length) {
                crc.append(buffer, (uint32_t)length);
                });
        }
 
        uint32_t CRC_val = crc.getLastCRC(0xFFFFFFFFUL);
        ret += stream.write((uint8_t*)&CRC_val, 4); //Append CRC-32 at the end of message
 
        crc.end();
 
        return ret;
    }
 
    virtual size_t print(OSTREAM& stream, bool showIndexes = true) const {
        size_t ret = 0;
        if (showIndexes) {
            ret += stream.print("Index|");
        }
 
        ret += stream.print("Prior |"); //Priority
 
        uint8_t len = count();
        //Header printing
        if (len == 0) {
            const MeshPacket prHeader = MeshPacket();
            const InlinePrintable& printable = prHeader;
            ret += printable.printHeader(&stream);
        }
        else {
            const InlinePrintable& printable = front();
            ret += printable.printHeader(&stream);
        }
 
        //Dash print
        size_t dashCount = ret;
        for (size_t i = 0; i < dashCount; i++) {
            ret += stream.print('-');
        }
        ret += stream.println();
 
        //Rows print
        if (len == 0) {
            ret += stream.println("Empty");
        }
        else
        {
            uint8_t index = 0;
            uint8_t len_p0 = routeBuffer.count();
            for (int i = 0; i < len; i++) {
                //Index
                char txtBuffer[10] = { 0 };
                if (showIndexes) {
                    ITOA(i, txtBuffer, sizeof(txtBuffer) / sizeof(char), 10);
                    alignText(txtBuffer, 5, ' ', T_LEFT);
                    ret += stream.print(txtBuffer);
                    ret += stream.print('|');
                }
 
                if (index < len_p0) { //High priority
                    ret += stream.print(" High |");
                    index = i;
                    const InlinePrintable& printable = routeBuffer[index];
                    ret += printable.printLine(&stream);
                }
                else { //Low priority
                    ret += stream.print(" Low  |");
                    index = i - len_p0;
                    const InlinePrintable& printable = floodBuffer[index];
                    ret += printable.printLine(&stream);
                }
            }
        }
 
        return ret;
    }
 
    virtual size_t sizeOf() const {
        return routeBuffer.sizeOf() + floodBuffer.sizeOf();
    }
 
    size_t printTo(Print& p) const override
    {
        return print(p);
    }
 
protected:
    MeshPacketFIFO<ROUTE_BufferSize> routeBuffer; //Highest priority
    MeshPacketFIFO<FLOOD_BufferSize> floodBuffer; //Lowest priority
};
 
#endif // !INC_MESHBUFFERS_H_