#include "foc/core/torque.h"
#include "foc/foc_config.h"
#include "foc/motor/motor.h"
#include "foc/core/e_ctrl.h"
#include "foc/core/PMSM_FOC_Core.h"

/*
通过查表获取对应扭矩和速度时的Id和IQ的分配
*/
void torque_init(void) {

}

void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
	dq_out->d = 0;
	dq_out->q = torque;
}

float speed_target_from_throttle(float f_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 (PMSM_FOC_GetSpeedLimit() * ration);
}

float torque_target_from_throttle(float f_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 (PMSM_FOC_GetTorqueLimit() * ration);
}


void torque_speed_target(u8 run_mode, float f_throttle) {
	if (run_mode == CTRL_MODE_SPD) {
		float speed_Ref = speed_target_from_throttle(f_throttle);
		if (PMSM_FOC_is_epmMode()) {
			if (speed_Ref == 0.0f) {
				PMSM_FOC_Start_epmMove(false, EPM_Dir_None);
			}else {
				PMSM_FOC_Start_epmMove(true, EPM_Dir_Forward);
			}
		}
		PMSM_FOC_Set_Speed(speed_Ref);
	}else if (run_mode == CTRL_MODE_TRQ) {
		if (mc_throttle_released()) {
			eCtrl_enable_eBrake(true);
			PMSM_FOC_Set_Torque(0);
		}else {
			float torque = torque_target_from_throttle(f_throttle);
			eCtrl_enable_eBrake(false);
			PMSM_FOC_Set_Torque(torque);
		}
	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
			eCtrl_enable_eBrake(false);
		}
	}
}