#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/foc_config.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"

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

void mc_init(void) {
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	PMSM_FOC_CoreInit();
	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);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	eCtrl_init(1000, 2000);
	motor_encoder_start(motor.s_direction);
	PMSM_FOC_Start(mode);
	pwm_turn_on_low_side();
	task_udelay(500);
	phase_current_start_cali();
	pwm_start();
	adc_start_convert();
	phase_current_wait_cali();
	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);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		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_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_start_cali();
	pwm_start();
	adc_start_convert();
	phase_current_wait_cali();
	PMSM_FOC_Set_Angle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(3000);
	for (int i = 0; i < 50000000; i++) {
		for (s16 angle = 0; angle < 360; angle++) {
			PMSM_FOC_Set_Angle(angle);
			
			motor_encoder_offset(angle);
			
			delay_us(1000);
		}
	}
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor_encoder_offset_finish();
}

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

static float _get_speed_from_throttle(void) {
	if (motor.b_ignor_throttle) {
		return motor.s_targetFix;
	}
	float f_throttle = motor.throttle;
	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
		return 0;
	}
	float delta = f_throttle - (THROTTLE_LOW_VALUE);

	float ration = delta / (THROTTLE_MAX_VALUE - THROTTLE_LOW_VALUE);

	return (THROTTLE_MIN_RPM + PMSM_FOC_GetSpeedLimit() * ration);
}

static float _get_idq_from_throttle(void) {
	if (motor.b_ignor_throttle) {
		return motor.s_targetFix;
	}

	float f_throttle = motor.throttle;
	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
		return 0;
	}
	float delta = f_throttle - (THROTTLE_LOW_VALUE);

	float ration = delta / (THROTTLE_MAX_VALUE - THROTTLE_LOW_VALUE);

	return (THROTTLE_MIN_IDQ + MAX_iDQ * ration);
}

/*do 50 times filter*/
static void brake_timer_handler(shark_timer_t *t) {
	int count = 50;
	int settimes = 0;
	while(count-- >= 0) {
		bool b1 = gpio_input_bit_get(GPIOB, GPIO_PIN_4) == SET;
		bool b2 = gpio_input_bit_get(GPIOB, GPIO_PIN_5) == SET;
		if (b1 && b2) {
			settimes++;
		}		
	}
	if (settimes == 0) {
		PMSM_FOC_Brake(true);
	}else if (settimes == 50) {
		PMSM_FOC_Brake(false);
	}else {
		//有干扰，do nothing
	}
}

static shark_timer_t _brake_timer = TIMER_INIT(_brake_timer, brake_timer_handler);

void MC_Brake_IRQHandler(void){
	shark_timer_post(&_brake_timer, 0);
}
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;
	}
	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();
		if (get_throttle_float() != motor.throttle) {
			motor.throttle = get_throttle_float();
			if (runMode == CTRL_MODE_SPD) {
				float speed_Ref = _get_speed_from_throttle();
				PMSM_FOC_Set_Speed(speed_Ref);
			}else if (runMode == CTRL_MODE_TRQ) {
				float torque_idq = _get_idq_from_throttle();
				PMSM_FOC_Set_Torque(torque_idq);
			}
		}		
		PMSM_FOC_idqCalc();
	}
}