#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"
#include "app/nv_storage.h"
/*
通过查表获取对应扭矩和速度时的Id和IQ的分配
*/
static torque_lut_t *_trq_tbl = NULL;

void torque_init(void) {
	_trq_tbl = nv_get_trq_tlb();
}

void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
	if ((_trq_tbl == NULL) || (torque < 0 || rpm < 0)) {
		dq_out->d = 0;
		dq_out->q = torque;
		return;
	}
	int trq_idx = (int)torque / TBL_TRQ_INTVAL;
	int rpm_idx = (int)rpm / TBL_SPD_INTVAL;
	if (trq_idx >= MAX_TRQ_POINTS) {
		trq_idx = MAX_TRQ_POINTS -1;
	}
	if (rpm_idx >= MAX_SPD_POINTS) {
		rpm_idx = MAX_SPD_POINTS -1;
	}
	s16 d = _trq_tbl->dq[trq_idx][rpm_idx].d;
	s16 q = _trq_tbl->dq[trq_idx][rpm_idx].q;
	if (trq_idx < MAX_TRQ_POINTS - 1) {
		trq_idx += 1;
	}
	if (rpm_idx < MAX_SPD_POINTS - 1) {
		rpm_idx += 1;
	}
	s16 d_delta = _trq_tbl->dq[trq_idx][rpm_idx].d - d;
	s16 q_delta = _trq_tbl->dq[trq_idx][rpm_idx].q - q;
	float comp_ceof = 0.5f * ((torque - torque/TBL_TRQ_INTVAL*TBL_TRQ_INTVAL)/(float)TBL_TRQ_INTVAL + (rpm - rpm/TBL_SPD_INTVAL*TBL_SPD_INTVAL)/(float)TBL_SPD_INTVAL);

	dq_out->d = d + d_delta * comp_ceof;
	dq_out->q = q + q_delta * comp_ceof;

}

float speed_target_from_throttle(float f_throttle) {

	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
		return 0;
	}
	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);

	float ration = delta / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);

	return (PMSM_FOC_GetSpeedLimit() * ration);
}

float torque_target_from_throttle(float f_throttle) {
	if (f_throttle <= (nv_get_foc_params()->n_minThroVol)) {
		return 0;
	}
	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);

	float ration = delta / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);

	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 (mc_is_epm()) {
			if (speed_Ref == 0.0f) {
				mc_throttle_epm_move(EPM_Dir_None);
			}else {
				mc_throttle_epm_move(EPM_Dir_Forward);
			}
		}else {
			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);
		}
	}
}