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

typedef struct {
	float value;
	float filted_value;
	int   filted_int;
	u16   adc_offset;
	float lowpass;
}samples_t;

static u32 sample_task(void *);
static samples_t _vbus;
static samples_t _vref;
#ifdef THROTTLE_CHAN
static samples_t _throttle;
#endif
#ifdef U_VOL_ADC_CHAN
static samples_t _uvw_phase[3];
#endif
#ifdef MOTOR_TEMP_ADC_CHAN
static samples_t motor_temp;
#endif
#ifdef MOS_TEMP_ADC_CHAN
static samples_t mos_temp;
#endif
#undef MOS_TEMP1_ADC_CHAN
#ifdef MOS_TEMP1_ADC_CHAN
static samples_t mos_temp1;
#endif
#ifdef ACC_V_CHAN
static samples_t acc_vol;
#endif
#ifdef VBUS_I_CHAN
static samples_t _ibus;
#endif
void samples_init(void){
	_vref.filted_value = 0;
	_vref.value = 0;
	_vref.lowpass = 0.01f;
	sample_vref();
	_vbus.filted_value = (CONFIG_RATED_DC_VOL);
	_vbus.value = (CONFIG_RATED_DC_VOL);
	_vbus.lowpass = (0.01f);
#ifdef ACC_V_CHAN
	acc_vol.filted_value = (CONFIG_RATED_DC_VOL);
	acc_vol.value = (CONFIG_RATED_DC_VOL);
	acc_vol.lowpass = (0.01f);
#endif
	sample_vbus();

#ifdef THROTTLE_CHAN
	_throttle.filted_value = (0);
	_throttle.value = (0);
	_throttle.lowpass = (0.01f); 
	sample_throttle();
#endif
#ifdef U_VOL_ADC_CHAN
	_uvw_phase[0].value = _uvw_phase[0].filted_value = 0;
	_uvw_phase[0].lowpass = 0.01f;
	_uvw_phase[1].value = _uvw_phase[1].filted_value = 0;
	_uvw_phase[1].lowpass = 0.01f;
	_uvw_phase[2].value = _uvw_phase[2].filted_value = 0;
	_uvw_phase[2].lowpass = 0.01f;
	sample_uvw_phase();
#endif
#ifdef MOTOR_TEMP_ADC_CHAN
	motor_temp.value = motor_temp.filted_value = 0;
	motor_temp.lowpass = 0.01f;
	sample_motor_temp();
#endif
#ifdef MOS_TEMP_ADC_CHAN
	mos_temp.value = mos_temp.filted_value = 0;
	mos_temp.lowpass = 0.01f;
	sample_mos_temp();
#endif
#ifdef MOS_TEMP1_ADC_CHAN
	mos_temp1.value = mos_temp1.filted_value = 0;
	mos_temp1.lowpass = 0.01f;
	sample_mos_temp();
#endif
#ifdef VBUS_I_CHAN
	_ibus.value = _ibus.filted_value = 0;
	_ibus.lowpass = 0.01f;
	sample_ibus();
#endif
	shark_task_create(sample_task, NULL);
}


void get_phase_vols(float *uvw) {
	uvw[0] = _uvw_phase[0].filted_value;
	uvw[1] = _uvw_phase[1].filted_value;
	uvw[2] = _uvw_phase[2].filted_value;
}

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

int get_vbus_int(void){
	return _vbus.filted_int;
}

int get_acc_vol(void) {
#ifdef ACC_V_CHAN
	return acc_vol.filted_int;
#else
	return get_vbus_int();
#endif
}

float get_vbus_current(void) {
#ifdef VBUS_I_CHAN
	s16 ibus = _ibus.filted_value * 10.0f;
	return (float)ibus/10.0f;
#else
	return 0;
#endif
}

s16 get_motor_temp(void) {
	return motor_temp.filted_value;
}

s16 get_mos_temp(void) {
	return mos_temp.filted_value;
}
s16 get_mos_temp2(void) {
#ifdef MOS_TEMP1_ADC_CHAN
	return mos_temp1.filted_value;
#else
	return get_mos_temp();
#endif
}

float get_throttle_float(void) {
#ifdef THROTTLE_CHAN
	return _throttle.filted_value;
#else
	return 0.0f;
#endif
}

float get_adc_vref(void) {
	return _vref.filted_value;
}

static u32 sample_task(void *param) {
	sample_vbus();
	sample_ibus();
	sample_throttle();
	sample_uvw_phase();
	sample_motor_temp();
	sample_mos_temp();
	sample_vref();
	return 0;
}

void sample_vref(void) {
	float vadc = adc_get_vref();
	_vref.value = ADC_REFERENCE_VOLTAGE * vadc / ADC_FULL_MAX;
	LowPass_Filter(_vref.filted_value, _vref.value, _vref.lowpass);
}

void sample_vbus(void){
	u32 ticks = task_ticks_abs();
	s16 vadc = adc_get_vbus();
	_vbus.value = (float)vadc * VBUS_VOL_CEOF;
	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
	_vbus.filted_int = (int)_vbus.filted_value;
#ifdef ACC_V_CHAN
	vadc = adc_get_acc();
	acc_vol.value = (float)vadc * VBUS_VOL_CEOF;
	LowPass_Filter(acc_vol.filted_value, acc_vol.value, acc_vol.lowpass);
	acc_vol.filted_int = (int)acc_vol.filted_value;
#endif
}

void sample_ibus(void) {
#ifdef VBUS_I_CHAN
	s16 vadc = _ibus.adc_offset - adc_get_ibus();
	_ibus.value = (float)vadc * VBUS_I_CEOF;
	LowPass_Filter(_ibus.filted_value, _ibus.value, _ibus.lowpass);
#endif
}

void sample_ibus_offset(u16 offset) {
#ifdef VBUS_I_CHAN
	_ibus.adc_offset = offset;
#endif
}

void sample_throttle(void){
#ifdef THROTTLE_CHAN
	s16 vadc = adc_get_throttle();
	_throttle.value = (float)vadc * THROTTLE_VOL_CEOF;
	LowPass_Filter(_throttle.filted_value, _throttle.value, _throttle.lowpass);
#endif
}
void sample_uvw_phase(void) {
#ifdef U_VOL_ADC_CHAN
	u16 uvw[3];
	adc_get_uvw_phaseV(uvw);

	_uvw_phase[0].value = (float)uvw[0] * UVW_VOL_CEOF;
	LowPass_Filter(_uvw_phase[0].filted_value, _uvw_phase[0].value, _uvw_phase[0].lowpass);

	_uvw_phase[1].value = (float)uvw[1] * UVW_VOL_CEOF;
	LowPass_Filter(_uvw_phase[1].filted_value, _uvw_phase[1].value, _uvw_phase[1].lowpass);

	_uvw_phase[2].value = (float)uvw[2] * UVW_VOL_CEOF;
	LowPass_Filter(_uvw_phase[2].filted_value, _uvw_phase[2].value, _uvw_phase[2].lowpass);

#endif
}

void sample_motor_temp(void) {
#ifdef CONFIG_BOARD_MCXXX
	u16 adc = adc_get_motor_temp();
	u16 r = MOTOR_TEMP_R(adc);
	motor_temp.value = ntc_get_motor_temp(r);
	LowPass_Filter(motor_temp.filted_value, motor_temp.value, motor_temp.lowpass);
#endif
}

void sample_mos_temp(void) {
#ifdef CONFIG_BOARD_MCXXX
	u16 adc = adc_get_mos_temp();
	u16 r = MOS_TEMP_R(adc);
	mos_temp.value = ntc_get_mos_temp(r);
	LowPass_Filter(mos_temp.filted_value, mos_temp.value, mos_temp.lowpass);
#ifdef MOS_TEMP1_ADC_CHAN
	adc = adc_get_mos_temp2();
	r = MOS_TEMP_R(adc);
	mos_temp1.value = ntc_get_mos_temp(r);
	LowPass_Filter(mos_temp1.filted_value, mos_temp1.value, mos_temp1.lowpass);
#endif
#endif
}