#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"
#include "libs/logger.h"

static limter_t motor_temp_lim[3];
static limter_t mos_temp_lim[3];
static limter_t vol_under_lim[1];
static bool _inited = false;
static bool _can_recovery = true;
static s16 mot_temp, mos_temp;

static void limiter_init(void) {
	
	for (int i = 0; i < CONFIG_TEMP_PROT_NUM; i++) {
		motor_temp_lim[i].enter_pointer = mc_conf()->p_mot[i].enter_pointer;
		motor_temp_lim[i].exit_pointer = mc_conf()->p_mot[i].exit_pointer;
		motor_temp_lim[i].limit_value = mc_conf()->p_mot[i].limit_value;
		sys_debug("%d-%d-%d\n", motor_temp_lim[i].enter_pointer, motor_temp_lim[i].exit_pointer, motor_temp_lim[i].limit_value);
		mos_temp_lim[i].enter_pointer = mc_conf()->p_mos[i].enter_pointer;
		mos_temp_lim[i].exit_pointer = mc_conf()->p_mos[i].exit_pointer;
		mos_temp_lim[i].limit_value = mc_conf()->p_mos[i].limit_value;
		sys_debug("%d-%d-%d\n", mos_temp_lim[i].enter_pointer, mos_temp_lim[i].exit_pointer, mos_temp_lim[i].limit_value);
	}
	vol_under_lim[0].enter_pointer = mc_conf()->p_vol.enter_pointer;
	vol_under_lim[0].exit_pointer = mc_conf()->p_vol.exit_pointer;
	vol_under_lim[0].limit_value = mc_conf()->p_vol.limit_value;
	//sys_debug("%d-%d-%d\n", vol_under_lim[0].enter_pointer, vol_under_lim[0].exit_pointer, vol_under_lim[0].limit_value);
	mot_temp = sample_motor_temp();
	mos_temp = sample_mos_temp();
}

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) >= 500){
			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) >= 500) {
			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) >= 5){
			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) >= 100) {
			lim->is_limit = false;
			lim->ticks = 0;
			return HW_LIMIT_NONE;
		}
		return lim->limit_value;
	}
}

static u16 _motor_limit(void) {
	static int temp_sensor_err = 0;
	static u16 lim_value  = HW_LIMIT_NONE;
	s16 temp = get_motor_temp_raw();
	if ((temp == 300) || ABS(temp - mot_temp) >= 20) {
		if (temp_sensor_err < 20) {
			temp_sensor_err++;
		}else {
			if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
				mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, mot_temp);
			}
		}
		return lim_value; //温度传感器异常，返回上次的限流
	}else {
		mot_temp = temp;
		temp_sensor_err = 0;
	}
	for(int i = 0; i < ARRAY_SIZE(motor_temp_lim); i++) {
		limter_t *lim = motor_temp_lim + i;
		lim_value = _temp_limiter(temp, lim);
		if (lim_value != HW_LIMIT_NONE) {
			if (lim_value == 0) {
				if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
					mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)PMSM_FOC_GetSpeed());
				}
			}else if (_can_recovery){
				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
			}
			gear_t *gear = mc_get_gear_config();

			float prv_lim_value;
			float next_lim_tmp;
			if (i < (ARRAY_SIZE(motor_temp_lim)-1)) {
				prv_lim_value = (float)motor_temp_lim[i + 1].limit_value;
			}else {
				prv_lim_value = 100.0f; //最低一级限流
			}
			if (i != 0) {
				next_lim_tmp = (float)motor_temp_lim[i - 1].enter_pointer;
			}else {
				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
			}
			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
			float delta_value = (prv_lim_value - (float)lim->limit_value);
			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
			curr_value = fclamp(curr_value, 0, prv_lim_value);
			lim_value = (u16)(((float)gear->max_torque * curr_value) / 100.0f);
			mc_set_motor_lim_level(i + 1);
			return lim_value;
		}else {
			mc_set_motor_lim_level(0);
		}
	}
	
	return HW_LIMIT_NONE;
}

static u16 _mos_limit(void) {
	static int temp_sensor_err = 0;
	static u16 lim_value  = HW_LIMIT_NONE;
	s16 temp = get_mos_temp_raw();
	if ((temp == -40) || ABS(temp - mos_temp) >= 20) {
		if (temp_sensor_err < 20) {
			temp_sensor_err++;
		}else {
			if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
				mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mos_temp);
			}
		}
		return lim_value; //温度传感器异常，返回上次的限流
	}else {
		mos_temp = temp;
		temp_sensor_err = 0;
	}
	for(int i = 0; i < ARRAY_SIZE(mos_temp_lim); i++) {
		limter_t *lim = mos_temp_lim + i;
		lim_value = _temp_limiter(temp, lim);
		if (lim_value != HW_LIMIT_NONE) {
			if (lim_value == 0) {
				if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
					mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)PMSM_FOC_GetSpeed());
				}
			}else if (_can_recovery){
				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
			}
			gear_t *gear = mc_get_gear_config();

			float prv_lim_value;
			float next_lim_tmp;
			if (i < (ARRAY_SIZE(mos_temp_lim)-1)) {
				prv_lim_value = (float)mos_temp_lim[i + 1].limit_value;
			}else {
				prv_lim_value = 100.0f; //最低一级限流
			}
			if (i != 0) {
				next_lim_tmp = (float)mos_temp_lim[i - 1].enter_pointer;
			}else {
				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
			}
			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
			float delta_value = (prv_lim_value - (float)lim->limit_value);
			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
			curr_value = fclamp(curr_value, 0, prv_lim_value);
			lim_value = (u16)(((float)gear->max_torque * curr_value) / 100.0f);
			mc_set_mos_lim_level(i + 1);
			return lim_value;
		}else {
			mc_set_mos_lim_level(0);
		}
	}
	
	return HW_LIMIT_NONE;
}

/* this maybe limit power or torque, based on the current power */
u16 motor_temp_high_limit(void) {
	if (!_inited) {
		_inited = true;
		limiter_init();
	}
	return _motor_limit();
}

/* limit the max torque(max phase current) */
u16 mos_temp_high_limit(void) {
	if (!_inited) {
		_inited = true;
		limiter_init();
	}
	return _mos_limit();
}

/* limit the DC bus current */
u16 vbus_under_vol_limit(void) {
	if (!_inited) {
		_inited = true;
		limiter_init();
	}
	s16 vol = (s16)sample_vbus_raw();
	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) {
			if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
					mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)PMSM_FOC_GetSpeed());
				}
			}
			return lim_value;
		}
	}
	if (_can_recovery) {
		mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
	}
	return HW_LIMIT_NONE;
}