#include "foc/limit.h"
#include "foc/core/controller.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 temp_lim_t motor_tl;
static temp_lim_t mosfet_t;
static vol_limt_t vbus;
static bool _inited = false;
static bool _can_recovery = true;

static void limiter_init(void) {
	if (_inited) {
		return;
	}
	motor_tl.high = mc_conf()->p_mot[0].enter_pointer;
	motor_tl.high_lim = mc_conf()->p_mot[0].limit_value;
	motor_tl.mid = mc_conf()->p_mot[1].enter_pointer;
	motor_tl.mid_lim = mc_conf()->p_mot[1].limit_value;
	motor_tl.start = mc_conf()->p_mot[2].enter_pointer;
	motor_tl.start_lim = mc_conf()->p_mot[2].limit_value;
	motor_tl.curr_lim = 1.0f;
	motor_tl.temp = sample_motor_temp();
	sys_debug("mot: %d-%d, %d-%d, %d-%d\n", motor_tl.high, motor_tl.high_lim, motor_tl.mid, motor_tl.mid_lim, motor_tl.start, motor_tl.start_lim);
	mosfet_t.high = mc_conf()->p_mos[0].enter_pointer;
	mosfet_t.high_lim = mc_conf()->p_mos[0].limit_value;
	mosfet_t.mid = mc_conf()->p_mos[1].enter_pointer;
	mosfet_t.mid_lim = mc_conf()->p_mos[1].limit_value;
	mosfet_t.start = mc_conf()->p_mos[2].enter_pointer;
	mosfet_t.start_lim = mc_conf()->p_mos[2].limit_value;
	mosfet_t.curr_lim = 1.0f;
	mosfet_t.temp = sample_mos_temp();
	sys_debug("mos: %d-%d, %d-%d, %d-%d\n", mosfet_t.high, mosfet_t.high_lim, mosfet_t.mid, mosfet_t.mid_lim, mosfet_t.start, mosfet_t.start_lim);
	vbus.crit_low = mc_conf()->c.min_dc_vol;
	vbus.lower = mc_conf()->p_vol.enter_pointer;
	vbus.lower_lim = mc_conf()->p_vol.limit_value;
	sys_debug("vbus: %d, %d-%d\n", vbus.crit_low, vbus.lower, vbus.lower_lim);
	_inited = true;
}

static bool temp_limit_check(temp_lim_t *limit, s16 temp) {
	if (!limit->is_limit) {
		if (temp < limit->start) {
			limit->lim_ticks = 0;
		}else {
			if (++limit->lim_ticks >= 1) {
				limit->is_limit = true;
				limit->lim_ticks = 0;
			}
		}
	}
	if (limit->is_limit) {
		if (temp < limit->start) {
			if (++limit->lim_ticks >= 1) {
				limit->is_limit = false;
				limit->lim_ticks = 0;
			}
		}else {
			limit->lim_ticks = 0;
		}
	}

	return limit->is_limit;
}

#define TEMP_SENSOR_ERR_CNT 20

float get_temp_limit_value(temp_lim_t *limit, s16 temp, s16 err_temp) {
	if ((temp == err_temp) || ABS(temp - limit->temp) >= 20) {
		limit->temp = temp;
		if (limit->temp_sensor_err < TEMP_SENSOR_ERR_CNT) {
			limit->temp_sensor_err++;
			return limit->curr_lim;
		}else {
			return min(limit->curr_lim, 0.5f); //温度传感器异常，返回上次的限流
		}
	}else {
		limit->temp_sensor_err = 0;
	}
	limit->temp = temp;
	if (!temp_limit_check(limit, temp)) {
		limit->curr_lim = 1.0f;
	}else {
		if (temp < limit->start) {
			return limit->curr_lim; //keep prev limit value
		}else if (temp > limit->high) {
			limit->curr_lim = 0;
		}else {
			if (temp >= limit->start && temp <= limit->mid) {
				limit->curr_lim = f_map(temp, limit->start, limit->mid, limit->start_lim, limit->mid_lim)/100.0f;
			}else {
				limit->curr_lim = f_map(temp, limit->mid, limit->high, limit->mid_lim, limit->high_lim)/100.0f;
			}
		}
	}

	return limit->curr_lim;
}

/* this maybe limit power or torque, based on the current power */
float motor_temp_high_limit(void) {
	limiter_init();
	s16 temp = get_motor_temp_raw();
	float value = get_temp_limit_value(&motor_tl, temp, 300);
	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
		}
	}
	if (value == 0.0f) {
		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
		}
	}else if (_can_recovery){
		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
	}

	return value;
}

float motor_temp_high_limit_test(s16 temp) {
	limiter_init();
	float value = get_temp_limit_value(&motor_tl, temp, 300);
	if (motor_tl.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
		if (mc_set_critical_error(FOC_CRIT_MOT_TEMP_Sensor)) {
			mc_crit_err_add(FOC_CRIT_MOT_TEMP_Sensor, temp, motor_tl.temp);
		}
	}
	if (value == 0.0f) {
		if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
			mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
		}
	}else if (_can_recovery){
		mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
	}

	return value;
}


/* limit the max torque(max phase current) */
float mos_temp_high_limit(void) {
	limiter_init();
	s16 temp = get_mos_temp_raw();
	float value = get_temp_limit_value(&mosfet_t, temp, -40);
	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
		}
	}
	if (value == 0.0f) {
		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
		}
	}else if (_can_recovery){
		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
	}

	return value;
}

float mos_temp_high_limit_test(s16 temp) {
	limiter_init();
	float value = get_temp_limit_value(&mosfet_t, temp, -40);
	if (mosfet_t.temp_sensor_err == TEMP_SENSOR_ERR_CNT) {
		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Sensor)) {
			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Sensor, temp, mosfet_t.temp);
		}
	}
	if (value == 0.0f) {
		if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
			mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(mot_contrl()));
		}
	}else if (_can_recovery){
		mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
	}

	return value;
}


/* limit the DC bus current */
u16 vbus_voltage_low_limit(void) {
	limiter_init();
	s16 vol = (s16)sample_vbus_raw();
	if (!vbus.is_limit && (vol <= vbus.lower)) {
		vbus.is_limit = true;
		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
			}
		}
	}else if (vbus.is_limit && (vol > vbus.lower)) {
		vbus.is_limit = false;
		if (_can_recovery) {
			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
		}
	}
	if (vol > vbus.lower) {
		return HW_LIMIT_NONE;
	}else if (vol < vbus.crit_low) {
		return 0.0f;
	}
	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
}

u16 vbus_voltage_low_limit_test(s16 vol) {
	limiter_init();
	if (!vbus.is_limit && (vol <= vbus.lower)) {
		vbus.is_limit = true;
		if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
			if (mot_contrl_get_speed(mot_contrl()) > CONFIG_ZERO_SPEED_RPM) {
				mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(mot_contrl()));
			}
		}
	}else if (vbus.is_limit && (vol > vbus.lower)) {
		vbus.is_limit = false;
		if (_can_recovery) {
			mc_clr_critical_error(FOC_CRIT_UN_Vol_Err);
		}
	}
	if (vol > vbus.lower) {
		return HW_LIMIT_NONE;
	}else if (vol < vbus.crit_low) {
		return 0.0f;
	}
	return (u16)f_map(vol, vbus.crit_low, vbus.lower, 0.0f, vbus.lower_lim);
}


bool motor_temp_limited(void) {
	return motor_tl.is_limit;
}

bool mos_temp_limited(void) {
	return mosfet_t.is_limit;
}