#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"

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;
	foc->motor_s.theta = hall_sensor_get_theta();
	if (foc->override_p.is_override_theta) {
		foc->motor_s.theta = foc->override_p.theta;
	}
	get_phase_current(c_sample);
	Clark(c_sample->ia, c_sample->ib, c_sample->ic, &sample_ab);
	Park(&sample_ab, foc->motor_s.theta, &sample_dq);

	if (foc->mode == FOC_MODE_PI_DQ || foc->mode == FOC_MODE_PI_FULL) {
		v_dq.d = pi_control(&foc->PI_id, foc->dq_ref.d - sample_dq.d);
		v_dq.q = pi_control(&foc->PI_iq, foc->dq_ref.q - sample_dq.q);
	}else {
		v_dq.d = foc->dq_ref.d;
		v_dq.q = foc->dq_ref.q;
	}
	if (foc->override_p.is_override_v_dq) {
		v_dq.d = foc->override_p.v_dq.d;
		v_dq.q = foc->override_p.v_dq.q;
	}
	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);

	svpwm(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
	
	u32 sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
	
	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
}