#include "foc/limit.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "foc/motor/motor.h"
#include "foc/motor/motor_param.h"
#include "foc/samples.h"
#include "foc/mc_error.h"

static limter_t motor_temp_lim[] = {//电机过温限流，限制相电流
	{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 0},
	{.enter_pointer = 110, .exit_pointer = 100, .limit_value = CONFIG_MAX_MOTOR_TORQUE/2},
	{.enter_pointer = 100, .exit_pointer = 90, .limit_value = CONFIG_MAX_MOTOR_TORQUE/3*2},
}; 
static limter_t mos_temp_lim[] = { //mos过温限流，限制相电流
	{.enter_pointer = 95, .exit_pointer = 85, .limit_value = 0},
	{.enter_pointer = 86, .exit_pointer = 80, .limit_value = CONFIG_MAX_MOTOR_TORQUE/2},
	{.enter_pointer = 81, .exit_pointer = 70, .limit_value = CONFIG_MAX_MOTOR_TORQUE/3*2},
};  
static limter_t vol_under_lim[] = { //欠压限流,限制母线
	{.enter_pointer = 20, .exit_pointer = 20, .limit_value = 10},
};

void limter_set_under_voltage(s16 und_vol) {
	vol_under_lim[0].enter_pointer = und_vol;
	vol_under_lim[0].exit_pointer = und_vol + 8;
}

static u16 _temp_limiter(s16 temp, limter_t *lim) {
	if (!lim->is_limit) {
		if (temp < lim->enter_pointer) {
			lim->ticks = 0;
			return HW_LIMIT_NONE;
		}
		if (lim->ticks == 0) {
			lim->ticks = get_tick_ms();
		}else if (get_delta_ms(lim->ticks) >= 5000){
			lim->is_limit = true;
			lim->ticks = 0;
			return lim->limit_value;
		}
		return HW_LIMIT_NONE;
	}else {
		if (temp >= lim->exit_pointer) {
			lim->ticks = 0;
			return lim->limit_value;
		}
		if (lim->ticks == 0) {
			lim->ticks = get_tick_ms();
		}else if (get_delta_ms(lim->ticks) >= 5000) {
			lim->is_limit = false;
			lim->ticks = 0;
			return HW_LIMIT_NONE;
		}
		return lim->limit_value;
	}
}

static u16 _vol_limiter(s16 vol, limter_t *lim) {
	if (!lim->is_limit) {
		if (vol > lim->enter_pointer) {
			lim->ticks = 0;
			return HW_LIMIT_NONE;
		}
		if (lim->ticks == 0) {
			lim->ticks = get_tick_ms();
		}else if (get_delta_ms(lim->ticks) >= 1000){
			lim->is_limit = true;
			lim->ticks = 0;
			return lim->limit_value;
		}
		return HW_LIMIT_NONE;
	}else {
		if (vol <= lim->exit_pointer) {
			lim->ticks = 0;
			return lim->limit_value;
		}
		if (lim->ticks == 0) {
			lim->ticks = get_tick_ms();
		}else if (get_delta_ms(lim->ticks) >= 1000) {
			lim->is_limit = false;
			lim->ticks = 0;
			return HW_LIMIT_NONE;
		}
		return lim->limit_value;
	}
}

static u16 _motor_limit(void) {
	s16 temp = get_motor_temp();
	for(int i = 0; i < ARRAY_SIZE(motor_temp_lim); i++) {
		limter_t *lim = motor_temp_lim + i;
		u16 lim_value = _temp_limiter(temp, lim);
		if (lim_value != HW_LIMIT_NONE) {
			if (lim_value == 0) {
				mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Err);
			}
			err_add_record(FOC_CRIT_MOTOR_TEMP_Err, temp);
			return lim_value;
		}
	}
	mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Err);
	return HW_LIMIT_NONE;
}

static u16 _mos_limit(void) {
	s16 temp = get_mos_temp();
	for(int i = 0; i < ARRAY_SIZE(mos_temp_lim); i++) {
		limter_t *lim = mos_temp_lim + i;
		u16 lim_value = _temp_limiter(temp, lim);
		if (lim_value != HW_LIMIT_NONE) {
			if (lim_value == 0) {
				mc_set_critical_error(FOC_CRIT_MOS_TEMP_Err);
			}
			err_add_record(FOC_CRIT_MOS_TEMP_Err, temp);
			return lim_value;
		}
	}
	mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Err);
	return HW_LIMIT_NONE;
}

u16 torque_temp_high_limit(void) {
	u16 motor_lim = _motor_limit();
	u16 mos_lim   = _mos_limit();
	return min(motor_lim, mos_lim);
}

u16 vbus_current_vol_lower_limit(void) {
	s16 vol = get_vbus_int();
	for(int i = 0; i < ARRAY_SIZE(vol_under_lim); i++) {
		limter_t *lim = vol_under_lim + i;
		u16 lim_value = _vol_limiter(vol, lim);
		if (lim_value != HW_LIMIT_NONE) {
			mc_set_critical_error(FOC_CRIT_UN_Vol_Err);
			return lim_value;
		}
	}
	mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
	return HW_LIMIT_NONE;
}