#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"

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

#define USE_PWM_ONLY

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) {
	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 void encoder_run_pll(float cnt) {
	float pll_comp = 0.0f;
	if (g_encoder.b_timer_ov) {
		if(ENC_Direction() == ENC_DIR_DOWN){
			pll_comp = -((float)g_encoder.cpr);
		}else {
			pll_comp = g_encoder.cpr;
		}
		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;
}

float encoder_get_theta(void) {
	if (!g_encoder.b_index_found) {
		return g_encoder.pwm_angle;
	}
	u32 cnt = ENC_COUNT;
	__NOP();__NOP();__NOP();__NOP();
	if (ENC_OverFlow()) {
		cnt = ENC_COUNT;
		g_encoder.b_timer_ov = true;
		ENC_ClearUpFlags();
	}
	if (cnt == g_encoder.last_cnt) {
		g_encoder.interpolation += g_encoder.est_vel_counts * FOC_CTRL_US;
		if (g_encoder.interpolation > 1.0f) {
			g_encoder.interpolation = 1.0f;
		}else if (g_encoder.interpolation < -1.0f) {
			g_encoder.interpolation = -1.0f;
		}
	}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;
	rand_angle(g_encoder.abi_angle);

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

void encoder_detect_offset(float angle){
	//plot_3data16(angle, g_encoder.pwm_angle, g_encoder.abi_angle);
}

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 信号捕获一个周期的处理 */
void ENC_PWM_Duty_Handler(float t, float d) {
	float 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;
	}
	g_encoder.pwm_count = (u32)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) {
		encoder_sync_pwm_abs();
	}
}


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