#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"
#include "bsp/delay.h"

typedef struct {
	float value;
	float filted_value;
	float lowpass;
}samples_t;

static void sample_vbus(void);
static void sample_throttle(void);

static u32 sample_task(void *);
static samples_t _vbus;
#ifdef THROTTLE_CHAN
static samples_t _throttle;
#endif
void samples_init(void){
	_vbus.filted_value = (MAX_vDC);
	_vbus.value = (MAX_vDC);
	_vbus.lowpass = (0.2f); 

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

	sample_throttle();
#endif
	sample_vbus();

	shark_task_create(sample_task, NULL);
}



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

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


float get_throttle_float(void) {
	return _throttle.filted_value;
}
static u32 sample_task(void *param) {
	sample_vbus();
	sample_throttle();
	return 0;
}
u32 sapmple_delta;
static void sample_vbus(void){
	u32 ticks = task_ticks_abs();
	s16 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 3);
	_vbus.value = (float)vadc * VBUS_VOL_CEOF;
	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
	sapmple_delta = task_ticks_rel(ticks);
}

static void sample_throttle(void){
#ifdef THROTTLE_CHAN
	s16 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
	_throttle.value = (float)vadc * THROTTLE_VOL_CEOF;
	LowPass_Filter(_throttle.filted_value, _throttle.value, _throttle.lowpass);
#endif
}