#include "foc/samples.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "math/fast_math.h"
#include "os/os_task.h"
#include "math/fix_math.h"

typedef struct {
	sfix6_t value;
	sfix6_t filted_value;
	sfix10_t lowpass;
}samples_t;

static void sample_vbus(void);

static void sample_timer_handler(shark_timer_t *);
static samples_t _vbus;

static shark_timer_t   sample_timer = TIMER_INIT(sample_timer, sample_timer_handler);
void samples_init(void){
	_vbus.filted_value = MAX_VBUS_VOLTAGE;
	_vbus.value = MAX_VBUS_VOLTAGE;
	_vbus.lowpass = _F2sFix10(0.2f); 

	sample_vbus();

	shark_timer_post(&sample_timer, 1);
}

s16 get_vbus_sfix6(void) {
	return _vbus.filted_value;
}

float get_vbus_float(void) {
	return sfix6toF(_vbus.filted_value);
}

static void sample_timer_handler(shark_timer_t *timer) {
	sample_vbus();
	shark_timer_post(&sample_timer, 1);
}

static void sample_vbus(void){
	s32 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 16);
	s32 fix_vbus = (vadc * ADC_REFERENCE_VOLTAGE * 45 / 4096);//1:44
	s32 fix_filter = _vbus.filted_value;
	LowPass_Filter(fix_filter, fix_vbus, _vbus.lowpass); //sfix6 * sfix10
	_vbus.filted_value = fix_filter >> 10; // to sfix6
}