marte-debug/Source/Components/Interfaces/DebugService/DebugCore.h

#ifndef DEBUGCORE_H
#define DEBUGCORE_H

#include "CompilerTypes.h"
#include "TypeDescriptor.h"
#include "StreamString.h"
#include <string.h> 

namespace MARTe {

struct DebugSignalInfo {
    void* memoryAddress;
    TypeDescriptor type;
    StreamString name;
    uint8 numberOfDimensions;
    uint32 numberOfElements;
    volatile bool isTracing;
    volatile bool isForcing;
    uint8 forcedValue[1024]; 
    uint32 internalID;
    volatile uint32 decimationFactor;
    volatile uint32 decimationCounter;
};

#pragma pack(push, 1)
struct TraceHeader {
    uint32 magic;      // 0xDA7A57AD
    uint32 seq;        // Sequence number
    uint64 timestamp;  // HighRes timestamp
    uint32 count;      // Number of samples in payload
};
#pragma pack(pop)

/**
 * @brief Ring buffer for high-frequency signal tracing.
 * @details New format per sample: [ID:4][Timestamp:8][Size:4][Data:N]
 */
class TraceRingBuffer {
public:
    TraceRingBuffer() {
        bufferSize = 0;
        buffer = NULL_PTR(uint8*);
        readIndex = 0;
        writeIndex = 0;
    }

    ~TraceRingBuffer() {
        if (buffer != NULL_PTR(uint8*)) {
            delete[] buffer;
        }
    }

    bool Init(uint32 size) {
        if (buffer != NULL_PTR(uint8*)) {
            delete[] buffer;
        }
        bufferSize = size;
        buffer = new uint8[bufferSize];
        readIndex = 0;
        writeIndex = 0;
        return (buffer != NULL_PTR(uint8*));
    }

    bool Push(uint32 signalID, uint64 timestamp, void* data, uint32 size) {
        uint32 packetSize = 4 + 8 + 4 + size; // ID + TS + Size + Data
        uint32 read = readIndex; 
        uint32 write = writeIndex;
        
        uint32 available = 0;
        if (read <= write) {
            available = bufferSize - (write - read) - 1;
        } else {
            available = read - write - 1;
        }

        if (available < packetSize) return false;

        uint32 tempWrite = write;
        WriteToBuffer(&tempWrite, &signalID, 4);
        WriteToBuffer(&tempWrite, &timestamp, 8);
        WriteToBuffer(&tempWrite, &size, 4);
        WriteToBuffer(&tempWrite, data, size);
        
        writeIndex = tempWrite;
        return true;
    }

    bool Pop(uint32 &signalID, uint64 &timestamp, void* dataBuffer, uint32 &size, uint32 maxSize) {
        uint32 read = readIndex;
        uint32 write = writeIndex;
        if (read == write) return false;

        uint32 tempRead = read;
        uint32 tempId = 0;
        uint64 tempTs = 0;
        uint32 tempSize = 0;
        
        ReadFromBuffer(&tempRead, &tempId, 4);
        ReadFromBuffer(&tempRead, &tempTs, 8);
        ReadFromBuffer(&tempRead, &tempSize, 4);
        
        if (tempSize > maxSize) {
            readIndex = write;
            return false;
        }

        ReadFromBuffer(&tempRead, dataBuffer, tempSize);
        
        signalID = tempId;
        timestamp = tempTs;
        size = tempSize;
        
        readIndex = tempRead;
        return true;
    }

    uint32 Count() {
        uint32 read = readIndex;
        uint32 write = writeIndex;
        if (write >= read) return write - read;
        return bufferSize - (read - write);
    }

private:
    void WriteToBuffer(uint32 *idx, void* src, uint32 count) {
        uint32 current = *idx;
        uint32 spaceToEnd = bufferSize - current;
        if (count <= spaceToEnd) {
            memcpy(&buffer[current], src, count);
            *idx = (current + count) % bufferSize;
        } else {
            memcpy(&buffer[current], src, spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            memcpy(&buffer[0], (uint8*)src + spaceToEnd, remaining);
            *idx = remaining;
        }
    }

    void ReadFromBuffer(uint32 *idx, void* dst, uint32 count) {
        uint32 current = *idx;
        uint32 spaceToEnd = bufferSize - current;
        if (count <= spaceToEnd) {
            memcpy(dst, &buffer[current], count);
            *idx = (current + count) % bufferSize;
        } else {
            memcpy(dst, &buffer[current], spaceToEnd);
            uint32 remaining = count - spaceToEnd;
            memcpy((uint8*)dst + spaceToEnd, &buffer[0], remaining);
            *idx = remaining;
        }
    }

    volatile uint32 readIndex;
    volatile uint32 writeIndex;
    uint32 bufferSize;
    uint8 *buffer;
};

}

#endif