blk_box_tc/main/drivers/state_tracking.cpp

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

static const char* PLAYBACK_TAG = "playback";

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

static std::vector<bool(*)(const char*, 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));
    if (buf == nullptr) {
        ESP_LOGE(PLAYBACK_TAG, "buf alloc failure");
        vTaskDelete(NULL);
    }

    while (state == STATE_PLAYBACK && playback_stream != nullptr) {
        char* ret = fgets(buf, size, playback_stream);
        if (ret == nullptr) {
            break;
        }
        if (buf[0] == '\0') {
            ESP_LOGI(PLAYBACK_TAG, "playback done");
            break;
        }
        // get rid of the '\n' and possibly '\r' in the string
        for (int i = 0; i < size; i++) {
            if (buf[i] == '\0') break;
            if (buf[i] == '\r' || buf[i] == '\n') {
                buf[i] = '\0';
                break;
            }
        }
        ESP_LOGI(PLAYBACK_TAG, "handling: %s", buf);

        // look for a comma, indicating the end of the "ticks" part
        // TODO: replace with strtok
        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_TAG, "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;
            }
            i++;
        }
        if (colon_pos == 0) {
            ESP_LOGE(PLAYBACK_TAG, "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_TAG, "Playback is behind by %ld ticks!", ticks_to_wait);
        }
        if (ticks_to_wait > 0) {
            vTaskDelay(ticks_to_wait);
        }
        
        bool matched = false;
        for (const auto& fn : replay_fns) {
            matched = (fn)(event_name, arg);
            if (matched) break;
        }
        if (!matched) {
            ESP_LOGW(PLAYBACK_TAG, "Failed to match event: %s!", event_name);
        }
    }

    free(buf);
    playback_task_handle = NULL;

    stop_playback();
    vTaskDelete(NULL);
}

void register_replay_fn(bool (*replay_fn)(const char*, char*)) {
    replay_fns.push_back(replay_fn);
}

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", 4096, NULL, 2, &playback_task_handle);
    return true;
}

bool stop_playback() {
    if (state != STATE_PLAYBACK) return false;
    
    state = STATE_IDLE;
    if (should_close_playback_stream) {
        fclose(playback_stream);
        playback_stream = nullptr;
    }

    if (playback_task_handle != nullptr) {
        // TODO: NO!! This leaks the malloc. Instead, use a queue to request a graceful stop
        vTaskDelete(playback_task_handle);
        playback_task_handle = NULL;
    }
    return true;
}

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