#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 void limiter_init(void) {
	mc_limit_t *limiter = nv_get_limter();
	for (int i = 0; i < TEMP_LIMITER_NUM; i++) {
		motor_temp_lim[i].enter_pointer = limiter->motor[i].enter_pointer;
		motor_temp_lim[i].exit_pointer = limiter->motor[i].exit_pointer;
		motor_temp_lim[i].limit_value = limiter->motor[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 = limiter->mos[i].enter_pointer;
		mos_temp_lim[i].exit_pointer = limiter->mos[i].exit_pointer;
		mos_temp_lim[i].limit_value = limiter->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 = limiter->vbus.enter_pointer;
	vol_under_lim[0].exit_pointer = limiter->vbus.exit_pointer;
	vol_under_lim[0].limit_value = limiter->vbus.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);
}

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) {
	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) {
				if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Err)) {
					mc_crit_err_add_s16(FOC_CRIT_MOTOR_TEMP_Err, temp);
				}
			}else if (_can_recovery){
				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Err);
			}
			mc_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->n_max_trq * curr_value) / 100.0f);
			mc_set_motor_lim_level(i + 1);
			return lim_value;
		}
	}
	
	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) {
				if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Err)) {
					mc_crit_err_add_s16(FOC_CRIT_MOS_TEMP_Err, temp);
				}
			}else if (_can_recovery){
				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Err);
			}
			mc_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->n_max_trq * curr_value) / 100.0f);
			mc_set_mos_lim_level(i + 1);
			return lim_value;
		}
	}
	
	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)) {
				mc_crit_err_add_s16(FOC_CRIT_UN_Vol_Err, vol);
			}
			return lim_value;
		}
	}
	if (_can_recovery) {
		mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
	}
	return HW_LIMIT_NONE;
}