#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;
	float lowpass;
}samples_t;

static u32 sample_task(void *);
static samples_t _vbus;
#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

void samples_init(void){
	_vbus.filted_value = (CONFIG_RATED_DC_VOL);
	_vbus.value = (CONFIG_RATED_DC_VOL);
	_vbus.lowpass = (0.01f); 
	sample_vbus();
#ifdef THROTTLE_CHAN
	_throttle.filted_value = (0);
	_throttle.value = (0);
	_throttle.lowpass = (0.001f); 
	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.01;
	sample_motor_temp();
#endif
#ifdef MOS_TEMP_ADC_CHAN
	mos_temp.value = mos_temp.filted_value = 0;
	mos_temp.lowpass = 0.01;
	sample_mos_temp();
#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;
}

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

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

float get_throttle_float(void) {
#ifdef THROTTLE_CHAN
	return _throttle.filted_value;
#else
	return 0.0f;
#endif
}
static u32 sample_task(void *param) {
	sample_vbus();
	sample_throttle();
	sample_uvw_phase();
	sample_motor_temp();
	sample_mos_temp();
	return 0;
}

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;
}

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) {
#ifndef MOTOR_BLUESHARK_OLD
	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) {
	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);
}