// input_filter.c
#include "input_filter.h"
#include "esp_log.h"
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <stdint.h>

static const char *TAG = "inputfilter";
static TickType_t last_data_time = 0;

#define MIN_TAU_MS 100  // Minimum time constant in ms

double getFactor(uint32_t delta_ms, uint32_t tau_ms)
{
    if (tau_ms < MIN_TAU_MS)
    {
        tau_ms = MIN_TAU_MS;
    }
    double factor = 1.0 - exp(-((double)delta_ms / (double)tau_ms));
    ESP_LOGD(TAG, "delta_ms=%" PRIu32 ", tau_ms=%" PRIu32 ", factor=%f", delta_ms, tau_ms, factor);
    return factor;
}

double filter(double input, double filtered, uint32_t tau_ms)
{
    TickType_t now = xTaskGetTickCount();
    uint32_t delta_ms = (now - last_data_time) * portTICK_PERIOD_MS;
    last_data_time = now;
    
    double factor = getFactor(delta_ms, tau_ms);
    filtered = input * factor + filtered * (1.0 - factor);
    
    ESP_LOGD(TAG, "input=%f, filtered=%f", input, filtered);
    return filtered;
}
