/**
******************************************************************************
* @file    filters.c
* @brief   Реализация библиотеки фильтров
******************************************************************************
*/
#include "filters.h"

#ifdef FILTERS_ENABLE

// ==================== FLOAT ВЕРСИИ ====================

// Вспомогательная функция для сравнения float
static int Filter_float_compare(const void *a, const void *b) {
    float fa = *(const float*)a;
    float fb = *(const float*)b;
    if (fa < fb) return -1;
    if (fa > fb) return 1;
    return 0;
}

/**
  * @brief Инициализация медианного фильтра (float)
  * @param filter Указатель на структуру фильтра
  */
void FilterMedian_Init(FilterMedian_t* filter, uint8_t size) {
    if (filter == NULL) return;
    if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
    
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->index = 0;
    filter->size = size;
    
}

/**
  * @brief Обработка значения медианным фильтром (float)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
float FilterMedian_Process(FilterMedian_t* filter, float input) {
    if (filter == NULL) return input;
    
    // Добавляем значение в буфер
    filter->buffer[filter->index] = input;
    filter->index = (filter->index + 1) % filter->size;
    
    // Копируем буфер для сортировки
    float sort_buffer[FILTER_MEDIAN_MAX_SIZE];
    memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
    
    // Сортируем и возвращаем медиану
    qsort(sort_buffer, filter->size, sizeof(float), Filter_float_compare);
    return sort_buffer[filter->size / 2];
}

/**
  * @brief Инициализация экспоненциального фильтра (float)
  * @param filter Указатель на структуру фильтра
  * @param alpha Коэффициент сглаживания (0..1)
  */
void FilterExp_Init(FilterExp_t* filter, float alpha) {
    if (filter == NULL) return;
    
    filter->alpha = alpha;
    filter->value = 0.0f;
    filter->initialized = 0;
}

/**
  * @brief Обработка значения экспоненциальным фильтром (float)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
float FilterExp_Process(FilterExp_t* filter, float input) {
    if (filter == NULL) return input;
    
    if (!filter->initialized) {
        filter->value = input;
        filter->initialized = 1;
        return input;
    }
    
    filter->value = filter->alpha * input + (1.0f - filter->alpha) * filter->value;
    return filter->value;
}

/**
  * @brief Инициализация фильтра скользящего среднего (float)
  * @param filter Указатель на структуру фильтра
  */
void FilterAverage_Init(FilterAverage_t* filter, uint8_t size) {
    if (filter == NULL) return;
    if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
    
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->size = size;
    filter->sum = 0.0f;
    filter->index = 0;
    filter->count = 0;
}

/**
  * @brief Обработка значения фильтром скользящего среднего (float)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
float FilterAverage_Process(FilterAverage_t* filter, float input) {
    if (filter == NULL) return input;
    
    // Вычитаем старое значение из суммы
    if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
        filter->sum -= filter->buffer[filter->index];
    } else {
        filter->count++;
    }
    
    // Добавляем новое значение
    filter->buffer[filter->index] = input;
    filter->sum += input;
    filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
    
    return filter->sum / filter->count;
}

/**
  * @brief Инициализация полиномиальной коррекции (float)
  * @param filter Указатель на структуру коррекции
  * @param coeffs Массив коэффициентов полинома
  * @param order Порядок полинома
  * @return 0 - успех, -1 - ошибка
  */
int FilterPoly_Init(FilterPoly_t* filter, float* coeffs, uint8_t order) {
    if (filter == NULL || coeffs == NULL) return -1;
    if (order > FILTER_POLY_MAX_ORDER) return -1;
    
    filter->order = order;
    memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(float));
    return 0;
}

/**
  * @brief Применение полиномиальной коррекции к значению (float)
  * @param filter Указатель на структуру коррекции
  * @param input Входное значение
  * @return Скорректированное значение
  */
float FilterPoly_Process(FilterPoly_t* filter, float input) {
    if (filter == NULL) return input;
    
    float result = 0.0f;
    float x_power = 1.0f;
    
    for (uint8_t i = 0; i <= filter->order; i++) {
        result += filter->coefficients[i] * x_power;
        x_power *= input;
    }
    
    return result;
}

/**
  * @brief Инициализация табличного фильтра (float)
  * @param filter Указатель на структуру фильтра
  * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию)
  * @param output_arr Массив выходных значений
  * @param size Размер таблицы
  * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
  */
void FilterLUT_Init(FilterLUT_t* filter, float* input_arr, float* output_arr, uint16_t size, uint8_t interpolation) {
    if (filter == NULL || input_arr == NULL || output_arr == NULL) return;
    
    filter->input_values = input_arr;
    filter->output_values = output_arr;
    filter->size = size;
    filter->interpolation = interpolation;
}

/**
  * @brief Обработка значения табличным фильтром (float)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Выходное значение по таблице
  */
float FilterLUT_Process(FilterLUT_t* filter, float input) {
    if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
        return input;
    }
    
    // Поиск ближайших значений в таблице
    uint16_t left_index = 0;
    uint16_t right_index = filter->size - 1;
    
    // Если значение за пределами таблицы - возвращаем крайние значения
    if (input <= filter->input_values[0]) {
        return filter->output_values[0];
    }
    if (input >= filter->input_values[right_index]) {
        return filter->output_values[right_index];
    }
    
    // Бинарный поиск позиции
    while (right_index - left_index > 1) {
        uint16_t mid_index = left_index + (right_index - left_index) / 2;
        if (input <= filter->input_values[mid_index]) {
            right_index = mid_index;
        } else {
            left_index = mid_index;
        }
    }
    
    // Без интерполяции - возвращаем значение левой границы
    if (!filter->interpolation) {
        return filter->output_values[left_index];
    }
    
    // Линейная интерполяция
    float x0 = filter->input_values[left_index];
    float x1 = filter->input_values[right_index];
    float y0 = filter->output_values[left_index];
    float y1 = filter->output_values[right_index];
    
    if (x1 == x0) {
        return y0; // Избегаем деления на ноль
    }
    
    return y0 + (input - x0) * (y1 - y0) / (x1 - x0);
}
// ==================== INT32_T ВЕРСИИ ====================

// Вспомогательная функция для сравнения int32_t
static int Filter_int32_compare(const void *a, const void *b) {
    int32_t ia = *(const int32_t*)a;
    int32_t ib = *(const int32_t*)b;
    if (ia < ib) return -1;
    if (ia > ib) return 1;
    return 0;
}

/**
  * @brief Инициализация медианного фильтра (int32_t)
  * @param filter Указатель на структуру фильтра
  */
void FilterMedianInt_Init(FilterMedianInt_t* filter, uint8_t size) {
    if (filter == NULL) return;
    if (size == 0 || size > FILTER_MEDIAN_MAX_SIZE) return;
    
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->index = 0;
    filter->size = size;
}

/**
  * @brief Обработка значения медианным фильтром (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
int32_t FilterMedianInt_Process(FilterMedianInt_t* filter, int32_t input) {
    if (filter == NULL) return input;
    
    // Добавляем значение в буфер
    filter->buffer[filter->index] = input;
    filter->index = (filter->index + 1) % filter->size;
    
    // Копируем буфер для сортировки
    int32_t sort_buffer[FILTER_MEDIAN_MAX_SIZE];
    memcpy(sort_buffer, filter->buffer, sizeof(sort_buffer));
    
    // Сортируем и возвращаем медиану
    qsort(sort_buffer, filter->size, sizeof(int32_t), Filter_int32_compare);
    return sort_buffer[filter->size / 2];
}

/**
  * @brief Инициализация экспоненциального фильтра (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param alpha Коэффициент сглаживания в масштабированном виде
  * @param scale Масштаб коэффициента (например 100 для работы с процентами)
  */
void FilterExpInt_Init(FilterExpInt_t* filter, int32_t alpha, int32_t scale) {
    if (filter == NULL) return;
    
    filter->alpha = alpha;
    filter->scale = scale;
    filter->value = 0;
    filter->initialized = 0;
}

/**
  * @brief Обработка значения экспоненциальным фильтром (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
int32_t FilterExpInt_Process(FilterExpInt_t* filter, int32_t input) {
    if (filter == NULL) return input;
    
    if (!filter->initialized) {
        filter->value = input;
        filter->initialized = 1;
        return input;
    }
    
    // value = (alpha * input + (scale - alpha) * value) / scale
    int64_t result = (int64_t)filter->alpha * input + 
                    (int64_t)(filter->scale - filter->alpha) * filter->value;
    filter->value = (int32_t)(result / filter->scale);
    
    return filter->value;
}

/**
  * @brief Инициализация фильтра скользящего среднего (int32_t)
  * @param filter Указатель на структуру фильтра
  */
void FilterAverageInt_Init(FilterAverageInt_t* filter, uint8_t size) {
    if (filter == NULL) return;
    if (size == 0 || size > FILTER_AVERAGE_MAX_SIZE) return;
    
    memset(filter->buffer, 0, sizeof(filter->buffer));
    filter->size = size;
    filter->sum = 0;
    filter->index = 0;
    filter->count = 0;
}

/**
  * @brief Обработка значения фильтром скользящего среднего (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Отфильтрованное значение
  */
int32_t FilterAverageInt_Process(FilterAverageInt_t* filter, int32_t input) {
    if (filter == NULL) return input;
    
    // Вычитаем старое значение из суммы
    if (filter->count == FILTER_AVERAGE_MAX_SIZE) {
        filter->sum -= filter->buffer[filter->index];
    } else {
        filter->count++;
    }
    
    // Добавляем новое значение
    filter->buffer[filter->index] = input;
    filter->sum += input;
    filter->index = (filter->index + 1) % FILTER_AVERAGE_MAX_SIZE;
    
    return (int32_t)(filter->sum / filter->count);
}

/**
  * @brief Инициализация полиномиальной коррекции (int32_t)
  * @param filter Указатель на структуру коррекции
  * @param coeffs Массив коэффициентов полинома в масштабированном виде
  * @param order Порядок полинома
  * @param scale Масштаб коэффициентов
  * @return 0 - успех, -1 - ошибка
  */
int FilterPolyInt_Init(FilterPolyInt_t* filter, int32_t* coeffs, uint8_t order, int32_t scale) {
    if (filter == NULL || coeffs == NULL) return -1;
    if (order > FILTER_POLY_MAX_ORDER) return -1;
    
    filter->order = order;
    filter->scale = scale;
    memcpy(filter->coefficients, coeffs, (order + 1) * sizeof(int32_t));
    return 0;
}

/**
  * @brief Применение полиномиальной коррекции к значению (int32_t)
  * @param filter Указатель на структуру коррекции
  * @param input Входное значение
  * @return Скорректированное значение
  */
int32_t FilterPolyInt_Process(FilterPolyInt_t* filter, int32_t input) {
    if (filter == NULL) return input;
    
    // coefficients[0] = a_n * scale
    // coefficients[1] = a_{n-1} * scale  
    // ...
    // coefficients[n] = a_0 * scale
    
    int64_t result = filter->coefficients[0]; // Старший коэффициент
    int64_t x_scaled = input;
    
    for (uint8_t i = 1; i <= filter->order; i++) {
        result = (result * x_scaled) / filter->scale + filter->coefficients[i];
    }
    
    // Домножаем на scale для a_0
    result = (result * filter->scale);
    
    return (int32_t)(result / filter->scale);
}

/**
  * @brief Инициализация табличного фильтра (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param input_arr Массив входных значений (должен быть отсортирован по возрастанию)
  * @param output_arr Массив выходных значений
  * @param size Размер таблицы
  * @param interpolation Флаг интерполяции (0 - ближайшее значение, 1 - линейная интерполяция)
  */
void FilterLUTInt_Init(FilterLUTInt_t* filter, int32_t* input_arr, int32_t* output_arr, uint16_t size, uint8_t interpolation) {
    if (filter == NULL || input_arr == NULL || output_arr == NULL) return;
    
    filter->input_values = input_arr;
    filter->output_values = output_arr;
    filter->size = size;
    filter->interpolation = interpolation;
}

/**
  * @brief Обработка значения табличным фильтром (int32_t)
  * @param filter Указатель на структуру фильтра
  * @param input Входное значение
  * @return Выходное значение по таблице
  */
int32_t FilterLUTInt_Process(FilterLUTInt_t* filter, int32_t input) {
    if (filter == NULL || filter->input_values == NULL || filter->output_values == NULL) {
        return input;
    }
    
    // Поиск ближайших значений в таблице
    uint16_t left_index = 0;
    uint16_t right_index = filter->size - 1;
    
    // Если значение за пределами таблицы - возвращаем крайние значения
    if (input <= filter->input_values[0]) {
        return filter->output_values[0];
    }
    if (input >= filter->input_values[right_index]) {
        return filter->output_values[right_index];
    }
    
    // Бинарный поиск позиции
    while (right_index - left_index > 1) {
        uint16_t mid_index = left_index + (right_index - left_index) / 2;
        if (input <= filter->input_values[mid_index]) {
            right_index = mid_index;
        } else {
            left_index = mid_index;
        }
    }
    
    // Без интерполяции - возвращаем значение левой границы
    if (!filter->interpolation) {
        return filter->output_values[left_index];
    }
    
    // Линейная интерполяция (целочисленная)
    int64_t x0 = filter->input_values[left_index];
    int64_t x1 = filter->input_values[right_index];
    int64_t y0 = filter->output_values[left_index];
    int64_t y1 = filter->output_values[right_index];
    
    if (x1 == x0) {
        return (int32_t)y0; // Избегаем деления на ноль
    }
    
    int64_t result = y0 + (input - x0) * (y1 - y0) / (x1 - x0);
    return (int32_t)result;
}
#endif // FILTERS_ENABLE