#include "foc/motor/motor.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/core/etcs.h"
#include "math/fast_math.h"

static bool b_etcs_en = true;
static bool b_etcs_running = false;
static float f_fvel = 0, f_torque_ref, f_torque_tcs, f_acc = 0.0f;
static u32   n_fv_ts = 0;
static int  n_etcs_run_cnt = 0;
#define CONFIG_ENTER_TCS_THRO  200
#define CONFIG_EXIT_TCS_THRO  80
void etcs_set_fvel(float vel) {
	float vel_delta = vel - f_fvel;
	float ts_delta = (float)get_delta_ms(n_fv_ts);
	f_fvel = vel;
	n_fv_ts = get_tick_ms();
	float acc = vel_delta / ts_delta;
	f_acc = LowPass_Filter(f_acc, acc, 0.2f);
}
void etcs_set_torque(float torque) {
	f_torque_ref = torque;	
}
bool etcs_is_running(void) {
	return b_etcs_running;
}
void etcs_enable(bool enable) {
	b_etcs_en = enable;
}

void etcs_reset_torque(float torque) {
	f_torque_ref = torque;
	eCtrl_reset_Torque(torque);
}

void etcs_process(void) {
	if (!b_etcs_en) {
		PMSM_FOC_Set_Torque(f_torque_ref);
		return;
	}
	float mot_vel = PMSM_FOC_GetSpeed();
	float f_vel = f_fvel + f_acc * (float)get_delta_ms(n_fv_ts);
	if (!b_etcs_running && (mot_vel >= (f_vel + CONFIG_ENTER_TCS_THRO))) {
		b_etcs_running = true;
	}else if (b_etcs_running && (mot_vel < (f_vel + CONFIG_EXIT_TCS_THRO))) {
		b_etcs_running = false;
	}
	if (b_etcs_running) {
		if (f_torque_tcs > 0) {
			n_etcs_run_cnt ++;
			f_torque_tcs = f_torque_ref - n_etcs_run_cnt * 1.0f;
			if (f_torque_tcs < 0) {
				f_torque_tcs = 0;
			}
		}
	}else {
		if (n_etcs_run_cnt <= 0) {
			f_torque_tcs = f_torque_ref;
		}else {
			n_etcs_run_cnt--;
			f_torque_tcs = f_torque_tcs + 1.0f;
			if (n_etcs_run_cnt <= 0 || f_torque_tcs >= f_torque_ref) {
				n_etcs_run_cnt = 0;
				f_torque_tcs = f_torque_ref;
			}
		}
	}
	PMSM_FOC_Set_Torque(f_torque_tcs);
}