#include "bsp/bsp.h"
#include "bsp/enc_intf.h"
#include "bsp/timer_count32.h"
#include "foc/motor/encoder.h"
#include "foc/motor/motor_param.h"
#include "libs/logger.h"
#include "app/nv_storage.h"
#include "math/fast_math.h"
#include "encoder_off3.h"

static void encoder_do_offset_calibrate(void) ;
static void _detect_off_finished(void);

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

encoder_t g_encoder;

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.DT = FOC_CTRL_US;
	g_encoder.est_pll.max_wp = g_encoder.cpr;
	g_encoder.pll_bandwidth = 0;
	g_encoder.pll_bandwidth_shadow = nv_get_motor_params()->est_pll_band;
	encoder_pll_update_gain();
	PLL_Reset(&g_encoder.est_pll);
}


void encoder_init(void) {
	encoder_init_clear(POSITIVE);
	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(s8 diretcion) {
	_init_pll();
	g_encoder.cpr = ENC_MAX_RES;
	g_encoder.enc_offset = nv_get_motor_params()->offset;
	g_encoder.motor_poles = nv_get_motor_params()->poles;
	g_encoder.b_index_found = false;
	g_encoder.direction = diretcion;
	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.interpolation = 0.0f;
	g_encoder.cali_angle = INVALID_ANGLE;
}

void encoder_lock_position(bool enable) {
	if (g_encoder.b_lock_pos != enable) {
		g_encoder.b_lock_pos = enable;
		if (enable) {
			encoder_set_bandwidth(nv_get_motor_params()->pos_lock_pll_band);
		}else {
			encoder_set_bandwidth(nv_get_motor_params()->est_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;
    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
        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 u32 _abi_count(void) {
#ifdef ENCODER_CC_INVERT
	return (g_encoder.cpr - ENC_COUNT);
#else
	return ENC_COUNT;
#endif
}

/* 偏心补偿 */
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
}

float encoder_get_theta(void) {
	if (!g_encoder.b_index_found) {
		return g_encoder.pwm_angle;
	}
	u32 cnt = _abi_count();
	__NOP();__NOP();__NOP();__NOP();
	if (ENC_OverFlow()) {
		cnt = _abi_count();
		g_encoder.b_timer_ov = true;
		ENC_ClearUpFlags();
	}
	
    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_counts * FOC_CTRL_US;
			if (g_encoder.interpolation > ENC_MAX_interpolation) {
				g_encoder.interpolation = ENC_MAX_interpolation;
			}else if (g_encoder.interpolation < -ENC_MAX_interpolation) {
				g_encoder.interpolation = -ENC_MAX_interpolation;
			}
		}else {
			g_encoder.interpolation = 0.0f;
		}
	}
	if (g_encoder.cali_angle != INVALID_ANGLE) {
		g_encoder.interpolation = 0.0f;
	}
	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
	g_encoder.abi_angle += _eccentricity_compensation(cnt);
	rand_angle(g_encoder.abi_angle);
	g_encoder.last_cnt = cnt;
	g_encoder.last_us = timer_count32_get();

	if (g_encoder.cali_angle != INVALID_ANGLE) {
		encoder_do_offset_calibrate();
	}
	
	return g_encoder.abi_angle;
}

float encoder_get_speed(void) {
	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f;
}

void _encoder_caliberate_init(void) {
	if (g_encoder.encoder_off_map != NULL) {
		return;
	}
	u32 mask = cpu_enter_critical();
	g_encoder.encoder_off_map = (s16 *)os_alloc(g_encoder.cpr * sizeof(s16));
	g_encoder.encoder_off_count = (u8 *)os_alloc(g_encoder.cpr);
	
	for (int i = 0; i < g_encoder.cpr; i++) {
		g_encoder.encoder_off_map[i] = 0;
		g_encoder.encoder_off_count[i] = 0;
	}
	cpu_exit_critical(mask);
}

void _encoder_caliberate_deinit(void) {
	if (g_encoder.encoder_off_map != NULL) {
		os_free(g_encoder.encoder_off_map);
		os_free(g_encoder.encoder_off_count);
	}
	g_encoder.encoder_off_map = NULL;
	g_encoder.encoder_off_count = NULL;
}

#define MIN_OFF_COUNT 10
void encoder_detect_offset(float angle){
#if 1
	_encoder_caliberate_init();
	g_encoder.cali_angle = angle;
#else
	plot_2data16((s16)angle, (s16)g_encoder.abi_angle);
#endif
}

static void encoder_do_offset_calibrate(void) {
	float delta = (g_encoder.abi_angle - g_encoder.cali_angle);
	if (delta > 200) {
		delta = delta - 360;
	}
	if (delta < -200) {
		delta = delta + 360;
	}
	if (g_encoder.direction == POSITIVE) {
		if ((g_encoder.encoder_off_count[g_encoder.last_cnt] & 0xF) <= MIN_OFF_COUNT) {
			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x01;
		}
	}else {
		if (((g_encoder.encoder_off_count[g_encoder.last_cnt] >> 4) & 0xF) <= MIN_OFF_COUNT) {
			g_encoder.encoder_off_map[g_encoder.last_cnt] += (s16)(delta*100.0f);
			g_encoder.encoder_off_count[g_encoder.last_cnt] += 0x10;
		}
	}
}

bool encoder_detect_finish(void) {
	u8 off_count = 0;
	for (int i = 0; i < 1024; i++) {
		if (g_encoder.direction == POSITIVE) {
			off_count = g_encoder.encoder_off_count[i] & 0xF;
		}else {
			off_count = (g_encoder.encoder_off_count[i] >> 4)& 0xF;
		}
		if (off_count <= MIN_OFF_COUNT) {
			return false;
		}
	}
	if (g_encoder.direction == NEGATIVE) {
		g_encoder.cali_angle = INVALID_ANGLE;
		_detect_off_finished();//output data to PC tools, and use Matlab do FIR filter
		_encoder_caliberate_deinit();
	}
	return true;
}


static void _detect_off_finished(void) {
	for (int i = 0; i < 1024; i++) {
		float angle_off = g_encoder.encoder_off_map[i] / (((g_encoder.encoder_off_count[i] >> 4)&0xF) + (g_encoder.encoder_off_count[i]&0xF));
		plot_1data16((s16)angle_off);
		delay_ms(2);
		wdog_reload();
	}	
}

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

float encoder_zero_phase_detect(void) {
	float phase = g_encoder.pwm_angle;
	float total_ph = phase;
	int count = 0;
	for(; count < 10; count++) {
		delay_ms(ENC_PWM_Min_P * 1000 + 2); //wait time for pwm
		if ABS(phase - g_encoder.pwm_angle > 2.0f) {
			return INVALID_ANGLE;
		}
		phase = g_encoder.pwm_angle;
		total_ph += phase;
	}
	sys_debug("offset = %f\n", (total_ph/(float)count));
	return (total_ph/(float)count);
}


static void encoder_sync_pwm_abs(void) {
	ENC_COUNT = 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;
}

/*I 信号的中断处理，一圈一个中断*/
void ENC_ABI_IRQHandler(void) {
	g_encoder.b_index_cnt = ENC_COUNT;	
	if (!g_encoder.b_index_found){
		encoder_sync_pwm_abs();
	}
}

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


/*PWM 信号捕获一个周期的处理 */
static int pwm_count = 0;
static int pwm_check_count = 0;
void ENC_PWM_Duty_Handler(float t, float d) {
	float duty = ENC_Duty(d, t);
	if (duty < ENC_PWM_Min_P || duty > 1.0f) {
		return;
	}
	float Nr = ENC_Duty_2_Pluse_Nr(duty);
	if (Nr < 0) {
		return;
	}
	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;	
	rand_angle(g_encoder.pwm_angle);
	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
		encoder_sync_pwm_abs();
	}
	pwm_check_count ++;
}
static u32 _check_time = 0;
bool ENC_Check_error(void) {
	bool error = false;
	if (get_delta_ms(_check_time) > 1000) {
		if (pwm_check_count == 0) {
			error = true;
		}
		pwm_check_count = 0;
		_check_time = get_tick_ms();
	}
	return error;
}

float encoder_get_pwm_angle(void) {
	return g_encoder.pwm_angle;
}