#include "platform/console.h"

#include <Arduino.h>

#ifdef RORO_DEBUG
#include <esp_log.h>
#include <freertos/FreeRTOS.h>

#include <algorithm>
#include <cstdio>
#endif

namespace roro {

Console console;

#ifdef RORO_DEBUG
namespace {

// Any task may write (ESP-IDF logs come from everywhere), so the ring is behind a spinlock, held
// only for the copy.
portMUX_TYPE ringLock = portMUX_INITIALIZER_UNLOCKED;
vprintf_like_t espLogNext = nullptr;

int teeEspLog(const char* format, va_list args) {
    char line[200];
    va_list copy;
    va_copy(copy, args);
    int n = vsnprintf(line, sizeof line, format, copy);
    va_end(copy);
    if (n > 0) console.toRing(reinterpret_cast<const uint8_t*>(line), std::min<size_t>(n, sizeof line - 1));
    return espLogNext ? espLogNext(format, args) : n;  // the serial port, as before
}

}  // namespace

void Console::captureEspLogs() { espLogNext = esp_log_set_vprintf(teeEspLog); }

void Console::toRing(const uint8_t* data, size_t len) {
    if (len > kRingBytes) {
        data += len - kRingBytes;  // only the tail can fit
        len = kRingBytes;
    }
    portENTER_CRITICAL(&ringLock);
    size_t at = head_ % kRingBytes;
    size_t first = std::min(len, kRingBytes - at);
    memcpy(ring_ + at, data, first);
    memcpy(ring_, data + first, len - first);
    head_ += len;
    portEXIT_CRITICAL(&ringLock);
}

uint32_t Console::oldest() const {
    portENTER_CRITICAL(&ringLock);
    uint32_t pos = head_ > kRingBytes ? head_ - kRingBytes : 0;
    portEXIT_CRITICAL(&ringLock);
    return pos;
}

size_t Console::readSince(uint32_t& pos, uint8_t* out, size_t max, uint32_t& skipped) {
    portENTER_CRITICAL(&ringLock);
    uint32_t from = head_ > kRingBytes ? head_ - kRingBytes : 0;
    skipped = pos < from ? from - pos : 0;
    if (pos < from) pos = from;
    size_t n = std::min<size_t>(max, head_ - pos);
    size_t at = pos % kRingBytes;
    size_t first = std::min(n, kRingBytes - at);
    memcpy(out, ring_ + at, first);
    memcpy(out + first, ring_, n - first);
    pos += n;
    portEXIT_CRITICAL(&ringLock);
    return n;
}
#endif

size_t Console::write(const uint8_t* data, size_t len) {
#ifdef RORO_DEBUG
    toRing(data, len);
#endif
    // Never wait for the USB host. One that's attached but not reading (a VM, a closed terminal)
    // would stall the caller for up to 2 s per write while HWCDC retries; the ring keeps it anyway.
    if (Serial.availableForWrite() >= static_cast<int>(len)) Serial.write(data, len);
    return len;
}

}  // namespace roro