#include "foc/samples.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "math/fast_math.h"
#include "os/co_task.h"
#include "os/timer.h"

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

static void sample_vbus(void);
static void sample_gas(void);
static void sample_timer_handler(timer_t *);
static samples_t _vbus;
static samples_t _gas;
static 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 = 0.2f;
	_gas.filted_value = 0.0f;
	_gas.value = 0.0f;
	_gas.lowpass = 0.2f;

	sample_vbus();
	sample_gas();
	timer_post(&sample_timer, 1);
}

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

float get_gas_sample(void) {
	return _gas.filted_value;
}


static void sample_timer_handler(timer_t *timer) {
	sample_vbus();
	sample_gas();
	timer_post(&sample_timer, 5);
}

static void sample_vbus(void){
	u32 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 16);
	_vbus.value = ((float)vadc)/(4096.0f) * ADC_REFERENCE_VOLTAGE * 45 / 1000; //1:44
	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
}

static void sample_gas(void){
#ifdef GD32_DEMO
#else
	u32 vadc = adc_sample_regular_channel(HANDLERBAR_CHAN, 16);
	_gas.value = vadc * ADC_REFERENCE_VOLTAGE /4096.0f;
	LowPass_Filter(_gas.filted_value, _gas.value, _gas.lowpass);
#endif	
}