#include "foc/samples.h"
#include "bsp/bsp_driver.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 "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[2];
#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
#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.value = sample_vbus_raw();
	_vbus.filted_value = _vbus.value;
	_vbus.lowpass = (0.01f);
#ifdef ACC_V_CHAN
	acc_vol.value = sample_acc_vol_raw();
	acc_vol.filted_value = acc_vol.value;
	acc_vol.lowpass = (0.01f);
#endif
	sample_vbus();

#ifdef THROTTLE_CHAN
	_throttle[0].value = (float)adc_get_throttle() * THROTTLE_VOL_CEOF;
	_throttle[0].filted_value = _throttle[0].value;
	_throttle[0].lowpass = (0.2f); //200 rad/s bandwith, sample s in 1ms task
#ifdef THROTTLE2_CHAN
	_throttle[1].value = (float)adc_get_throttle2() * THROTTLE_VOL_CEOF;
	_throttle[1].filted_value = _throttle[1].value;
	_throttle[1].lowpass = (0.2f);
#endif
	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.1f;
	sample_motor_temp();
#endif
#ifdef MOS_TEMP_ADC_CHAN
	mos_temp.value = mos_temp.filted_value = 0;
	mos_temp.lowpass = 0.1f;
	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_filtered(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 NO_VALID_CURRENT;
#endif
}

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

s16 get_mos_temp(void) {
	return mos_temp.filted_value;
}

s16 get_motor_temp_raw(void) {
	return motor_temp.value;
}

s16 get_mos_temp_raw(void) {
	return mos_temp.value;
}

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

float get_throttle2_float(void) {
#ifdef THROTTLE2_CHAN
	return _throttle[1].filted_value;
#else
	return get_throttle_float();
#endif
}


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

float get_thro_5v_float(void) {
	u16 adc = adc_get_thro_5v();
	if (adc == 0xFFFF) {
		return 5.0f;
	}
	return (float)adc * (ADC_REFERENCE_VOLTAGE*(2.0f)/ADC_FULL_MAX);
}

float get_thro2_5v_float(void) {
	u16 adc = adc_get_thro2_5v();
	if (adc == 0xFFFF) {
		return 5.0f;
	}
	return (float)adc * (ADC_REFERENCE_VOLTAGE*(2.0f)/ADC_FULL_MAX);
}


static u32 sample_task(void *param) {
	sample_vbus();
	sample_ibus();
	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);
}

float sample_vbus_raw(void) {
	s16 vadc = adc_get_vbus();
	return (float)vadc * VBUS_VOL_CEOF;
}

float sample_acc_vol_raw(void) {
	s16 vadc = adc_get_acc();
	return (float)vadc * VBUS_VOL_CEOF;
}

void sample_vbus(void){
	_vbus.value = sample_vbus_raw();
	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
	_vbus.filted_int = (int)_vbus.filted_value;
#ifdef ACC_V_CHAN
	acc_vol.value = sample_acc_vol_raw();
	LowPass_Filter(acc_vol.filted_value, acc_vol.value, acc_vol.lowpass);
	acc_vol.filted_int = (int)acc_vol.filted_value;
#endif
}

float sample_ibus_raw(void) {
#ifdef CONFIG_VBUS_I_POSITIVE
	s16 vadc = adc_get_ibus() - _ibus.adc_offset;
#else
	s16 vadc = _ibus.adc_offset - adc_get_ibus();
#endif
	return (float)vadc * VBUS_I_CEOF;
}

void sample_ibus(void) {
#ifdef VBUS_I_CHAN
	_ibus.value = sample_ibus_raw();
	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[0].value = (float)vadc * THROTTLE_VOL_CEOF;
	LowPass_Filter(_throttle[0].filted_value, _throttle[0].value, _throttle[0].lowpass);
#endif
#ifdef THROTTLE2_CHAN
	s16 vadc2 = adc_get_throttle2();
	_throttle[1].value = (float)vadc2 * THROTTLE_VOL_CEOF;
	LowPass_Filter(_throttle[1].filted_value, _throttle[1].value, _throttle[1].lowpass);
#endif

}

void get_uvw_phases_raw(float *uvw_raw) {
	u16 uvw[3];
	adc_get_uvw_phaseV(uvw);
	uvw_raw[0] = ((float)uvw[0] * UVW_VOL_CEOF);
	uvw_raw[1] = ((float)uvw[1] * UVW_VOL_CEOF);
	uvw_raw[2] = ((float)uvw[2] * UVW_VOL_CEOF);
}

void sample_uvw_phase(void) {
#ifdef U_VOL_ADC_CHAN
	float uvw[3];
	get_uvw_phases_raw(uvw);

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

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

	_uvw_phase[2].value = uvw[2];
	LowPass_Filter(_uvw_phase[2].filted_value, _uvw_phase[2].value, _uvw_phase[2].lowpass);
#endif
}

s16 sample_motor_temp(void) {
#ifdef CONFIG_BOARD_MCXXX
	u16 adc = adc_get_motor_temp();
	u32 r = (u32)MOTOR_TEMP_R(adc);
	if (r > 65535) {
		r = 65535;
	}
	motor_temp.value = ntc_get_motor_temp(r);
	LowPass_Filter(motor_temp.filted_value, motor_temp.value, motor_temp.lowpass);
	return (s16)motor_temp.value;
#else
	return 0;
#endif
}

s16 sample_mos_temp(void) {
#ifdef CONFIG_BOARD_MCXXX
	u16 adc = adc_get_mos_temp();
	u32 r = (u32)MOS_TEMP_R(adc);
	if (r > 65535) {
		r = 65535;
	}
	mos_temp.value = ntc_get_mos_temp(r);
	LowPass_Filter(mos_temp.filted_value, mos_temp.value, mos_temp.lowpass);
	return (s16)mos_temp.value;
#else
	return 0;
#endif
}

void sample_log(void) {
	sys_debug("Thro Vol %f-%f, %f-%f\n", get_thro_5v_float(), get_throttle_float(), get_thro2_5v_float(), get_throttle2_float());
}