#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/foc_config.h"
#include "foc/mc_error.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/timer_count32.h"
#include "libs/time_measure.h"
#include "bsp/delay.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"
#include "foc/commands.h"
#include "libs/logger.h"
#include "bsp/sched_timer.h"
#include "foc/core/e_ctrl.h"
#include "foc/motor/motor_param.h"
#include "foc/core/torque.h"
#include "app/nv_storage.h"

static motor_t motor = {
	.s_direction = POSITIVE,
};

static void mc_gpio_init(void) {
#ifdef GPIO_BRAKE_IN_GROUP
	rcu_periph_clock_enable(GPIO_BRAKE_IN_RCU);
	gpio_init(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_MODE, GPIO_OSPEED_50MHZ, GPIO_BRAKE_IN_PIN);
	
	gpio_exti_source_select(GPIO_BRAKE_EXIT_SRC_GROUP, GPIO_BRAKE_EXIT_SRC_PIN);
	exti_init(GPIO_BRAKE_EXTI, EXTI_INTERRUPT, EXTI_TRIG_FALLING);
	nvic_irq_enable(GPIO_BRAKE_IRQ, EBREAK_IRQ_PRIORITY, 0U);
	exti_interrupt_flag_clear(GPIO_BRAKE_EXTI);
	exti_interrupt_enable(GPIO_BRAKE_EXTI);	
#endif
}

void mc_init(void) {
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	PMSM_FOC_CoreInit();
	mc_gpio_init();
	sched_timer_enable(SPD_CTRL_MS);
}

motor_t * mc_params(void) {
	return &motor;
}

bool mc_start(u8 mode) {
	if (motor.b_start) {
		return true;
	}
	if (mode > CTRL_MODE_CURRENT) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (PMSM_FOC_GetSpeed() > 10.0f) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		err_add_record(FOC_NowAllowed_With_Speed, (s16)PMSM_FOC_GetSpeed());
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		err_add_record(FOC_Throttle_Err, get_throttle_float() * 10);
		return false;
	}
	eCtrl_init(2000, 100);
	motor_encoder_start(motor.s_direction);
	PMSM_FOC_Start(mode);
	pwm_turn_on_low_side();
	delay_ms(500);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	motor.throttle = 0;
	motor.b_start = true;
	gpio_led2_enable(true);
	return true;
}

bool mc_stop(void) {
	if (!motor.b_start) {
		return true;
	}
	if (PMSM_FOC_GetSpeed() > 10.0f) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		err_add_record(FOC_NowAllowed_With_Speed, (s16)PMSM_FOC_GetSpeed());
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		err_add_record(FOC_Throttle_Err, get_throttle_float() * 10);
		return false;
	}	
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor.b_start = false;
	gpio_led2_enable(false);
	return true;
}

void mc_set_spd_torque(s32 target) {
	motor.b_ignor_throttle = true;
	motor.s_targetFix = target;
}

void mc_use_throttle(void) {
	motor.b_ignor_throttle = false;
}


void mc_encoder_off_calibrate(s16 vd) {
	if (PMSM_FOC_Is_Start()) {
		return;
	}
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_Angle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(2000);
	motor_encoder_set_direction(POSITIVE);
	for (int i = 0; i < 1000000; i++) {
		for (float angle = 0; angle < 360; angle++) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(2);
			motor_encoder_offset(angle);
		}
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	delay_ms(500);
	motor_encoder_set_direction(NEGATIVE);
	for (int i = 0; i < 1000000; i++) {
		for (float angle = 359; angle >= 0; angle--) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(2);
			motor_encoder_offset(angle);
		}
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	
}

bool mc_encoder_zero_calibrate(s16 vd) {
	if (PMSM_FOC_Is_Start()) {
		return false;
	}
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_Angle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(2000);
	float phase = motor_encoder_zero_phase_detect();
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	if (phase != INVALID_ANGLE) {
		nv_save_angle_offset(phase);
		return true;
	}
	return false;
}


bool mc_current_sensor_calibrate(float current) {
	if (!mc_start(CTRL_MODE_OPEN)) {
		return false;
	}
	phase_current_sensor_start_calibrate(current);
	phase_current_calibrate_wait();
	return true;
}

bool mc_lock_motor(bool lock) {
	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	PMSM_FOC_LockMotor(lock); //if mot enabled, foc core will do lock
	if (!motor.b_start) {
		if (lock) {
			pwm_start();
			pwm_update_duty(0, 0, 0);
		}else {
			pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
			pwm_stop();
		}
	}
	return true;
}


bool mc_throttle_released(void) {
	return get_throttle_float() < THROTTLE_LOW_VALUE;
}


void MC_Brake_IRQHandler(void) {
#ifdef GPIO_BRAKE_IN_GROUP
	int count = 50;
	int settimes = 0;
	while(count-- >= 0) {
		bool b1 = gpio_input_bit_get(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_PIN) == SET;
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
		PMSM_FOC_Brake(false);
	}else if (settimes == 50) {
		PMSM_FOC_Brake(true);
	}else {
		//有干扰，do nothing
		motor.n_brake_errors++;
	}
#endif
}


void MC_Protect_IRQHandler(void){
	if (!motor.b_start) {
		return;
	}
	PMSM_FOC_Stop(); //三相50%占空比输出，防止mos过压击穿
	pwm_start();
	adc_start_convert();
	PMSM_FOC_SetErrCode(FOC_Phase_Curr_OV_Err);
}

measure_time_t g_meas_timeup = {.intval_max_time = 62, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
void TIMER_UP_IRQHandler(void){
	//phase_current_adc_triger();
	time_measure_start(&g_meas_timeup);
}

measure_time_t g_meas_foc = {.exec_max_time = 20, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void ADC_IRQHandler(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	if (phase_current_sensor_do_calibrate()){
		pwm_update_duty(100, FOC_PWM_Half_Period-100, 100);
		pwm_update_sample(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1, PHASE_BC);
		return;
	}
	TIME_MEATURE_START();
	PMSM_FOC_Schedule();
	TIME_MEATURE_END();
}

//#define ANGLE_TEST
#ifdef ANGLE_TEST
static void _debug_angle(void) {
	if (motor.b_start) {
		PMSM_FOC_Set_Angle(motor.s_testAngle);
		if (++motor.s_testAngle >= 360) {
			motor.s_testAngle = 0;
		}
	}
}
#endif
/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
void Sched_MC_mTask(void) {
	u8 runMode = PMSM_FOC_CtrlMode();
#if ANGLE_TEST
	_debug_angle();
#endif
	if (runMode != CTRL_MODE_OPEN) {
		eCtrl_Running();
		float f_throttle = get_throttle_float();
		if (f_throttle != motor.throttle) {
			motor.throttle = f_throttle;
			torque_speed_target(runMode, f_throttle);
		}		
		PMSM_FOC_idqCalc();
	}
}