#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_throttle(void);

static u32 sample_task(void *);
static samples_t _vbus;
static samples_t _throttle;

void samples_init(void){
	_vbus.filted_value = i2sFix6(MAX_VBUS_VOLTAGE);
	_vbus.value = i2sFix6(MAX_VBUS_VOLTAGE);
	_vbus.lowpass = ftoSfix10(0.2f); 

	_throttle.filted_value = i2sFix6(0);
	_throttle.value = i2sFix6(0);
	_throttle.lowpass = ftoSfix10(0.2f); 

	sample_throttle();
	sample_vbus();

	shark_task_create(sample_task, NULL);
}

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

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

float get_throttle_sfix6(void) {
	return (_throttle.filted_value);
}

float get_throttle_float(void) {
	return sfix6toF(_throttle.filted_value);
}
static u32 sample_task(void *param) {
	sample_vbus();
	sample_throttle();
	return 0;
}

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

static void sample_throttle(void){
	s32 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
	s32 fix_V = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
	s32 V_filter = _throttle.filted_value;
	LowPass_Filter(V_filter, ftoSfix6(fix_V), _vbus.lowpass); //sfix6 * sfix10
	_throttle.filted_value = V_filter >> 10; // to sfix6
}