blk_box_tc/main/drivers/state_tracking.cpp

#include "state_tracking.h"
#include <unistd.h>
#include <vector>

enum state_t {
    STATE_IDLE = 0,
    STATE_RECORDING = 1,
    STATE_PLAYBACK = 2,
};

static std::vector<void(*)(const char*, const char*)> replay_fns;

static bool should_close_recording_stream;
static FILE* recording_stream;
static uint32_t recording_start_time;
TaskHandle_t flush_file_task_handle;

static bool should_close_playback_stream;
static FILE* playback_stream;
static uint32_t playback_start_time;
TaskHandle_t playback_task_handle;

static volatile state_t state = STATE_IDLE;


/// @brief Periodically flushes and syncs (if neccesary) the output stream.
/// @param arg unused.
static void flush_file_task(void* arg) {
    while (state == STATE_RECORDING && recording_stream != nullptr) {
        fflush(recording_stream);

        int fd = fileno(recording_stream);
        if (fd != -1) {
            fsync(fd);
        }
        vTaskDelay(pdMS_TO_TICKS(5000));
    }

    flush_file_task_handle = NULL;
    vTaskDelete(NULL);
}

static void playback_task(void* arg) {
    const size_t size = 16*1024;
    char* buf = (char*) malloc(size*sizeof(char));

    while (state == STATE_PLAYBACK && playback_stream != nullptr) {
        fgets(buf, size, playback_stream);

        // look for a comma, indicating the end of the "ticks" part
        size_t comma_pos = 0;
        for (int i = 0; i < 11; i++) {
            if (buf[i] == ',') {
                comma_pos = i;
                break;
            }
        }
        if (comma_pos == 0) {
            ESP_LOGE("playback", "Failed to find comma in playback line");
            continue;
        }

        buf[comma_pos] = '\0';
        uint32_t tick = atoi(buf);

        // now look for the colon to indicate the end of the event name
        size_t colon_pos = 0;
        int i = comma_pos + 1; // start looking right after the comma
        while (i < size) {
            if (buf[i] == ':') {
                colon_pos = i;
                break;
            }
        }
        if (colon_pos == 0) {
            ESP_LOGE("playback", "Failed to find colon in playback line");
            continue;
        }

        buf[colon_pos] = '\0';
        char* event_name = buf + (comma_pos + 1);
        char* arg = buf + (colon_pos + 1);

        int32_t ticks_to_wait = ((int32_t) tick) - (int32_t)(xTaskGetTickCount() - playback_start_time);

        if (ticks_to_wait < 0) {
            ESP_LOGW("playback", "Playback is behind by %ld ticks!", ticks_to_wait);
        }
        if (ticks_to_wait > 0) {
            vTaskDelay(ticks_to_wait);
        }

        for (const auto& fn : replay_fns) {
            (fn)(event_name, arg);
        }
    }
}

void register_event_cb(void (*replay_callback)(const char*, const char*)) {
    replay_fns.push_back(replay_callback);
}

void event_occured(const char* name, const char* arg) {
    if (state != STATE_RECORDING) return;
    if (name == nullptr) return;
    if (recording_stream == nullptr) {
        ESP_LOGE("state_tracking", "We are in state recording, but recording stream is null");
        return;
    }

    arg = (arg == nullptr) ? "" : arg;
    uint32_t ticks = xTaskGetTickCount() - recording_start_time;
    fprintf(recording_stream, "%ld,%s:%s\n", ticks, name, (arg == nullptr) ? "" : arg);
}

bool set_recording_source(FILE* stream, bool should_close) {
    if (state == STATE_RECORDING) return false;

    recording_stream = stream;
    should_close_recording_stream = should_close;
    return true;
}

bool set_playback_source(FILE* stream, bool should_close) {
    if (state == STATE_PLAYBACK) return false;

    playback_stream = stream;
    should_close_playback_stream = should_close;
    return true;
}

bool start_recording() {
    if (state != STATE_IDLE) return false;
    if (recording_stream == nullptr) return false;

    state = STATE_RECORDING;
    recording_start_time = xTaskGetTickCount();
    xTaskCreate(flush_file_task, "flush_recording", 2048, NULL, 2, &flush_file_task_handle);
    return true;
}

bool stop_recording() {
    if (state != STATE_RECORDING) return false;
    
    state = STATE_IDLE;
    fflush(recording_stream);
    if (should_close_recording_stream) {
        fclose(recording_stream);
        recording_stream = nullptr;
    }

    if (flush_file_task_handle != nullptr) {
        vTaskDelete(flush_file_task_handle);
        flush_file_task_handle = NULL;
    }
    return true;
}

bool start_playback() {
    if (state != STATE_IDLE) return false;
    if (playback_stream == nullptr) return false;

    state = STATE_PLAYBACK;
    playback_start_time = xTaskGetTickCount();
    xTaskCreate(playback_task, "playback", 2048, NULL, 2, &playback_task_handle);
    return true;
}

bool stop_playback() {
    // TODO: impl
    return false;
}

bool is_state_tracking() {
    return state == STATE_RECORDING;
}