/*
 * filter.c
 *
 *  Created on: May 15, 2025
 *      Author: 911BL
 *
 * Handles functions related to filtering the imu signals
 */
#include "middleware/filter.h"

void init_filter(filter_info* f) // Initilizes the filter
{
	for (int i = 0; i < FILTER_SIZE; i++) {
		 f->buffer[i] = 0;
	}
		f->index = 0;
}

int16_t filter_apply(filter_info* f, int16_t new_sample) // Filters a signal 
{
	int32_t sum = 0;
	int16_t filtered;

	f->buffer[f->index] = new_sample;
	f->index = (f->index + 1) % FILTER_SIZE;

	for (int i = 0; i < FILTER_SIZE; i++) {
	        sum += f->buffer[i];
	}
	filtered = (int16_t)(sum / FILTER_SIZE);
	return filtered;
}

