#include "foc/motor/motor.h"
#include "foc/motor/hall.h"
#include "math/fast_math.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"

const motor_param_t motor_params = {
	.poles = 5,
	.ld = 0.578477f,
	.lq = 5.78477f,
	.rs = 1.088f,
	.inertia = 3.319367f,
	.b_emf = 4.332566f,
};

static bool _motor_started = false;

void motor_start() {
	if (_motor_started) {
		return;
	}
	pwm_start();
	adc_start_convert();
	_motor_started = true;
}

void motor_stop() {
	if (!_motor_started) {
		return;
	}
	adc_stop_convert();
	pwm_stop();
	_motor_started = false;
}

void motor_drvier_low_side(bool on){
	if (on) {
		pwm_turn_on_low_side();
	}
}

u32  motor_get_raw_speed(void) {
	return (u32)hall_sensor_get_speed();
}

u32  motor_get_speed(void) {
	static u32 _speed = 0;
	float raw_speed = hall_sensor_get_speed();
	if (raw_speed == 0.0f) {
		_speed = 0;
	}
	_speed = LowPass_Filter(_speed, raw_speed, 0.8f);
	return _speed;
}


void motor_update_duty(s32 A, s32 B, s32 C, s32 next_a, s32 next_b, s32 next_c) {
#if SHUNT_NUM==THREE_SHUNTS_SAMPLE	
	pwm_update_duty(A, B, C);
#else
	pwm_wait_and_clear_updata();
	pwm_update_duty_dma(A, B, C, next_a, next_b, next_c);
#endif
}
void motor_update_sample(u32 samp1, u32 samp2, u8 sector) {
	pwm_update_2smaples(samp1, samp2);
#ifdef ENABLE_AUX_TIMER
	if (samp2 < FOC_PWM_Half_Period) {
		adc_update_ext_trigger(ADC_TRIGGER_PHASE);
	}else {
		adc_update_ext_trigger(ADC_TRIGGER_PHASE2);
	}
#endif	
	adc_current_sample_config(sector);
}