#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;
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 void limiter_init(void) {
	mc_limit_t *limiter = nv_get_limter();
	for (int i = 0; i < 3; 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) >= 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) {
	if (!_inited) {
		_inited = true;
		limiter_init();
	}
	u16 motor_lim = _motor_limit();
	u16 mos_lim   = _mos_limit();
	return min(motor_lim, mos_lim);
}

u16 vbus_current_vol_lower_limit(void) {
	if (!_inited) {
		_inited = true;
		limiter_init();
	}
	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;
}