#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_off.h"

#define ANGLE_OFFSET (-50.0f)//133.0f
/* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
#define DIR_ADJUGE_MAX_CNT 10
#define PLL_BANDWIDTH 200
encoder_t g_encoder;

u8   encoder_off_count[1024] = {0};
s16  encoder_off_map[1024] = {0};
s16  encoder_off_r_map[1024] = {0};
u8   encoder_off_r_count[1024] = {0};

bool encoder_off_finished = false;
s16  encoder_off_comp = 940.0f;
float encoder_off_mul = 1.0f;
int encoder_off_cn_add = 542;
float encoder_off_angle_add = 0.0f;
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) {
	for (int i = i; i < 1024; i++) {
		encoder_off_count[i] = 0;
		encoder_off_map[i] = 0;
		encoder_off_r_map[i] = 0;
		encoder_off_r_count[i] = 0;
	}
	encoder_init_clear(POSITIVE);
	enc_intf_init(ENC_MAX_RES);
}

void encoder_set_direction(s8 direction) {
	encoder_init_clear(direction);
}

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()->encoder_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;
}

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

static __INLINE u32 _abi_count(void) {
#ifdef ENCODER_CC_INVERT
	return (g_encoder.cpr - ENC_COUNT);
#else
	return ENC_COUNT;
#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();
	}
	encoder_run_pll((float)(cnt));
    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;
    }
	if (snap_to_zero_vel) {
		g_encoder.interpolation = 0.5f;
	}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;
		}
	}
	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
	if (encoder_off_finished) {
		int cnt_off = (cnt + FIR_PHASE_SHIFT) % g_encoder.cpr;
		g_encoder.abi_angle -= ((_encoder_off_map[cnt_off])/100.0f);
	}
	rand_angle(g_encoder.abi_angle);

	
	g_encoder.last_cnt = cnt;
	g_encoder.last_us = timer_count32_get();
	return g_encoder.abi_angle;
}

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


extern int jtag_plot;
void encoder_detect_offset(float angle, bool r){
	float delta = (g_encoder.abi_angle - angle);
	if (delta > 200) {
		delta = delta - 360;
	}
	if (delta < -200) {
		delta = delta + 360;
	}
	int last_cn = (g_encoder.last_cnt + 0) % 1024;
	if (!r) {
		encoder_off_map[last_cn] = (s16)(delta*100.0f);
		encoder_off_count[last_cn] ++;
	}else {
		encoder_off_r_map[last_cn] = (s16)(delta*100.0f);
		encoder_off_r_count[last_cn] ++;
	}
	if (jtag_plot==1) {
		plot_1data16((s16)(delta*100.0f));
	}
	encoder_off_finished = false;
}


float encoder_detect_finish(bool r) {
	for (int i = 0; i < 1024; i++) {
		if (!r) {
			if (encoder_off_count[i] <= 2) {
				return false;
			}
		}else {
			if (encoder_off_r_count[i] <= 2) {
				return false;
			}
		}
	}
	encoder_off_finished = true;
	return encoder_off_finished;
}


void encoder_detect_off_finished(void) {
	for (int i = 0; i < 100; i++) {
		plot_3data16(0,0,1000);
		delay_ms(2);
	}
	for (int i = 0; i < 1024; i++) {
		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
		plot_3data16(0, 0 , angle_off);
		delay_ms(2);
	}
	for (int i = 0; i < 1024; i++) {
		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
		plot_3data16(0, 0 , angle_off);
		delay_ms(2);

	}
	for (int i = 0; i < 1024; i++) {
		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
		plot_3data16(0, 0 , angle_off);
		delay_ms(2);
	}	
}

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

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


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