/**
  ******************************************************************************
  * @file    filter.c
  * @brief   实现基于滑动窗口与去极值平均的实时滤波算法
  ******************************************************************************
  * @attention
  *
  * 本模块实现了一种滑动窗口滤波算法，结合了去极值平均思想，
  * 适用于对实时采样数据进行平滑处理与抗干扰控制。
  *
  * 算法流程如下：
  * 1. 每次采集一个新的原始数据点，放入滑动窗口中；
  *    - 滑动窗口大小可配置；
  *    - 当窗口满后，每次加入新数据会替换最旧数据；
  *
  * 2. 对滑动窗口中所有数据进行去极值平均处理：
  *    - 将窗口中的数据排序；
  *    - 去除最大值和最小值；
  *    - 对中间有效数据求平均，作为最终滤波输出值；
  *
  * 特性：
  * - 实时性高，适合连续单点采样（如每1ms一个点）；
  * - 抗尖峰干扰能力强，响应速度快；
  * - 平滑程度由窗口大小控制，可灵活调节。
  *
  * 用法：
  * - 初始化每个传感器对应的 SlidingWindowFilter 实例；
  * - 每采集一个新数据点时调用 SlidingFilter_Update() 更新并获取滤波结果；
  *
  ******************************************************************************
**/

#include "filter.h"
#include <string.h>

static void QuickSort_Float(float arr[], int left, int right) {
    if (left >= right) return;
    float pivot = arr[left];
    int i = left, j = right;
    while (i < j) {
        while (i < j && arr[j] >= pivot) j--;
        arr[i] = arr[j];
        while (i < j && arr[i] <= pivot) i++;
        arr[j] = arr[i];
    }
    arr[i] = pivot;
    QuickSort_Float(arr, left, i - 1);
    QuickSort_Float(arr, i + 1, right);
}

// 初始化滑动窗口
void SlidingFilter_Init(SlidingWindowFilter* filter) {
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->index = 0;
    filter->full = 0;
}

// 滑动窗口滤波，输入为一个新采样点，输出为当前滤波值
float SlidingFilter_Update(SlidingWindowFilter* filter, float new_sample) {
    filter->buffer[filter->index] = new_sample;

    filter->index = (filter->index + 1) % SLIDING_WINDOW_SIZE;
    if (filter->index == 0) {
        filter->full = 1;
    }

    uint8_t count = filter->full ? SLIDING_WINDOW_SIZE : filter->index;

    // 拷贝当前有效窗口数据
    float temp[SLIDING_WINDOW_SIZE];
    memcpy(temp, filter->buffer, count * sizeof(float));

    // 排序
    QuickSort_Float(temp, 0, count - 1);

    // 去极值平均
    float sum = 0.0f;
    if (count > 2) {
        for (uint8_t i = 1; i < count - 1; i++) {
            sum += temp[i];
        }
        return sum / (count - 2);
    } else {
        // 不足3个数据，不做去极值处理
        sum = 0.0f;
        for (uint8_t i = 0; i < count; i++) {
            sum += temp[i];
        }
        return sum / count;
    }
}
