#include "foc.h"
#include "arm_math.h"
#include "math/fix_math.h"
#include "math/fast_math.h"
#include "foc/core/svpwm.h"
#include "libs/logger.h"
#include "bsp/bsp_driver.h"
#include "foc/mc_config.h"
#include "foc/foc_config.h"
#include "foc/motor/current.h"


void foc_init(foc_t *foc) {
	PI_Controller_Reset(&foc->daxis, 0);
	PI_Controller_Reset(&foc->qaxis, 0);

	foc->daxis.kp = mc_conf()->c.pid[PID_ID_ID].kp;
	foc->daxis.ki = mc_conf()->c.pid[PID_ID_ID].ki;
	foc->daxis.kd = mc_conf()->c.pid[PID_ID_ID].kd;
	foc->daxis.ts = foc->ts;
	foc->qaxis.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
	foc->qaxis.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
	foc->qaxis.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
	foc->qaxis.ts = foc->ts;
	line_ramp_init(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_COUNT);
	line_ramp_set_minstep(&foc->in.target_id, CONFIG_CURRENT_LOOP_RAMP_STEP_MIN);
	line_ramp_init(&foc->in.target_iq, CONFIG_CURRENT_LOOP_RAMP_COUNT);
	line_ramp_set_minstep(&foc->in.target_iq, CONFIG_CURRENT_LOOP_RAMP_STEP_MIN);
	foc->mot_velocity_filterd = 0;
	foc->mot_vel_radusPers = 0;
	memset(&foc->in, 0, sizeof(foc_in_t));
	memset(&foc->out, 0, sizeof(foc_out_t));
}

void foc_abc_2_dq(float a, float b, float c, float *d, float *q) {
	float alpha = a;
	float beta  = ONE_BY_SQRT3 * (b - c);
	float cos,sin;
	arm_sin_cos(0, &sin, &cos);
	*d = alpha * cos + beta * sin;
	*q = -alpha * sin + beta * cos;
}


static __INLINE float id_feedforward(foc_t *foc) {
#ifdef CONFIG_CURRENT_LOOP_DECOUPE
	return -(mc_conf()->m.lq * foc->out.curr_dq.q * foc->mot_vel_radusPers);
#else
	return 0;
#endif
}

static __INLINE float iq_feedforward(foc_t *foc) {
#ifdef CONFIG_CURRENT_LOOP_DECOUPE
	return (mc_conf()->m.ld * foc->out.curr_dq.d + mc_conf()->m.flux) * foc->mot_vel_radusPers;
#else
	return 0;
#endif
}

void foc_update(foc_t *foc) {
	float max_Vdc = foc->in.dc_vol * CONFIG_SVM_MODULATION;
	
	LowPass_Filter(foc->mot_velocity_filterd, foc->in.mot_velocity, 0.01f);
	foc->mot_vel_radusPers = foc->mot_velocity_filterd / 30.0f * PI * mc_conf()->m.poles;
	arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
	park(foc, &foc->in.curr_ab, &foc->out.curr_dq);
	if (!foc->in.b_openloop) {
		/* limiter Vd output for PI controller */
		float max_vd = max_Vdc * SQRT3_BY_2;
		foc->daxis.max = max_vd;
		foc->daxis.min = -max_vd;
		float err = line_ramp_step(&foc->in.target_id) - foc->out.curr_dq.d;
		float id_ff = id_feedforward(foc);
		foc->in.target_vol_dq.d = PI_Controller_Current(&foc->daxis, err, id_ff);

		/* limiter Vq output for PI controller */
		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
		foc->qaxis.max = max_vq;
		foc->qaxis.min = -max_vq;
		err = line_ramp_step(&foc->in.target_iq) - foc->out.curr_dq.q;
		float iq_ff = iq_feedforward(foc);
		foc->in.target_vol_dq.q = PI_Controller_Current(&foc->qaxis, err, iq_ff);
	}else {
		float max_vd = max_Vdc * SQRT3_BY_2;
		foc->in.target_vol_dq.d = fclamp(foc->in.target_vol_dq.d, -max_vd, max_vd);
		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
		foc->in.target_vol_dq.q = fclamp(foc->in.target_vol_dq.q, -max_vq, max_vq);
	}

	foc->out.vol_dq.d = foc->in.target_vol_dq.d;
	foc->out.vol_dq.q = foc->in.target_vol_dq.q;
#ifdef CONFIG_Volvec_Delay_Comp
	if (foc->mot_velocity_filterd >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
		float vol_dq_angle = foc->in.mot_angle + foc->mot_velocity_filterd / PI * 180.0f * (foc->ts * 0.8f);
		rand_angle(vol_dq_angle);
		arm_sin_cos(vol_dq_angle, &foc->sin, &foc->cos);
	}
#endif
	rev_park(foc, &foc->out.vol_dq, &foc->out.vol_albeta);
	
	SVM_Duty_Fix(&foc->out.vol_albeta, foc->in.dc_vol, foc->half_period, &foc->out);

	phase_current_point(&foc->out);
	
	pwm_update_duty(foc->out.duty[0], foc->out.duty[1], foc->out.duty[2]);
	pwm_update_sample(foc->out.sample1, foc->out.sample2, foc->out.sample_phase);
}