#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "init.h"
#include "tool.h"
#include "filter.h"

static void test_median(void)
{
    double data[] = {1, 2, 3, 2.5, 3.5, 2, 3, 4, 5};
    int size = sizeof(data) / sizeof(data[0]);
    int windowSize = 3;

    filter_median(data, size, windowSize);

    printf("Filtered data: \n");
    for (int i = 0; i < size; i++) 
    {
        printf("%f ", data[i]);
    }
    printf("\n");
}

static void test_average(void)
{
    double data[] = {1, 2, 3, 2.5, 3.5, 2, 3, 4, 5};
    int size = sizeof(data) / sizeof(data[0]);
    int windowSize = 3;
    
    filter_average(data, size, windowSize);

    printf("Filtered data: \n");
    for (int i = 0; i < size; i++) 
    {
        printf("%f ", data[i]);
    }
    printf("\n");
}

static void test_kalman(void)
{
    double measurements[] = {1.0, 2.0, 3.0, 2.5, 3.5, 2.0, 3.0, 4.0, 5.0};
    double estimates[1];
    int numMeasurements = sizeof(measurements) / sizeof(measurements[0]);
    double processNoise = 0.1;
    double measurementNoise = 0.2;
    
    filter_kalman(measurements, estimates, numMeasurements, processNoise, measurementNoise);

    printf("Estimated value: %f\n", estimates[0]);
}

static void test(void)
{
    printf("pid test!\r\n");
    
    test_median();
    // test_average();
    // test_kalman();
}
init_export_app(test);