#include "hal/hal.h"
#include "hal/pwm.h"
#include "foc_task.h"
#include "phase_current.h"
#include "park_clark.h"
#include "hall_sensor.h"
#include "svpwm.h"

static void __inline foc_update_theta(motor_foc_t *foc) {
	float angle = 0.0f;
	if (foc->override_p.is_override_theta) {
		angle = foc->override_p.theta;
	}else {
		angle = hall_sensor_get_theta();
	}
	foc->motor_s.theta = degree_2_pi(angle);
}

static void __inline foc_update_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
	if (foc->mode == FOC_MODE_PI_DQ || foc->mode == FOC_MODE_PI_FULL) {
		ref_out->d = pi_control(&foc->PI_id, foc->dq_ref.d - sampled->d);
		ref_out->q = pi_control(&foc->PI_iq, foc->dq_ref.q - sampled->q);
	}else {
		ref_out->d = foc->dq_ref.d;
		ref_out->q = foc->dq_ref.q;
	}
	if (foc->override_p.is_override_v_dq) {
		ref_out->d = foc->override_p.v_dq.d;
		ref_out->q = foc->override_p.v_dq.q;
	}
}


void foc_task(motor_foc_t *foc){
	current_samp_t *c_sample = &foc->current_samp;
	alpha_beta_t sample_ab, pwm_ab;
	dq_t         sample_dq, v_dq;
	phase_time_t         phase_time;
	u32 sample_point;

	/* 更新电角度 */
	foc_update_theta(foc);
	/* 采集3相电流 */
	phase_current_sample(c_sample);
	/* ABC坐标转alpha-beta坐标 */
	Clark(c_sample->ia, c_sample->ib, c_sample->ic, &sample_ab);
	/* alpha-beta坐标转旋转坐标系D-Q */
	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
	/* 处理D,Q电流环，速度环低频运行，不在此处处理*/
	foc_update_PI_Contrl(foc, &sample_dq, &v_dq);
	/* d-q坐标转 alpha-beta坐标，输入给pwm */
	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
	/* 生成 pwm，模拟正弦波,此处vbus需要动态采集 */
	svpwm(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
	/* 通过扇区和pwm duty 选择合适的3相电流采样点 */
	sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
	/* 更新duty和采样点到硬件TIM1中 */
	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
}