#include "os/os_types.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "libs/logger.h"
#include "math/fast_math.h"
#include "foc/mc_config.h"

static float F_Te = 0.0f; //电磁转矩生成的力
static float F_Air = 0.0f; //空气阻力
static float F_accl = 0.0f; //加速阻力
static float mot_vel = 0.0f; //公里每小时
static float mot_accl = 0.0f; //加速度 m/ss
static float C = 1.4f; //轮子周长
static float gear_ratio = 6.5f; //传动比
static float M  = 190.0f; //190公斤
static bool _init = false;
void F_all_Calc(void) {
	if (!_init) {
		_init = true;
		C = (float)mc_conf()->m.wheel_c / 100.0f;
		M = (float)mc_conf()->m.vehicle_w;
		gear_ratio = (float)mc_conf()->m.gear_ratio;
	}
	float kmph = PMSM_FOC_GetSpeed() / gear_ratio * C * 60.0f / 1000.0f;
	if (mot_vel == 0.0f) {
		mot_vel = kmph;
		mot_accl = 0.0f;
		F_Air = 0.0f;
		F_accl = 0.0f;
		return;
	}
	float delta_mph = (kmph - mot_vel) * 1000.0f / 3600.0f;
	float diff = delta_mph / (CONFIG_SPD_CTRL_MS / 1000.0f);
	LowPass_Filter(mot_accl, diff, 0.01f);

	F_accl = M * mot_accl;
	F_Air = SQ(kmph) * 0.02f;
	PMSM_FOC_Ctrl *foc = PMSM_FOC_Get();
	F_Te = 1.5f * foc->params.n_poles * (foc->params.flux * foc->out.s_RealIdq.q + 
					(foc->params.ld - foc->params.lq) * foc->out.s_RealIdq.q * foc->out.s_RealIdq.d) * gear_ratio / (C / (2 * PI));
	mot_vel = kmph;
}

float F_get_Te(void) {
	return F_Te;
}

float F_get_accl(void) {
	return F_accl;
}

float F_get_air(void) {
	return F_Air;
}

float F_get_MotAccl(void) {
	return mot_accl;
}

void F_debug(void) {
	sys_debug("F: %f, %f, %f, %f, %f\n", F_Te, F_Air, F_accl, mot_vel, mot_accl);
	sys_debug("Vel: %f, %f, %f\n", C, M, gear_ratio);
}