#include "foc/motor/motor.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "math/fast_math.h"
#include "foc/motor/mot_params_ind.h"
#include "libs/logger.h"
#include "prot/can_foc_msg.h"

/*
参考 MC_Simulink\modules\off_line_params_ind 仿真模型
*/

static void _rs_ind_timer_handler(shark_timer_t *);
static shark_timer_t _rs_ind_timer = TIMER_INIT(_rs_ind_timer, _rs_ind_timer_handler);
static void _ldq_ind_timer_handler(shark_timer_t *);
static shark_timer_t _ldq_ind_timer = TIMER_INIT(_ldq_ind_timer, _ldq_ind_timer_handler);

static float rs_id_max, rs_vd_max, rs_vd_now, rs_est_value;
static s32 rs_meas_time;
static bool b_rs_ind = false, b_rs_ested = false, b_ldq_ind = false, b_ld_ested = false, b_lq_ested = false;;
void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
	if (b_rs_ind || b_ldq_ind) {
		return;
	}
	b_rs_ind = true;
	b_rs_ested = false;
	rs_id_max = id_max;
	rs_vd_max = vd_max;
	rs_vd_now = 2.0f;
	rs_meas_time = time;
	PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);

	shark_timer_post(&_rs_ind_timer, 10);
}

void mot_params_ind_stop(void) {
	shark_timer_cancel(&_rs_ind_timer);
	shark_timer_cancel(&_ldq_ind_timer);
	u32 mask = cpu_enter_critical();
	b_rs_ind = false;
	b_ldq_ind = false;
	PMSM_FOC_SetOpenVdq(0, 0);
	cpu_exit_critical(mask);
}

static void _rs_ind_timer_handler(shark_timer_t *t) {
	bool finish = false;
	static int wait_iq_0_cnt = 0;
	if (!b_rs_ind) {
		PMSM_FOC_SetOpenVdq(0, 0);
		return;
	}
	if (PMSM_FOC_Get()->out.s_RealIdq.d < rs_id_max) {
		rs_vd_now += 0.1f;
		wait_iq_0_cnt = 0;
		if (rs_vd_now >= rs_vd_max) {
			PMSM_FOC_SetOpenVdq(0, 0);
			b_rs_ind = false;
			sys_debug("id not reach max id %f\n", PMSM_FOC_Get()->out.s_RealIdq.d);
			return;
		}
		PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
	}else {
		if (ABS(PMSM_FOC_Get()->out.s_FilterIdq.q) > 1.0f) {
			wait_iq_0_cnt++;
			if (wait_iq_0_cnt >= 200) {
				PMSM_FOC_SetOpenVdq(0, 0);
				b_rs_ind = false;
				sys_debug("iq is bigger chan 1, %f\n", PMSM_FOC_Get()->out.s_FilterIdq.q);
				return;
			}
		}else {
			float *iabc = PMSM_FOC_Get()->in.s_iABC;
			float d, q;
			PMSM_FOC_ABC2Dq(SIGN(iabc[0]), SIGN(iabc[1]), SIGN(iabc[2]), &d, &q);
			float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * PMSM_FOC_Get()->in.s_vDC * 1.5f;
			float vd = (rs_vd_now - dtc) * TWO_BY_THREE;
			float rs = vd / (PMSM_FOC_Get()->out.s_RealIdq.d + 0.00000000001f);
			rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
			if (rs_meas_time-- <= 0) {
				mc_ind_motor_start(false);
				mot_params_ind_stop();
				finish = true;
				b_rs_ested = true;
				sys_debug("est rs = %f\n", rs_est_value);
				sys_debug("vd is %f\n", rs_vd_now);
			}
		}
	}
	if (!finish) {
		shark_timer_post(&_rs_ind_timer, 10);
	}
}

float mot_params_get_est_rs(void) {
	return rs_est_value;
}

bool mot_params_rs_ested(void) {
	return b_rs_ested;
}

static float *v_samples = NULL, *i_samples = NULL;
static float hj_v, hj_freq, hj_n, hj_w, hj_samples, K_terms, Vdead;
static float hj_real, hj_image;
static u16   n_ind_ld, n_samples;
static float ld_est_value, lq_est_value;
static s32   ldq_est_wait_cnt = 0;
void mot_params_ind_inductance(float v, float freq, u16 l_type) {
	if (b_ldq_ind || b_rs_ind) {
		return;
	}
	
	hj_v = v;
	hj_freq = freq;
	hj_n = (float)FOC_PWM_FS / hj_freq;
	hj_samples = hj_n * 20;
	K_terms = (s32) (0.5f + hj_samples*hj_freq/(float)FOC_PWM_FS);
	Vdead = PMSM_FOC_Get()->in.s_vDC * 0.5f * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
	hj_w = 360.0f / hj_n;

	float fft_angle = 360.0f / hj_samples * K_terms;
	SinCos_Lut(fft_angle, &hj_image, &hj_real);
	hj_real = hj_real * 2.0f;
	n_ind_ld = l_type;
	n_samples = 0;
	ldq_est_wait_cnt = 0;
	if (v_samples) {
		os_free(v_samples);
	}
	if (i_samples) {
		os_free(i_samples);
	}
	v_samples = os_alloc(sizeof(float) * hj_samples);
	i_samples = os_alloc(sizeof(float) * hj_samples);
	if (v_samples != NULL && i_samples != NULL) {
		b_ldq_ind = true;
		shark_timer_post(&_ldq_ind_timer, 50);
	}
}

static void _ldq_ind_timer_handler(shark_timer_t *t) {
	if (n_samples >= (hj_samples + 1)) {
		mc_ind_motor_start(false);
		mot_params_calc_inductance();
		mot_params_ind_stop();
	}else {
		ldq_est_wait_cnt ++;
		if (ldq_est_wait_cnt >= 20) {
			mc_ind_motor_start(false);
			mot_params_ind_stop();
			sys_debug("ldq ind timeout %d\n", ldq_est_wait_cnt);
		}else {
			shark_timer_post(&_ldq_ind_timer, 50);
		}
	}
}


void mot_params_ind_ld(float v, float freq) {
	b_ld_ested = false;
	mot_params_ind_inductance(v, freq, L_TYPE_D);
}

void mot_params_ind_lq(float v, float freq) {
	b_lq_ested = false;
	mot_params_ind_inductance(v, freq, L_TYPE_Q);
}

void mot_params_high_freq_inject(void) {
	if (!b_ldq_ind) {
		return;
	}
	float hj_angle = hj_w * (float)n_samples;
	rand_angle(hj_angle);
	float s, c;
	SinCos_Lut(hj_angle, &s, &c);
	float vd = 0, vq = 0;
	if (n_ind_ld == 1) {
		vd = hj_v * c;
	}else {
		vq = hj_v * c;
	}
	PMSM_FOC_SetOpenVdq_Immediate(vd, vq);
}

bool mot_params_hj_sample_vi(float vd, float vq, float id, float iq) {
	if (!b_ldq_ind) {
		return true;
	}
	if (n_samples >= 1 && (n_samples) <= (hj_samples + 1)) {
		if (n_ind_ld == L_TYPE_D) {
			v_samples[n_samples - 1] = vd;
			i_samples[n_samples - 1] = id;
		}else {
			v_samples[n_samples - 1] = vq;
			i_samples[n_samples - 1] = iq;
		}
	}
	n_samples ++;
	return false;
}

void goertzel_dft(float *x, float *real, float *image, float *mag) {
	float y, d1 = 0, d2 = 0;
	for (int i = 0; i < hj_samples; i++) {
		y = x[i] + hj_real * d1 - d2;
		d2 = d1;
		d1 = y;
	}
	*real = d1 - (d2 * 0.5f * hj_real);
	*image = -d2 * hj_image;
	*mag = sqrtf(SQ(*real) + SQ(*image));
}

void mot_params_calc_inductance(void) {
	float v_real, v_image, v_mag;
	float i_real, i_image, i_mag;
	if (!b_ldq_ind) {
		return;
	}
	goertzel_dft(v_samples, &v_real, &v_image, &v_mag);
	goertzel_dft(i_samples, &i_real, &i_image, &i_mag);

	v_mag -= Vdead * hj_samples*0.5f;
	
	float z_angle = fast_atan_2(i_real, i_image) - fast_atan_2(v_real, v_image);
	float s,c;
	SinCos_Lut(z_angle, &s, &c);

	float z_mag = v_mag / (i_mag + 0.0000001f);
	float z_real = z_mag * c;
	float z_image = z_mag * s;
	float Ri = SQ(z_real - b_rs_ested) + SQ(z_image)/(z_real - b_rs_ested + 0.0000001f);
	float l = Ri * (z_real - rs_est_value)/(hj_freq * 2 * PI * z_image + 0.0000001f);
	if (n_ind_ld == L_TYPE_D) {
		ld_est_value = l;
		b_ld_ested = true;
		sys_debug("ld = %f\n", ld_est_value);
	}else {
		lq_est_value = l;
		b_lq_ested = true;
		sys_debug("lq = %f\n", lq_est_value);
	}
	b_ldq_ind = false;
}

float mot_params_get_est_ld(void) {
	return ld_est_value;
}

float mot_params_get_est_lq(void) {
	return lq_est_value;
}

bool mot_params_ld_ested(void) {
	return b_ld_ested;
}
bool mot_params_lq_ested(void) {
	return b_lq_ested;
}