#ifndef DEBUGCORE_H
#define DEBUGCORE_H

#include "CompilerTypes.h"
#include "TypeDescriptor.h"
#include "StreamString.h"

namespace MARTe {

struct DebugSignalInfo {
    void* memoryAddress;
    TypeDescriptor type;
    StreamString name;
    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)

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, void* data, uint32 size) {
        uint32 packetSize = 4 + 4 + size;
        uint32 read = readIndex; 
        uint32 write = writeIndex;
        uint32 available = (read <= write) ? (bufferSize - (write - read) - 1) : (read - write - 1);

        if (available < packetSize) return false;

        // Use temporary write index to ensure atomic update
        uint32 tempWrite = write;
        WriteToBuffer(&tempWrite, &signalID, 4);
        WriteToBuffer(&tempWrite, &size, 4);
        WriteToBuffer(&tempWrite, data, size);
        
        // Final atomic update
        writeIndex = tempWrite;
        return true;
    }

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

        uint32 tempRead = read;
        uint32 tempId, tempSize;
        ReadFromBuffer(&tempRead, &tempId, 4);
        ReadFromBuffer(&tempRead, &tempSize, 4);
        
        if (tempSize > maxSize) {
            // Error case: drop data up to writeIndex
            readIndex = write;
            return false;
        }

        ReadFromBuffer(&tempRead, dataBuffer, tempSize);
        signalID = tempId;
        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) {
        uint8* s = (uint8*)src;
        for (uint32 i=0; i<count; i++) {
            buffer[*idx] = s[i];
            *idx = (*idx + 1) % bufferSize;
        }
    }

    void ReadFromBuffer(uint32 *idx, void* dst, uint32 count) {
        uint8* d = (uint8*)dst;
        for (uint32 i=0; i<count; i++) {
            d[i] = buffer[*idx];
            *idx = (*idx + 1) % bufferSize;
        }
    }

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

}

#endif