#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/core/foc_core.h"
#include "math/fast_math.h"
#include "bsp/delay.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"

static bool _motor_started = false;

bool mc_start(u8 mode) {
	if (_motor_started) {
		return true;
	}
	if (mode > TRQ_MODE) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (mc_get_speed() > 10) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	pwm_turn_on_low_side();
	task_udelay(500);
	phase_current_calibrate();
	PMSM_FOC_Start(mode);
	pwm_start();
	adc_start_convert();
	_motor_started = true;
	return true;
}

bool mc_stop(void) {
	if (!_motor_started) {
		return true;
	}
	if (mc_get_speed() > 10) {
		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_started = false;
	return true;
}

bool mc_target_speed(s16 rpm) {
	return PMSM_FOC_Set_Speed(rpm, 1000);
}

bool mc_cruise_speed(s16 rpm) {
	return PMSM_FOC_Set_CruiseSpeed(rpm);
}

bool mc_enable_cruise(boolean_T enable) {
	return PMSM_FOC_EnableCruise(enable);
}

s16 mc_get_speed(void) {
	return PMSM_FOC_GetSpeed();
}

bool mc_throttle_released(void) {
	return true;
}