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

#define CONFIG_ENTER_TCS_THRO  200
#define CONFIG_EXIT_TCS_THRO  80

void etcs_init(etcs_t *etcs) {
	memset(etcs, 0, sizeof(etcs_t));
	etcs->output = 1.0f;
}

void etcs_set_fvel(etcs_t *etcs, float vel) {
	float vel_delta = vel - etcs->f_fvel;
	float ts_delta = (float)get_delta_ms(etcs->n_fv_ts);
	etcs->f_fvel = vel;
	etcs->n_fv_ts = get_tick_ms();
	float acc = vel_delta / ts_delta;
	etcs->f_acc = LowPass_Filter(etcs->f_acc, acc, 0.2f);
}

float etcs_process(etcs_t *etcs) {
	if (!etcs->b_etcs_en) {
		etcs->n_etcs_run_cnt = 0;
		etcs->output = 1.0f;
		return etcs->output;
	}
	float mot_vel = mot_contrl_get_speed(&motor.controller);
	float f_vel = etcs->f_fvel + etcs->f_acc * (float)get_delta_ms(etcs->n_fv_ts);
	if (!etcs->b_etcs_running && (mot_vel >= (f_vel + CONFIG_ENTER_TCS_THRO))) {
		etcs->b_etcs_running = true;
	}else if (etcs->b_etcs_running && (mot_vel < (f_vel + CONFIG_EXIT_TCS_THRO))) {
		etcs->b_etcs_running = false;
	}
	if (etcs->b_etcs_running) {
		if (etcs->output > 0) {
			etcs->n_etcs_run_cnt ++;
			etcs->output = etcs->output - 0.01f;
			if (etcs->output < 0) {
				etcs->output = 0;
			}
		}
	}else {
		if (etcs->n_etcs_run_cnt > 0) {
			etcs->n_etcs_run_cnt--;
			etcs->output = etcs->output + 0.01f;
			if (etcs->n_etcs_run_cnt <= 0 || etcs->output >= 1.0f) {
				etcs->n_etcs_run_cnt = 0;
				etcs->output = 1.0f;
			}		
		}
	}
	return etcs->output;
}