Implemented history writer

2026-06-14 14:09:51 +02:00
parent f25bd7f08e
commit dd7dd22cb0
27 changed files with 1537 additions and 104 deletions
@@ -0,0 +1,540 @@
+/**
+ * @file HistoryWriter.cpp
+ * @brief Disk-backed circular history storage implementation.
+ */
+
+#include "HistoryWriter.h"
+#include "AdvancedErrorManagement.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <math.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <sys/statvfs.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+namespace StreamHub {
+
+/*---------------------------------------------------------------------------*/
+/* Constructor / Destructor                                                   */
+/*---------------------------------------------------------------------------*/
+
+HistoryWriter::HistoryWriter()
+    : scratchT_(static_cast<float64 *>(0)),
+      scratchV_(static_cast<float64 *>(0)),
+      files_(static_cast<SignalFile (*)[kHistMaxSignals]>(0)),
+      fileActive_(static_cast<bool (*)[kHistMaxSignals]>(0)),
+      enabled_(false),
+      durationHours_(1.0),
+      decimation_(1u),
+      flushIntervalSec_(5u),
+      minDiskFreeMB_(500u),
+      diskLow_(false) {
+    directory_[0] = '\0';
+    files_ = new SignalFile[kHistMaxSessions][kHistMaxSignals];
+    fileActive_ = new bool[kHistMaxSessions][kHistMaxSignals];
+    for (uint32 i = 0u; i < kHistMaxSessions; i++) {
+        for (uint32 s = 0u; s < kHistMaxSignals; s++) {
+            files_[i][s].fd = -1;
+            files_[i][s].capacity = 0u;
+            files_[i][s].head = 0u;
+            files_[i][s].count = 0u;
+            files_[i][s].tOldest = 0.0;
+            files_[i][s].tNewest = 0.0;
+            files_[i][s].readCursor = 0u;
+            files_[i][s].decimCounter = 0u;
+            files_[i][s].headerDirty = false;
+            files_[i][s].sourceId[0] = '\0';
+            files_[i][s].signalName[0] = '\0';
+            fileActive_[i][s] = false;
+        }
+    }
+}
+
+HistoryWriter::~HistoryWriter() {
+    /* Flush and close all open files */
+    for (uint32 i = 0u; i < kHistMaxSessions; i++) {
+        for (uint32 s = 0u; s < kHistMaxSignals; s++) {
+            if (files_[i][s].fd >= 0) {
+                FlushHeader(files_[i][s]);
+                (void) close(files_[i][s].fd);
+                files_[i][s].fd = -1;
+            }
+        }
+    }
+    delete[] scratchT_;
+    delete[] scratchV_;
+    delete[] files_;
+    delete[] fileActive_;
+}
+
+/*---------------------------------------------------------------------------*/
+/* Initialise                                                                 */
+/*---------------------------------------------------------------------------*/
+
+bool HistoryWriter::Initialise(StructuredDataI &cfg) {
+    StreamString dir;
+    if (!cfg.Read("Directory", dir) || dir.Size() == 0u) {
+        REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
+            "HistoryWriter: no Directory specified, history disabled.");
+        return false;
+    }
+    strncpy(directory_, dir.Buffer(), sizeof(directory_) - 1u);
+    directory_[sizeof(directory_) - 1u] = '\0';
+
+    /* Remove trailing slash */
+    size_t dLen = strlen(directory_);
+    if (dLen > 1u && directory_[dLen - 1u] == '/') {
+        directory_[dLen - 1u] = '\0';
+    }
+
+    float64 durF = 1.0;
+    if (cfg.Read("DurationHours", durF)) {
+        durationHours_ = (durF > 0.0) ? durF : 1.0;
+    }
+
+    uint32 tmp = 0u;
+    if (cfg.Read("Decimation", tmp))       { decimation_       = (tmp > 0u) ? tmp : 1u; }
+    if (cfg.Read("FlushIntervalSec", tmp)) { flushIntervalSec_ = (tmp > 0u) ? tmp : 5u; }
+    if (cfg.Read("MinDiskFreeMB", tmp))    { minDiskFreeMB_     = tmp; }
+
+    /* Create root directory */
+    (void) mkdir(directory_, 0755);
+
+    /* Allocate scratch buffers */
+    scratchT_ = new float64[kReadScratch];
+    scratchV_ = new float64[kReadScratch];
+
+    enabled_ = true;
+    REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
+        "HistoryWriter: enabled, dir=%s, duration=%.1fh, decimation=%u, flush=%us, minDisk=%uMB.",
+        directory_, durationHours_, decimation_, flushIntervalSec_, minDiskFreeMB_);
+    return true;
+}
+
+/*---------------------------------------------------------------------------*/
+/* OnSourceConfigured                                                         */
+/*---------------------------------------------------------------------------*/
+
+void HistoryWriter::OnSourceConfigured(uint32 sessionIdx, const char *sourceId,
+                                       UDPSourceSession &sess) {
+    if (!enabled_ || sessionIdx >= kHistMaxSessions) { return; }
+
+    const uint32 numSigs = sess.GetNumSignals();
+    for (uint32 s = 0u; s < numSigs && s < kHistMaxSignals; s++) {
+        MARTe::UDPSSignalDescriptor desc;
+        if (!sess.GetSignalDescriptor(s, desc)) { continue; }
+
+        /* Skip time-only signals (type uint64, typically TimeArray) */
+        if (desc.typeCode == 13u) { continue; } /* uint64 = 13 in UDPS */
+
+        float64 estRate = static_cast<float64>(desc.samplingRate);
+        if (estRate <= 0.0) { estRate = 1000.0; } /* fallback */
+
+        if (OpenSignalFile(sessionIdx, s, sourceId, desc.name, estRate)) {
+            fileActive_[sessionIdx][s] = true;
+        }
+    }
+}
+
+/*---------------------------------------------------------------------------*/
+/* OpenSignalFile                                                             */
+/*---------------------------------------------------------------------------*/
+
+bool HistoryWriter::OpenSignalFile(uint32 sessionIdx, uint32 sigIdx,
+                                   const char *sourceId, const char *signalName,
+                                   float64 estimatedRateHz) {
+    /* Create source subdirectory */
+    char srcDir[640];
+    snprintf(srcDir, sizeof(srcDir), "%s/%s", directory_, sourceId);
+    (void) mkdir(srcDir, 0755);
+
+    char filePath[768];
+    snprintf(filePath, sizeof(filePath), "%s/%s.shist", srcDir, signalName);
+
+    /* Calculate capacity: duration × rate / decimation */
+    float64 totalSamples = durationHours_ * 3600.0 * estimatedRateHz
+                           / static_cast<float64>(decimation_);
+    uint32 capacity = static_cast<uint32>(ceil(totalSamples));
+    if (capacity < 1000u) { capacity = 1000u; }
+
+    SignalFile &sf = files_[sessionIdx][sigIdx];
+
+    /* Try to reopen an existing file */
+    int fd = open(filePath, O_RDWR);
+    if (fd >= 0) {
+        uint8 hdr[kHistHeaderSize];
+        ssize_t nr = pread(fd, hdr, kHistHeaderSize, 0);
+        if (nr == static_cast<ssize_t>(kHistHeaderSize) &&
+            memcmp(hdr, kHistMagic, 4) == 0) {
+            /* Read existing header */
+            uint32 ver = 0u, fileCap = 0u;
+            memcpy(&ver,     &hdr[4],  4);
+            memcpy(&fileCap, &hdr[8],  4);
+            if (ver == 1u && fileCap == capacity) {
+                /* Reuse existing file */
+                memcpy(&sf.head,     &hdr[12], 4);
+                memcpy(&sf.count,    &hdr[16], 4);
+                memcpy(&sf.tOldest,  &hdr[24], 8);
+                memcpy(&sf.tNewest,  &hdr[32], 8);
+                sf.fd = fd;
+                sf.capacity = fileCap;
+                sf.decimCounter = 0u;
+                sf.readCursor = 0u;
+                sf.headerDirty = false;
+                strncpy(sf.sourceId, sourceId, sizeof(sf.sourceId) - 1u);
+                sf.sourceId[sizeof(sf.sourceId) - 1u] = '\0';
+                strncpy(sf.signalName, signalName, sizeof(sf.signalName) - 1u);
+                sf.signalName[sizeof(sf.signalName) - 1u] = '\0';
+
+                REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
+                    "HistoryWriter: reopened %s (cap=%u, count=%u).",
+                    filePath, fileCap, sf.count);
+                return true;
+            }
+        }
+        /* Capacity changed or corrupt — recreate */
+        (void) close(fd);
+        fd = -1;
+    }
+
+    /* Create new file */
+    fd = open(filePath, O_RDWR | O_CREAT | O_TRUNC, 0644);
+    if (fd < 0) {
+        REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
+            "HistoryWriter: cannot create %s: %s", filePath, strerror(errno));
+        return false;
+    }
+
+    /* Pre-allocate: header + capacity × 16 bytes */
+    off_t fileSize = static_cast<off_t>(kHistHeaderSize)
+                   + static_cast<off_t>(capacity) * 16;
+    if (ftruncate(fd, fileSize) != 0) {
+        REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
+            "HistoryWriter: ftruncate %s failed: %s", filePath, strerror(errno));
+        (void) close(fd);
+        return false;
+    }
+
+    /* Write header */
+    uint8 hdr[kHistHeaderSize];
+    memset(hdr, 0, kHistHeaderSize);
+    memcpy(&hdr[0], kHistMagic, 4);
+    uint32 ver = 1u;
+    memcpy(&hdr[4],  &ver,        4);
+    memcpy(&hdr[8],  &capacity,   4);
+    /* head=0, count=0, decimation, tOldest=0, tNewest=0 */
+    memcpy(&hdr[20], &decimation_, 4);
+    (void) pwrite(fd, hdr, kHistHeaderSize, 0);
+
+    sf.fd = fd;
+    sf.capacity = capacity;
+    sf.head = 0u;
+    sf.count = 0u;
+    sf.tOldest = 0.0;
+    sf.tNewest = 0.0;
+    sf.readCursor = 0u;
+    sf.decimCounter = 0u;
+    sf.headerDirty = false;
+    strncpy(sf.sourceId, sourceId, sizeof(sf.sourceId) - 1u);
+    sf.sourceId[sizeof(sf.sourceId) - 1u] = '\0';
+    strncpy(sf.signalName, signalName, sizeof(sf.signalName) - 1u);
+    sf.signalName[sizeof(sf.signalName) - 1u] = '\0';
+
+    REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
+        "HistoryWriter: created %s (cap=%u, %.1f MB).",
+        filePath, capacity,
+        static_cast<double>(fileSize) / (1024.0 * 1024.0));
+    return true;
+}
+
+/*---------------------------------------------------------------------------*/
+/* WriteTick                                                                  */
+/*---------------------------------------------------------------------------*/
+
+void HistoryWriter::WriteTick(uint32 sessionIdx, UDPSourceSession &sess) {
+    if (!enabled_ || sessionIdx >= kHistMaxSessions || diskLow_) { return; }
+
+    const uint32 numSigs = sess.GetNumSignals();
+    for (uint32 s = 0u; s < numSigs && s < kHistMaxSignals; s++) {
+        if (!fileActive_[sessionIdx][s]) { continue; }
+        SignalFile &sf = files_[sessionIdx][s];
+        if (sf.fd < 0) { continue; }
+
+        /* Read new samples from the in-memory ring since last tick */
+        uint32 nRaw = sess.ReadSignalSince(s, sf.readCursor,
+                                           scratchT_, scratchV_, kReadScratch);
+        if (nRaw == 0u) { continue; }
+
+        if (decimation_ <= 1u) {
+            WritePairs(sf, scratchT_, scratchV_, nRaw);
+        } else {
+            /* Apply decimation: take every Nth sample */
+            uint32 nOut = 0u;
+            for (uint32 i = 0u; i < nRaw; i++) {
+                sf.decimCounter++;
+                if (sf.decimCounter >= static_cast<uint64>(decimation_)) {
+                    scratchT_[nOut] = scratchT_[i];
+                    scratchV_[nOut] = scratchV_[i];
+                    nOut++;
+                    sf.decimCounter = 0u;
+                }
+            }
+            if (nOut > 0u) {
+                WritePairs(sf, scratchT_, scratchV_, nOut);
+            }
+        }
+    }
+}
+
+/*---------------------------------------------------------------------------*/
+/* WritePairs                                                                 */
+/*---------------------------------------------------------------------------*/
+
+void HistoryWriter::WritePairs(SignalFile &sf, const float64 *t,
+                               const float64 *v, uint32 n) {
+    for (uint32 i = 0u; i < n; i++) {
+        /* Write one (t,v) pair at the current head position */
+        off_t offset = static_cast<off_t>(kHistHeaderSize)
+                     + static_cast<off_t>(sf.head) * 16;
+        float64 pair[2] = { t[i], v[i] };
+        (void) pwrite(sf.fd, pair, 16, offset);
+
+        sf.head = (sf.head + 1u) % sf.capacity;
+        if (sf.count < sf.capacity) {
+            sf.count++;
+        }
+    }
+
+    /* Update time bounds */
+    sf.tNewest = t[n - 1u];
+    if (sf.count <= n) {
+        sf.tOldest = t[0];
+    } else {
+        /* Read the oldest entry's time from disk */
+        uint32 oldestIdx = (sf.head + sf.capacity - sf.count) % sf.capacity;
+        off_t offset = static_cast<off_t>(kHistHeaderSize)
+                     + static_cast<off_t>(oldestIdx) * 16;
+        float64 oldestT = 0.0;
+        (void) pread(sf.fd, &oldestT, 8, offset);
+        sf.tOldest = oldestT;
+    }
+    sf.headerDirty = true;
+}
+
+/*---------------------------------------------------------------------------*/
+/* FlushHeaders                                                               */
+/*---------------------------------------------------------------------------*/
+
+void HistoryWriter::FlushHeaders() {
+    if (!enabled_) { return; }
+
+    /* Periodic disk space check */
+    diskLow_ = !HasSufficientDisk();
+    if (diskLow_) {
+        REPORT_ERROR_STATIC(MARTe::ErrorManagement::Warning,
+            "HistoryWriter: disk space below %u MB, writing paused.", minDiskFreeMB_);
+    }
+
+    for (uint32 i = 0u; i < kHistMaxSessions; i++) {
+        for (uint32 s = 0u; s < kHistMaxSignals; s++) {
+            if (!fileActive_[i][s]) { continue; }
+            SignalFile &sf = files_[i][s];
+            if (sf.fd >= 0 && sf.headerDirty) {
+                FlushHeader(sf);
+            }
+        }
+    }
+}
+
+void HistoryWriter::FlushHeader(SignalFile &sf) {
+    uint8 hdr[kHistHeaderSize];
+    memset(hdr, 0, kHistHeaderSize);
+    memcpy(&hdr[0],  kHistMagic,      4);
+    uint32 ver = 1u;
+    memcpy(&hdr[4],  &ver,            4);
+    memcpy(&hdr[8],  &sf.capacity,    4);
+    memcpy(&hdr[12], &sf.head,        4);
+    memcpy(&hdr[16], &sf.count,       4);
+    memcpy(&hdr[20], &decimation_,    4);
+    memcpy(&hdr[24], &sf.tOldest,     8);
+    memcpy(&hdr[32], &sf.tNewest,     8);
+    (void) pwrite(sf.fd, hdr, kHistHeaderSize, 0);
+    (void) fdatasync(sf.fd);
+    sf.headerDirty = false;
+}
+
+/*---------------------------------------------------------------------------*/
+/* ReadRange                                                                  */
+/*---------------------------------------------------------------------------*/
+
+uint32 HistoryWriter::ReadRange(const char *sourceId, const char *signalName,
+                                float64 t0, float64 t1,
+                                float64 *tOut, float64 *vOut,
+                                uint32 maxOut) const {
+    const SignalFile *sf = FindFile(sourceId, signalName);
+    if (sf == static_cast<const SignalFile *>(0) || sf->fd < 0 ||
+        sf->count == 0u || t1 < t0) {
+        return 0u;
+    }
+
+    const uint32 fileCap = sf->capacity;
+    const uint32 cnt = sf->count;
+    const uint32 oldest = (sf->head + fileCap - cnt) % fileCap;
+    const int fd = sf->fd;
+
+    /* Binary search over the logical order (same algorithm as SignalRingBuffer).
+     * We read individual timestamps from disk using pread. */
+
+    /* lo = first logical index with t >= t0 */
+    uint32 lo = 0u;
+    {
+        uint32 a = 0u, b = cnt;
+        while (a < b) {
+            const uint32 mid = a + ((b - a) >> 1);
+            const uint32 phys = (oldest + mid) % fileCap;
+            float64 tv = 0.0;
+            (void) pread(fd, &tv, 8,
+                static_cast<off_t>(kHistHeaderSize) + static_cast<off_t>(phys) * 16);
+            if (tv < t0) { a = mid + 1u; } else { b = mid; }
+        }
+        lo = a;
+    }
+    /* hi = first logical index with t > t1 */
+    uint32 hi = lo;
+    {
+        uint32 a = lo, b = cnt;
+        while (a < b) {
+            const uint32 mid = a + ((b - a) >> 1);
+            const uint32 phys = (oldest + mid) % fileCap;
+            float64 tv = 0.0;
+            (void) pread(fd, &tv, 8,
+                static_cast<off_t>(kHistHeaderSize) + static_cast<off_t>(phys) * 16);
+            if (tv <= t1) { a = mid + 1u; } else { b = mid; }
+        }
+        hi = a;
+    }
+
+    uint32 nOut = hi - lo;
+    if (nOut > maxOut) { nOut = maxOut; }
+
+    /* Read pairs from disk */
+    for (uint32 i = 0u; i < nOut; i++) {
+        uint32 physIdx = (oldest + lo + i) % fileCap;
+        off_t fileOff = static_cast<off_t>(kHistHeaderSize)
+                      + static_cast<off_t>(physIdx) * 16;
+        float64 pair[2];
+        (void) pread(fd, pair, 16, fileOff);
+        tOut[i] = pair[0];
+        vOut[i] = pair[1];
+    }
+
+    return nOut;
+}
+
+/*---------------------------------------------------------------------------*/
+/* GetTimeRange                                                               */
+/*---------------------------------------------------------------------------*/
+
+bool HistoryWriter::GetTimeRange(const char *sourceId, const char *signalName,
+                                 float64 &t0, float64 &t1) const {
+    const SignalFile *sf = FindFile(sourceId, signalName);
+    if (sf == static_cast<const SignalFile *>(0) || sf->count == 0u) {
+        return false;
+    }
+    t0 = sf->tOldest;
+    t1 = sf->tNewest;
+    return true;
+}
+
+/*---------------------------------------------------------------------------*/
+/* AppendInfoJSON                                                             */
+/*---------------------------------------------------------------------------*/
+
+/* Forward-declare JsonAppendf from StreamHub.cpp — we use the same pattern */
+static bool HistJsonAppendf(char *&buf, uint32 &len, uint32 &cap,
+                            const char *fmt, ...) {
+    for (;;) {
+        va_list ap;
+        va_start(ap, fmt);
+        const int wrote = vsnprintf(buf + len, cap - len, fmt, ap);
+        va_end(ap);
+        if (wrote < 0) { return false; }
+        if (static_cast<uint32>(wrote) < (cap - len)) {
+            len += static_cast<uint32>(wrote);
+            return true;
+        }
+        uint32 newCap = cap * 2u;
+        while ((newCap - len) <= static_cast<uint32>(wrote)) {
+            newCap *= 2u;
+        }
+        char *nb = new char[newCap];
+        memcpy(nb, buf, len);
+        delete[] buf;
+        buf = nb;
+        cap = newCap;
+    }
+}
+
+void HistoryWriter::AppendInfoJSON(char *&buf, uint32 &off, uint32 &cap) const {
+    HistJsonAppendf(buf, off, cap,
+        "\"enabled\":%s,\"durationHours\":%.2f,\"decimation\":%u,\"signals\":{",
+        enabled_ ? "true" : "false", durationHours_, decimation_);
+
+    bool first = true;
+    for (uint32 i = 0u; i < kHistMaxSessions; i++) {
+        for (uint32 s = 0u; s < kHistMaxSignals; s++) {
+            if (!fileActive_[i][s]) { continue; }
+            const SignalFile &sf = files_[i][s];
+            if (sf.count == 0u) { continue; }
+
+            HistJsonAppendf(buf, off, cap,
+                "%s\"%s:%s\":{\"t0\":%.17g,\"t1\":%.17g,\"count\":%u,\"capacity\":%u}",
+                (first ? "" : ","),
+                sf.sourceId, sf.signalName,
+                sf.tOldest, sf.tNewest, sf.count, sf.capacity);
+            first = false;
+        }
+    }
+    HistJsonAppendf(buf, off, cap, "}");
+}
+
+/*---------------------------------------------------------------------------*/
+/* HasSufficientDisk                                                          */
+/*---------------------------------------------------------------------------*/
+
+bool HistoryWriter::HasSufficientDisk() const {
+    if (minDiskFreeMB_ == 0u) { return true; }
+
+    struct statvfs st;
+    if (statvfs(directory_, &st) != 0) { return true; /* assume OK */ }
+
+    uint64 freeMB = (static_cast<uint64>(st.f_bavail) * st.f_frsize)
+                    / (1024u * 1024u);
+    return freeMB >= static_cast<uint64>(minDiskFreeMB_);
+}
+
+/*---------------------------------------------------------------------------*/
+/* FindFile                                                                   */
+/*---------------------------------------------------------------------------*/
+
+const HistoryWriter::SignalFile* HistoryWriter::FindFile(
+    const char *sourceId, const char *signalName) const {
+    for (uint32 i = 0u; i < kHistMaxSessions; i++) {
+        for (uint32 s = 0u; s < kHistMaxSignals; s++) {
+            if (!fileActive_[i][s]) { continue; }
+            const SignalFile &sf = files_[i][s];
+            if (strcmp(sf.sourceId, sourceId) == 0 &&
+                strcmp(sf.signalName, signalName) == 0) {
+                return &sf;
+            }
+        }
+    }
+    return static_cast<const SignalFile *>(0);
+}
+
+} /* namespace StreamHub */