#include "bsp/bsp.h"
#include "bsp/bsp_driver.h"
#include "foc/motor/encoder.h"
#include "foc/motor/motor_param.h"
#include "libs/logger.h"
#include "foc/mc_config.h"
#include "math/fast_math.h"

/* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/

static encoder_t g_encoder;

static __INLINE s16 _abi_count(void) {
#ifdef ENCODER_CC_INVERT
	return ((g_encoder.cpr -1) - (s16)ENC_COUNT);
#else
	return (s16)ENC_COUNT;
#endif
}

static __INLINE void encoder_pll_update_gain(void) {
	if (g_encoder.pll_bandwidth_shadow != g_encoder.pll_bandwidth) {
		g_encoder.pll_bandwidth = g_encoder.pll_bandwidth_shadow;
		g_encoder.est_pll.kp = 2.0f * g_encoder.pll_bandwidth;
		g_encoder.est_pll.ki = 0.25f * g_encoder.est_pll.kp * g_encoder.est_pll.kp;		
	}
}

static void _init_pll(void) {
	g_encoder.est_pll.ts = FOC_CTRL_US;
	g_encoder.est_pll.max_wp = g_encoder.cpr;
	g_encoder.pll_bandwidth = 0;
	g_encoder.pll_bandwidth_shadow = mc_conf()->m.nor_pll_band;
	encoder_pll_update_gain();
	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
}


void encoder_init(void) {
	encoder_init_clear();
	enc_intf_init(ENC_MAX_RES);
}

void encoder_set_direction(s8 direction) {
	g_encoder.direction = direction;
	//g_encoder.cali_angle = INVALID_ANGLE;
}

void encoder_set_bandwidth(float bandwidth) {
	g_encoder.pll_bandwidth_shadow = bandwidth;
}

void encoder_init_clear(void) {
	g_encoder.cpr = ENC_MAX_RES;
	g_encoder.enc_offset = mc_conf()->m.encoder_offset;
	g_encoder.motor_poles = mc_conf()->m.poles;
	g_encoder.b_index_found = false;
	g_encoder.direction = POSITIVE;
	g_encoder.abi_angle = 0.0f;
	g_encoder.pwm_angle = 0.0f;
	g_encoder.est_angle_counts = 0;
	g_encoder.est_vel_counts = 0;
	g_encoder.est_vel_cnt_filter = 0;
	g_encoder.position = 0.0f;
	g_encoder.interpolation = 0.0f;
	//g_encoder.cali_angle = INVALID_ANGLE;
	g_encoder.enc_count_off = 0xFFFFFFFF;
	g_encoder.b_cali_err = false;
	g_encoder.produce_error = false;
	g_encoder.last_delta_cnt = MAX_S16;
	g_encoder.enc_maybe_err = ENCODER_NO_ERR;
	g_encoder.start_dianostic_cnt = 0;
	g_encoder.align_cnt = 0;
	g_encoder.align_step = 0;
	g_encoder.align_cnt_final = 0;
	g_encoder.pwm_time_ms = get_tick_ms();
	_init_pll();
}

void encoder_lock_position(bool enable) {
	if (g_encoder.b_lock_pos != enable) {
		g_encoder.b_lock_pos = enable;
		if (enable) {
			encoder_set_bandwidth(mc_conf()->m.pos_pll_band);
		}else {
			encoder_set_bandwidth(mc_conf()->m.nor_pll_band);
		}
	}
}

void encoder_epm_pll_band(bool epm) {
	if (epm) {
		encoder_set_bandwidth(mc_conf()->m.epm_pll_band);
	}else {
		encoder_set_bandwidth(mc_conf()->m.nor_pll_band);
	}
}

static __INLINE float _pll_over_comp(void) {
	u8 dir = ENC_DIR_DOWN;
#ifdef ENCODER_CC_INVERT
	dir = ENC_DIR_UP;
#endif
	if(ENC_Direction() == dir){
		return -((float)g_encoder.cpr);
	}
	return (float)g_encoder.cpr;
}

static __INLINE bool encoder_run_pll(float cnt) {
	float pll_comp = 0.0f;
	if (g_encoder.b_timer_ov) {
		pll_comp = _pll_over_comp();
		g_encoder.b_timer_ov = false;
	}
	encoder_pll_update_gain();
	g_encoder.est_vel_counts = PLL_run(&g_encoder.est_pll, cnt, pll_comp);
	g_encoder.est_angle_counts = g_encoder.est_pll.observer;
    bool snap_to_zero_vel = false;
	g_encoder.est_vel_cnt_filter = LowPass_Filter(g_encoder.est_vel_cnt_filter, g_encoder.est_vel_counts, 0.1f);
    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.ts * g_encoder.est_pll.ki) {
        g_encoder.est_vel_cnt_filter = g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
        snap_to_zero_vel = true;
    }
	return snap_to_zero_vel;
}

/* 偏心补偿 */
static __INLINE float _eccentricity_compensation(int cnt) {
#ifdef FIR_PHASE_SHIFT
	int cnt_off = (cnt + FIR_PHASE_SHIFT);//g_encoder.cpr;
	if (g_encoder.encoder_off_map != NULL) { //do offset calibrate, can not do encentricity compensation
		return 0.0f;
	}
	return -(S16Q10toF(_encoder_off_map[cnt_off]));
#else
	return 0.0f;
#endif
}

#define MAX_CPR_CNT_PER_CTL 40
static __INLINE bool encoder_ab_is_jitter(s16 cnt) {
	return ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2)));
}

#define CONFIG_ENC_DETECT_ERR
#define CONFIG_ENC_ERR_TEST
#define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm, 每隔控制周期 0.2232 机械角度
u32 enc_pwm_err_ms = 0;
s16 enc_delta_err1 = 0;
s16 enc_delta_err2 = 0;
static float enc_r = 0;
static s16 enc_cnt = 0;
static s16 enc_last_cnt = 0;
static s16 enc_test1 = 0;
static s16 enc_test2 = 0;
static s16 enc_test3 = 0;
static s16 enc_test4 = 0;

/* 9000RPM = 150 RPS = 150 * ENC_MAX_RES * FOC_CTRL_US = 39, 每个控制周期 39个count */
static void encoder_detect_error(s16 cnt) {
#ifdef CONFIG_ENC_DETECT_ERR
	static u32 _mayerr_cnt = 0;
	if (ENCODER_NO_ERR == g_encoder.enc_maybe_err) {
		s16 delta_cnt = cnt - g_encoder.last_cnt;
		bool skip = false;
		if (g_encoder.b_timer_ov) {
			bool is_jitter = encoder_ab_is_jitter(cnt); //需要处理低速在overflow附近震荡
			if (!is_jitter) {
				s16 com = _pll_over_comp();
				if (com > 0) {
					delta_cnt = delta_cnt + com;
				}else {
					delta_cnt = delta_cnt + com;
				}
				enc_test4 = delta_cnt;
			}
		}
		if ((delta_cnt > 200) || (delta_cnt < -200)) {
			enc_test1 = cnt;
			enc_test2 = g_encoder.last_cnt;
			enc_test3 = g_encoder.b_timer_ov;
		}
#ifdef CONFIG_ENC_ERR_TEST
		if (g_encoder.produce_error) {
			delta_cnt = 0;
		}
#endif
		if (g_encoder.last_delta_cnt == MAX_S16) {
			skip = true;
		}
		float ab_thr = CONFIG_ENC_DIANOSTIC_MIN_CNT*1.2f;
		if (g_encoder.last_delta_cnt < ab_thr) {
			if (get_delta_ms(g_encoder.pwm_time_ms) >= 8) {
				g_encoder.enc_maybe_err = ENCODER_PWM_ERR;
				enc_pwm_err_ms = get_delta_ms(g_encoder.pwm_time_ms);
				enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
			}
			if (!_mayerr_cnt) {
				skip = true;
				g_encoder.start_dianostic_cnt = 0;
			}
		}else if (g_encoder.start_dianostic_cnt < 0xFFFFFF) {
			g_encoder.start_dianostic_cnt ++;
		}
		if (!skip && ((g_encoder.last_delta_cnt > ab_thr) || (_mayerr_cnt != 0)) && (g_encoder.start_dianostic_cnt >= FOC_PWM_FS)) {
			float last_delta = (float)g_encoder.last_delta_cnt;
			float r = (float)delta_cnt / (last_delta + 0.0000001f);
			float r_abs = ABS(r);
			float r_thr;
			u32   cnt_thr;
			if (g_encoder.last_delta_cnt <= ab_thr * 2) {
				r_thr = 0.3f;
				cnt_thr = 4;
			}else if (g_encoder.last_delta_cnt <= ab_thr * 4) {
				r_thr = 0.5f;
				cnt_thr = 3;
			}else if (g_encoder.last_delta_cnt <= ab_thr * 6) {
				r_thr = 0.6f;
				cnt_thr = 2;
			}else {
				r_thr = 0.7f;
				cnt_thr = 1;
			}
			if (r_thr >= 0.5f) {
				if (r < 0) {
					r_thr = 2.0f;
					cnt_thr = 1;
				}else if (r_abs <= 0.01f) {
					cnt_thr = 1;
				}
			}
			if (r_abs <= r_thr || r_abs >= (2.0f - r_thr)) {
				_mayerr_cnt ++;
				if (_mayerr_cnt >= cnt_thr) {
					g_encoder.enc_maybe_err = ENCODER_AB_ERR;
					enc_delta_err1 = g_encoder.last_delta_cnt;
					enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
					enc_r = r;
					enc_cnt = delta_cnt;
					enc_last_cnt = _mayerr_cnt;
				}
			}else {
				_mayerr_cnt = 0;
			}
		}else {
			_mayerr_cnt = 0;
		}
		g_encoder.last_delta_cnt = delta_cnt;
	}
#else
	g_encoder.enc_maybe_err = ENCODER_NO_ERR;
#endif
}

float encoder_get_theta(bool detect_err) {
	if (!g_encoder.b_index_found) {
		return g_encoder.pwm_angle;
	}
	s16 cnt = _abi_count();
	__NOP();__NOP();__NOP();__NOP();
	if (ENC_OverFlow()) {
		cnt = _abi_count();
		if(encoder_ab_is_jitter(cnt)) { //需要处理低速在overflow附近震荡
			g_encoder.b_timer_ov = false;
		}else {
			g_encoder.b_timer_ov = true;
		}
		ENC_ClearUpFlags();
	}
	if (detect_err) {
		encoder_detect_error(cnt);
	}else {
		g_encoder.last_delta_cnt = MAX_S16;
		g_encoder.start_dianostic_cnt = 0;
	}
    bool snap_to_zero_vel = encoder_run_pll((float)(cnt));

	if (snap_to_zero_vel) {
		g_encoder.interpolation = 0.1f;
	}else {
		if (cnt == g_encoder.last_cnt) {
			g_encoder.interpolation += g_encoder.est_vel_cnt_filter * FOC_CTRL_US;
			g_encoder.interpolation = fclamp(g_encoder.interpolation, -ENC_MAX_interpolation, ENC_MAX_interpolation);
		}else {
			g_encoder.interpolation = 0.0f;
		}
	}
	step_towards(&g_encoder.align_step, (float)g_encoder.align_cnt_final, 0.01f);
	float angle_count = cnt + g_encoder.align_step + g_encoder.interpolation;
	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle(angle_count) * g_encoder.motor_poles + g_encoder.enc_offset;
	g_encoder.abi_angle += _eccentricity_compensation(cnt);
	norm_angle_deg(g_encoder.abi_angle);
	g_encoder.last_cnt = cnt;
	g_encoder.last_us = task_get_usecond();

	g_encoder.position += (g_encoder.est_vel_counts/g_encoder.cpr) * FOC_CTRL_US;

	return g_encoder.abi_angle;
}


void encoder_produce_error(bool error) {
	g_encoder.produce_error = error;
}

u8 encoder_may_error(void) {
	return g_encoder.enc_maybe_err;
}

float encoder_get_speed(void) {
	if (g_encoder.enc_maybe_err != ENCODER_NO_ERR) {
		return 0;
	}
	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f;
}

float encoder_get_vel_count(void) {
	return g_encoder.est_vel_counts;
}

float encoder_get_position(void) {
	return g_encoder.position;
}

float encoder_zero_phase_detect(float *enc_off) {	
	delay_ms(5);	
	float total_enc_off = g_encoder.pwm_count;
	s16 prev_offset = g_encoder.enc_offset;
	float phase = encoder_get_pwm_angle();
	float total_ph = phase;
	int count = 0;
	for(; count < 10; count++) {
		delay_ms(5); //wait time for pwm
		float angle_now = encoder_get_pwm_angle();
		if (ABS(phase - angle_now) > 2.0f) {
			g_encoder.enc_offset = prev_offset;
			g_encoder.enc_count_off = 0xFFFFFFFF;
			g_encoder.b_cali_err = true;
			sys_debug("err %f, %f, %d\n", phase, angle_now, count);
			return INVALID_ANGLE;
		}

		phase = angle_now;
		total_ph += phase;
		total_enc_off += g_encoder.pwm_count;
	}
	sys_debug("count = %d, %f, %d\n", count, total_enc_off, g_encoder.pwm_count);
	float offset_now = total_ph/(float)(count + 1);
	g_encoder.enc_offset = (s16)offset_now;
	g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count + 1));
	if (enc_off) {
		*enc_off = (float)g_encoder.enc_count_off;
		sys_debug("encoder off %f\n", *enc_off);
	}
	sys_debug("encoder ph off = %f\n", offset_now);
	return offset_now;
}

void encoder_clear_cnt_offset(void) {
	g_encoder.b_cali_err = false;
	g_encoder.enc_count_off = 0xFFFFFFFF;
}

u32 encoder_get_cnt_offset(void) {
	return g_encoder.enc_count_off;
}

bool encoder_get_cali_error(void) {
	return g_encoder.b_cali_err;
}

static void encoder_sync_pwm_abs(void) {
	u32 mask = cpu_enter_critical();
	ENC_COUNT = g_encoder.pwm_count;
	g_encoder.pwm_start_cnt = g_encoder.pwm_count;
	g_encoder.last_cnt = g_encoder.pwm_count;
	g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
	g_encoder.abi_angle = g_encoder.pwm_angle;
	g_encoder.b_index_found = true;
	g_encoder.last_delta_cnt = MAX_S16;
	g_encoder.align_cnt = g_encoder.align_step = 0;
	g_encoder.align_cnt_final = 0;
	g_encoder.z_index_counter = 0;
	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
	cpu_exit_critical(mask);
}

/*I 信号的中断处理，一圈一个中断*/
#define CONFIG_Z_IDX_MARGIN_B 4
#define CONFIG_Z_IDX_MARGIN_F 1
#define CONFIG_Z_IDX_FIXED_COUNT 30  /* Z信号最大允许的补偿机角度为 (30-5)/4096*360=2.2度*/
#define CONFIG_Z_IDX_MAX_CNT_PER_IRQ 4 /* 每次z信号中断，最大补偿机械角度(4)/4096*360=0.34，类似做低通滤波 */
#define CONFIG_Z_IDX_IGNORE_MAX_CNT 10
void ENC_ABI_IRQHandler(u32 count) {
	s16 z_margin;
#ifdef ENCODER_CC_INVERT
	g_encoder.z_index_cnt = (g_encoder.cpr -1) - count;
	if (g_encoder.est_vel_cnt_filter > 0){
		z_margin = CONFIG_Z_IDX_MARGIN_B;
	}else {
		z_margin = CONFIG_Z_IDX_MARGIN_F;
	}
#else
	g_encoder.z_index_cnt = count;
	if (g_encoder.est_vel_cnt_filter > 0){
		z_margin = CONFIG_Z_IDX_MARGIN_F;
	}else {
		z_margin = CONFIG_Z_IDX_MARGIN_B;
	}
#endif
  	g_encoder.z_index_counter++;
	s16 pre_cnt = g_encoder.align_cnt;
	if (g_encoder.z_index_cnt <= z_margin) {
		g_encoder.align_cnt = 0;
	}else if (g_encoder.z_index_cnt >= (g_encoder.cpr - z_margin)) {
		g_encoder.align_cnt = 0;
	}else if (g_encoder.z_index_cnt > z_margin && g_encoder.z_index_cnt < CONFIG_Z_IDX_FIXED_COUNT) {
		g_encoder.align_cnt = -(g_encoder.z_index_cnt - z_margin);
	}else if (g_encoder.z_index_cnt > (g_encoder.cpr - CONFIG_Z_IDX_FIXED_COUNT) && g_encoder.z_index_cnt < (g_encoder.cpr - z_margin)) {
		g_encoder.align_cnt =  g_encoder.cpr - g_encoder.z_index_cnt - z_margin;
	}else {
		//maybe error?
		g_encoder.z_index_err_counter++;
	}
	s16 delta = pre_cnt - g_encoder.align_cnt;
	if (ABS(delta) >= CONFIG_Z_IDX_IGNORE_MAX_CNT) {
		g_encoder.align_cnt = pre_cnt;
	}
	/* 编码器没有做零位置校准不能补偿 */
	if (g_encoder.enc_offset == 0) {
		step_towards_s16(&g_encoder.align_cnt_final, g_encoder.align_cnt, CONFIG_Z_IDX_MAX_CNT_PER_IRQ);
	}
}

/* 编码器AB信号读书溢出处理 */
void ENC_TIMER_Overflow(void) {
	//g_encoder.b_timer_ov = true;
}


/*PWM 信号捕获一个周期的处理 */
static int pwm_count = 0;
static int pwm_check_count = 0;
static int pwm_duty_err = 0;
static float pwm_err_min = 0;
static float pwm_err_max = 0;
void ENC_PWM_Duty_Handler(float t, float d) {
	float duty = ENC_Duty(d, t);
	if (duty < ENC_PWM_Min_P || duty > ENC_PWM_Max_P) {
		pwm_duty_err++;
		if (duty < ENC_PWM_Min_P) {
			pwm_err_min = duty;
			duty = ENC_PWM_Min_P;
		}else {
			pwm_err_max = duty;
			duty = ENC_PWM_Max_P;
		}
	}
	float Nr = ENC_Duty_2_Pluse_Nr(duty);
	if (Nr < 0) {
		Nr = 0;
	}else if (Nr > ENC_MAX_RES) {
		Nr = ENC_MAX_RES;
	}
	u32 n_nr = (u32)Nr;
	if (Nr - n_nr >= 0.5f) {
		g_encoder.pwm_count = n_nr + 1;
	}else {
		g_encoder.pwm_count = n_nr;
	}
	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * g_encoder.motor_poles + g_encoder.enc_offset;	
	norm_angle_deg(g_encoder.pwm_angle);
	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
		encoder_sync_pwm_abs();
	}
	pwm_check_count ++;
#ifdef CONFIG_ENC_ERR_TEST
	if (!g_encoder.produce_error) {
		g_encoder.pwm_time_ms = get_tick_ms();
	}
#else
	g_encoder.pwm_time_ms = get_tick_ms();
#endif
}
static u32 _check_time = 0;
bool ENC_Check_error(void) {
	bool error = false;
	if (get_delta_ms(_check_time) > 200) {
		if (pwm_check_count == 0) {
			error = true;
		}
		pwm_check_count = 0;
		_check_time = get_tick_ms();
	}
	return error;
}

float encoder_get_pwm_angle(void) {
#ifdef ENCODER_CC_INVERT
	g_encoder.pwm_angle = 360.0f - (g_encoder.pwm_angle - g_encoder.enc_offset) + g_encoder.enc_offset;
	norm_angle_deg(g_encoder.pwm_angle);
#endif
	return g_encoder.pwm_angle;
}

float encoder_get_abi_angle(void) {
	s16 cnt = _abi_count();
	float angle = ENC_Pluse_Nr_2_angle((float)(cnt + g_encoder.align_step)) * g_encoder.motor_poles + g_encoder.enc_offset;
	norm_angle_deg(angle);
	return angle;
}

void encoder_log(void) {
	sys_debug("pwm %f, abi %f\n", encoder_get_pwm_angle(), encoder_get_abi_angle());
	sys_debug("Z idx %d,%d,%d,%d\n", g_encoder.z_index_err_counter, g_encoder.pwm_start_cnt, g_encoder.z_index_cnt, g_encoder.z_index_counter);
	sys_debug("Z err: %f, %d,%d\n", g_encoder.align_step, g_encoder.align_cnt, g_encoder.align_cnt_final);
	sys_debug("pwm freq %f, err %d, %f, %f\n", enc_get_pwm_freq(), pwm_duty_err, pwm_err_min, pwm_err_max);
	if (g_encoder.enc_maybe_err) {
		sys_debug("E:%d,%d,%d,%d,%d,%d\n", enc_test1, enc_test2, enc_test3, enc_cnt, enc_last_cnt, enc_test4);
		sys_debug("E:%d, %d, %f, %d\n", enc_delta_err1, enc_delta_err2, enc_r, g_encoder.enc_maybe_err);
	}
}