#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 {
	s16q5_t value;
	s16q5_t filted_value;
	s16q14_t lowpass;
}samples_t;

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

static u32 sample_task(void *);
static samples_t _vbus;
#ifndef GD32_FOC_DEMO
static samples_t _throttle;
#endif
void samples_init(void){
	_vbus.filted_value = S16Q5(MAX_vDC);
	_vbus.value = S16Q5(MAX_vDC);
	_vbus.lowpass = S16Q14(0.2f); 

#ifndef GD32_FOC_DEMO
	_throttle.filted_value = (0);
	_throttle.value = (0);
	_throttle.lowpass = S16Q14(0.2f); 

	sample_throttle();
#endif
	sample_vbus();

	shark_task_create(sample_task, NULL);
}



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

s16 get_vbus_sfix5(void){
	return _vbus.filted_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;
}
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 = S16_mul(vadc, VBUS_VOL_CEOF, 9);
	LowPass_Filter_Fix(_vbus.filted_value, _vbus.value, _vbus.lowpass);
	sapmple_delta = task_ticks_rel(ticks);
}

static void sample_throttle(void){
#ifndef GD32_FOC_DEMO	
	s16 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
	_throttle.value = S16_mul(vadc, THROTTLE_VOL_CEOF, 9);
	LowPass_Filter_Fix(_throttle.filted_value, _throttle.value, _throttle.lowpass);
#endif
}