#include "foc/motor/motor.h"
#include "foc/core/controller.h"
#include "math/fast_math.h"
#include "foc/motor/mot_params_ind.h"
#include "libs/logger.h"
#include "foc/samples.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 void _flux_ind_timer_handler(shark_timer_t *);
static shark_timer_t _flux_ind_timer = TIMER_INIT(_flux_ind_timer, _flux_ind_timer_handler);

static float rs_id_max, rs_vd_max, rs_vd_now, rs_est_value = 0.011f;
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, b_flux_ind = false, b_flux_ested = false;
static u8   rs_ind_step = 0;
static u16  old_max_fw_id = 0xFFFF;
void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
	if (b_rs_ind || b_ldq_ind || b_flux_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;
	mot_contrl_set_angle(&motor.controller, 0);
	mot_contrl_set_vdq_immediate(&motor.controller, rs_vd_now, 0);

	rs_ind_step = 1;
	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);
	shark_timer_cancel(&_flux_ind_timer);
	u32 mask = cpu_enter_critical();
	if (old_max_fw_id != 0xFFFF) {
		mc_conf()->m.max_fw_id = old_max_fw_id;
		old_max_fw_id = 0xFFFF;
	}
	b_rs_ind = false;
	b_ldq_ind = false;
	b_flux_ind = false;
	mot_ctrl_set_ind_freq(mot_contrl(), 0);
	cpu_exit_critical(mask);
	mot_contrl_set_vdq(&motor.controller, 0, 0);
	mot_contrl_set_current(&motor.controller, 0);
}

static void _rs_ind_timer_handler(shark_timer_t *t) {
	bool finish = false;
	static int wait_iq_0_cnt = 0;
	if (!b_rs_ind) {
		mot_contrl_set_vdq(&motor.controller, 0, 0);
		return;
	}

	switch (rs_ind_step) {
	case 1:
		if (motor.controller.foc.out.curr_dq.d < rs_id_max) {
			rs_vd_now += 0.1f;
			wait_iq_0_cnt = 0;
			if (rs_vd_now >= rs_vd_max) {
				mot_contrl_set_vdq(&motor.controller, 0, 0);
				b_rs_ind = false;
				sys_debug("id not reach max id %f\n", motor.controller.foc.out.curr_dq.d);
				return;
			}
			mot_contrl_set_vdq_immediate(&motor.controller, rs_vd_now, 0);
		}else {
			rs_ind_step = 2;
			sys_debug("id reach the set\n");
		}
		break;
	case 2:
		if (ABS(motor.controller.foc.out.curr_dq.q) > 5.0f) {
			wait_iq_0_cnt++;
			if (wait_iq_0_cnt >= 200) {
				mot_contrl_set_vdq(&motor.controller, 0, 0);
				b_rs_ind = false;
				sys_debug("iq is larger %f\n", motor.controller.out_idq_filterd.q);
				return;
			}
		}else {
			wait_iq_0_cnt = 0;
			delay_ms(100);
			rs_ind_step = 3;
			sys_debug("start rs calc, %d\n", rs_meas_time);
		}
		break;
	case 3: {
		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_period) * motor.controller.foc.in.dc_vol * TWO_BY_THREE;
		float vd = rs_vd_now * TWO_BY_THREE - dtc;
		float id = motor.controller.foc.out.curr_dq.d;
		float rs = vd / (id + 0.0001f);
		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
		if (rs_meas_time-- <= 0) {
			mot_params_ind_stop();
			delay_ms(1000);
			mc_ind_motor_start(false);
			finish = true;
			b_rs_ested = true;
			sys_debug("est rs = %f-%f\n", rs_est_value, rs);
			sys_debug("vd-id is %f-%f-%f-%f, wait %d\n", rs_vd_now, id, dtc, vd, wait_iq_0_cnt);
		}
	}
	default:
		break;
	}
		
	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 || b_flux_ind) {
		return;
	}
	if (!b_rs_ested) { //必须先识别相电阻
		return;
	}
	hj_v = v;
	hj_freq = freq;
	hj_n = (float)FOC_PWM_FS / hj_freq;
	hj_samples = hj_n * 50;
	K_terms = (s32) (0.5f + hj_samples*hj_freq/(float)FOC_PWM_FS);
	Vdead = motor.controller.foc.in.dc_vol * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
	hj_w = 360.0f / hj_n;
	sys_debug("hj %f, %f, %f, %f, %f, %f, %f\n", hj_v, hj_freq, hj_n, hj_samples, K_terms, Vdead, hj_w);
	float fft_angle = 360.0f / hj_samples * K_terms;
	arm_sin_cos(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) {
		mot_ctrl_set_ind_freq(mot_contrl(), freq);
		b_ldq_ind = true;
		shark_timer_post(&_ldq_ind_timer, 10);
	}else {
		sys_debug("alloc error\n");
	}
}

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, 10);
		}
	}
}


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;
	norm_angle_deg(hj_angle);
	float s, c;
	arm_sin_cos(hj_angle, &s, &c);
	float vd = 0, vq = 0;
	if (n_ind_ld == L_TYPE_D) {
		vd = hj_v * c;
	}else {
		vq = hj_v * c;
	}
	mot_contrl_set_vdq_immediate(&motor.controller, 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)) {
		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 = NORM2_f(*real, *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);
	sys_debug("v %f, %f, %f\n", v_mag/(hj_samples*0.5f), v_real, v_image);
	sys_debug("i %f, %f, %f\n", i_mag/(hj_samples*0.5f), i_real, i_image);
	sys_debug("vmag %f, %f\n", v_mag, Vdead * hj_samples*0.5f);
#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
	v_mag -= Vdead * hj_samples*0.5f;
#endif
	float z_angle = fast_atan2(i_image, i_real) - fast_atan2(v_image, v_real);
	float s,c;
	arm_sin_cos(pi_2_degree(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 Rs = rs_est_value;
	float Ri = (SQ(z_real - Rs) + SQ(z_image))/(z_real - Rs + 0.0000001f);
	float l = Ri * (z_real - Rs)/(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;
}
static float motVelRadusPers, flux_wait_cnt = 0, flux_do_cnt = 0, flux_est_value = 0;
static bool _pending_flux_mc_stop = false;
static void _flux_ind_timer_handler(shark_timer_t *t) {
	float We = motor.controller.foc.mot_vel_radusPers;
	float delta = We - motVelRadusPers;
	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;
	if (We > 100 && ABS(delta) < 40) {
#if CONFIG_MOT_IND_USE_PHASE_SAMPLE==0
		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_period) * motor.controller.foc.in.dc_vol * 1.5f;
		float vq = (motor.controller.foc.out.vol_dq.q - dtc) * TWO_BY_THREE;
#else
		float vq = motor.controller.phase_v_dq.q;
#endif
		float flux = vq / We;
		flux_est_value = LowPass_Filter(flux_est_value, flux, 0.1f);
		flux_do_cnt ++;
	}else {
		flux_wait_cnt ++;
	}
	if ((flux_wait_cnt >= 500) || (flux_do_cnt >= 400)) {
		b_flux_ind = false;
		if (flux_wait_cnt >= 500) {
			sys_debug("ind flux error\n");
		}else {
			b_flux_ested = true;
			sys_debug("ind_flux finish, %f\n", flux_est_value);
		}
		mot_params_ind_stop();
		if (mot_contrl_get_speed(&motor.controller) < CONFIG_ZERO_SPEED_RPM) {
			mc_ind_motor_start(false);
		}else {
			_pending_flux_mc_stop = true;
		}
	}else {
		shark_timer_post(&_flux_ind_timer, 100);
	}
}

void mot_params_ind_flux(float id, float iq) {
	if (b_rs_ind || b_ldq_ind || b_flux_ind || _pending_flux_mc_stop) {
		return;
	}
	b_flux_ind = true;
	flux_wait_cnt = 0;
	flux_do_cnt = 0;
	b_flux_ested = false;
	old_max_fw_id = mc_conf()->m.max_fw_id;
	mc_conf()->m.max_fw_id = 0;
	mc_set_ctrl_mode(CTRL_MODE_CURRENT);
	mot_contrl_set_current(&motor.controller ,iq);
	shark_timer_post(&_flux_ind_timer, 10);
	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;
}

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;
}

bool mot_params_flux_ested(void) {
	return b_flux_ested;
}
float mot_params_get_est_flux(void) {
	return flux_est_value;
}

void mot_params_flux_stop(void) {
	if (_pending_flux_mc_stop && (mot_contrl_get_speed(&motor.controller) < CONFIG_ZERO_SPEED_RPM)) {
		mc_ind_motor_start(false);
		_pending_flux_mc_stop = false;
	}
}

bool mot_params_flux_pending(void) {
	return _pending_flux_mc_stop;
}