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

typedef struct {
	float value;
	float filted_value;
	float lowpass;
	s16   fix_value;
}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 = (MAX_vDC);
	_vbus.value = (MAX_vDC);
	_vbus.lowpass = (0.2f); 

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

	sample_throttle();
	sample_vbus();

	shark_task_create(sample_task, NULL);
}



float get_vbus_float(void) {
	return (float)((s32)(_vbus.filted_value * 100.0f)/100.0f);
}

s16 get_vbus_sfix5(void){
	return _vbus.fix_value;
}


float get_throttle_float(void) {
	return 0.0f;//(float)((s32)(_throttle.filted_value * 100.0f)/100.0f);
}
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);
	float fix_vbus = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
	LowPass_Filter(_vbus.filted_value, (fix_vbus), _vbus.lowpass);
	_vbus.fix_value = S16Q5toF(_vbus.filted_value);
}

static void sample_throttle(void){
	s32 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
	float fix_V = ((float)vadc * ADC_REFERENCE_VOLTAGE * 45.0F / 4096.0F);//1:44
	LowPass_Filter(_throttle.filted_value, (fix_V), _throttle.lowpass);
	_throttle.fix_value = S16Q5toF(_throttle.filted_value);
}