// psram_buffer.cpp - PSRAM-based Ring Buffer Implementation

#include "psram_buffer.h"
#include "config.h"

PSRAMRingBuffer canFrameBuffer;

static uint8_t* sdWriteBuffer = nullptr;
static uint8_t* signalBuffer = nullptr;
static bool psramInitialized = false;

PSRAMRingBuffer::PSRAMRingBuffer() 
    : _buffer(nullptr), _capacity(0), _head(0), _tail(0), _count(0), _mutex(nullptr) {
}

PSRAMRingBuffer::~PSRAMRingBuffer() {
    if (_buffer != nullptr) {
        free(_buffer);
        _buffer = nullptr;
    }
    if (_mutex != nullptr) {
        vSemaphoreDelete(_mutex);
        _mutex = nullptr;
    }
}

bool PSRAMRingBuffer::begin(size_t capacity) {
    if (_buffer != nullptr) {
        free(_buffer);
    }
    
    size_t bytesNeeded = capacity * sizeof(CanFrame);
    
    if (psramFound()) {
        _buffer = (CanFrame*)ps_malloc(bytesNeeded);
        Serial.printf("[PSRAM] Allocated %d bytes for %d frames\n", bytesNeeded, capacity);
    } else {
        _buffer = (CanFrame*)malloc(bytesNeeded);
        Serial.printf("[HEAP] Allocated %d bytes for %d frames\n", bytesNeeded, capacity);
    }
    
    if (_buffer == nullptr) {
        Serial.println("[ERROR] Failed to allocate ring buffer!");
        return false;
    }
    
    memset(_buffer, 0, bytesNeeded);
    _capacity = capacity;
    _head = 0;
    _tail = 0;
    _count = 0;
    
    _mutex = xSemaphoreCreateMutex();
    if (_mutex == nullptr) {
        free(_buffer);
        _buffer = nullptr;
        return false;
    }
    
    return true;
}

bool PSRAMRingBuffer::push(const CanFrame& frame) {
    if (_buffer == nullptr || _mutex == nullptr) return false;
    
    if (xSemaphoreTake(_mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return false;
    }
    
    if (_count >= _capacity) {
        _tail = (_tail + 1) % _capacity;
        _count--;
    }
    
    _buffer[_head] = frame;
    _head = (_head + 1) % _capacity;
    _count++;
    
    xSemaphoreGive(_mutex);
    return true;
}

bool PSRAMRingBuffer::pop(CanFrame& frame) {
    if (_buffer == nullptr || _mutex == nullptr) return false;
    
    if (xSemaphoreTake(_mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return false;
    }
    
    if (_count == 0) {
        xSemaphoreGive(_mutex);
        return false;
    }
    
    frame = _buffer[_tail];
    _tail = (_tail + 1) % _capacity;
    _count--;
    
    xSemaphoreGive(_mutex);
    return true;
}

bool PSRAMRingBuffer::peek(CanFrame& frame) {
    if (_buffer == nullptr || _mutex == nullptr || _count == 0) return false;
    
    if (xSemaphoreTake(_mutex, pdMS_TO_TICKS(10)) != pdTRUE) {
        return false;
    }
    
    frame = _buffer[_tail];
    xSemaphoreGive(_mutex);
    return true;
}

size_t PSRAMRingBuffer::available() {
    return _count;
}

size_t PSRAMRingBuffer::capacity() {
    return _capacity;
}

size_t PSRAMRingBuffer::freeSpace() {
    return _capacity - _count;
}

bool PSRAMRingBuffer::isFull() {
    return _count >= _capacity;
}

bool PSRAMRingBuffer::isEmpty() {
    return _count == 0;
}

void PSRAMRingBuffer::clear() {
    if (_mutex != nullptr && xSemaphoreTake(_mutex, pdMS_TO_TICKS(100)) == pdTRUE) {
        _head = 0;
        _tail = 0;
        _count = 0;
        xSemaphoreGive(_mutex);
    }
}

void PSRAMRingBuffer::flush() {
    clear();
}

CanFrame* PSRAMRingBuffer::getBuffer() {
    return _buffer;
}

size_t PSRAMRingBuffer::getHead() {
    return _head;
}

size_t PSRAMRingBuffer::getTail() {
    return _tail;
}

bool initPSRAMBuffers() {
    Serial.println("Initializing PSRAM buffers...");
    
    if (psramFound()) {
        Serial.printf("PSRAM detected: %d MB\n", ESP.getPsramSize() / (1024 * 1024));
    } else {
        Serial.println("WARNING: PSRAM not found! Using heap memory.");
    }
    
    size_t frameCount = MAX_PSRAM_CAN_FRAMES;
    if (!canFrameBuffer.begin(frameCount)) {
        Serial.println("Failed to initialize CAN frame buffer!");
        return false;
    }
    
    if (psramFound()) {
        sdWriteBuffer = (uint8_t*)ps_malloc(PSRAM_SD_BUFFER_SIZE);
        signalBuffer = (uint8_t*)ps_malloc(PSRAM_SIGNAL_BUFFER_SIZE);
    } else {
        sdWriteBuffer = (uint8_t*)malloc(PSRAM_SD_BUFFER_SIZE);
        signalBuffer = (uint8_t*)malloc(PSRAM_SIGNAL_BUFFER_SIZE);
    }
    
    if (sdWriteBuffer == nullptr || signalBuffer == nullptr) {
        Serial.println("Failed to allocate auxiliary buffers!");
        return false;
    }
    
    memset(sdWriteBuffer, 0, PSRAM_SD_BUFFER_SIZE);
    memset(signalBuffer, 0, PSRAM_SIGNAL_BUFFER_SIZE);
    
    psramInitialized = true;
    
    Serial.printf("CAN frame buffer: %d frames (%d KB)\n", 
                  frameCount, (frameCount * sizeof(CanFrame)) / 1024);
    Serial.printf("SD write buffer: %d KB\n", PSRAM_SD_BUFFER_SIZE / 1024);
    Serial.printf("Signal buffer: %d KB\n", PSRAM_SIGNAL_BUFFER_SIZE / 1024);
    
    printMemoryStatus();
    
    return true;
}

void deinitPSRAMBuffers() {
    canFrameBuffer.~PSRAMRingBuffer();
    
    if (sdWriteBuffer != nullptr) {
        free(sdWriteBuffer);
        sdWriteBuffer = nullptr;
    }
    
    if (signalBuffer != nullptr) {
        free(signalBuffer);
        signalBuffer = nullptr;
    }
    
    psramInitialized = false;
}

uint8_t* getSDWriteBuffer() {
    return sdWriteBuffer;
}

uint8_t* getSignalBuffer() {
    return signalBuffer;
}

size_t getFreePSRAM() {
    if (psramFound()) {
        return ESP.getFreePsram();
    }
    return 0;
}

size_t getUsedPSRAM() {
    if (psramFound()) {
        return ESP.getPsramSize() - ESP.getFreePsram();
    }
    return 0;
}

size_t getTotalPSRAM() {
    if (psramFound()) {
        return ESP.getPsramSize();
    }
    return 0;
}

void printMemoryStatus() {
    Serial.println("========== Memory Status ==========");
    Serial.printf("Heap: %d / %d KB (free / total)\n", 
                  ESP.getFreeHeap() / 1024, 
                  ESP.getHeapSize() / 1024);
    
    if (psramFound()) {
        Serial.printf("PSRAM: %d / %d KB (free / total)\n", 
                      ESP.getFreePsram() / 1024, 
                      ESP.getPsramSize() / 1024);
    }
    
    Serial.printf("CAN Buffer: %d / %d frames used\n", 
                  canFrameBuffer.available(), 
                  canFrameBuffer.capacity());
    Serial.println("===================================");
}