#include "bsp/gpio.h"
#include "bsp/ml5238.h"
#include "libs/logger.h"
#include "state.h"
#include "iostate.h"
#include "measure.h"
#include "measure_task.h"
#include "health.h"

#define MAX_CURRENT_FOR_CHARGER (20*1000) //最大充电电流20A
#define MIN_VOLTAGE_FOR_DISCHARGER (2.2f * CELLS_NUM * 1000) //允许能放电的最小电压
#define MIN_VOLTAGE_FOR_RECOVERY_DISCHARGER (2.3f * CELLS_NUM * 1000) //恢复放电的最小电压
#define MIN_VOLTAGE_FOR_POWER_DOWN (2.1f * CELLS_NUM* 1000)
#define SIGLE_CELL_LOWER_DISCHARGER_VOLTAGE (1820) //最小允许的电芯放电电压 1.8v, 考虑到采样的误差取 1.82

#define SIGLE_CELL_MAX_CHARGER_VOLTAGE (3800)//最大允许充电电压，3.9v,考虑到采样的误差取 3.88

static int8_t charger_normal_low_temp[PACK_TEMPS_NUM] = {0,0,0,0}; //正常的充电最低温度
static int8_t charger_normal_high_temp[PACK_TEMPS_NUM] = {50,50,50,55}; //正常的充电最高温度
static int8_t charger_lower_low_temp[PACK_TEMPS_NUM] = {-5,-5,-5,0}; //需要停止充电的最低温度
static int8_t charger_higher_high_temp[PACK_TEMPS_NUM] = {55,55,55,60}; //需要停止充电的最高温度

static int8_t discharger_normal_low_temp[PACK_TEMPS_NUM] = {-10,-10,-10,-5};//正常的放电最低温度
static int8_t discharger_normal_high_temp[PACK_TEMPS_NUM] = {50,50,50,55};//正常的放电最高温度
static int8_t discharger_lower_low_temp[PACK_TEMPS_NUM] = {-15,-15,-15,-10}; //需要停止放电的最低温度
static int8_t discharger_higher_high_temp[PACK_TEMPS_NUM] = {55,55,55,60};//需要停止放电的最高温度

/*定义低温和正常温度下的电池保护参数, [0]低温参数, [1]常温参数 */
/*能提供大电的最小电压*/
static float min_discharger_vol[] = {(2100 * CELLS_NUM), (2300 * CELLS_NUM)};//允许能放电的最小电压
static float min_discharger_recovery_vol[] = {(2200 * CELLS_NUM), (2400 * CELLS_NUM)};//恢复放电的最小电压
static float min_discharger_cell_vol[] = {2100, 2300};//允许能放电的最小电芯电压
static float min_discharger_cell_recovery_vol[] = {2200, 2400};//恢复放电的最小电芯电压

/*能提供动力的最小电压*/
static float min_discharger_power_vol[] = {(2100 * CELLS_NUM), (2300 * CELLS_NUM)}; //允许能提供动力的最小电压
static float min_discharger_power_recovery_vol[] = {(2100 * CELLS_NUM), (2300 * CELLS_NUM)}; //恢复能提供动力的最小电压
static float min_discharger_power_cell_vol[] = {2100, 2300}; //允许能提供动力的最小电芯电压
static float min_discharger_power_recovery_cell_vol[] = {2100, 2300}; //恢复能提供动力的最小电芯电压

/*电池PowerDown的最小电压 */
static float min_discharger_pdown_vol[] = {(2000 * CELLS_NUM), (2400 * CELLS_NUM)}; //power down的最小电压
static float min_discharger_pdown_cell_vol[] = {1900, 2200};                        //power down的最小电芯电压

/* health 模块，只检测状态，不做任何控制,如果有异常情况，控制中心会统一处理  */
static void check_ml5238_state(int event);
static void init_detect_timer(void);
static void load_delect_handler(shark_timer_t *timer);
static void charger_detect_handler(shark_timer_t *timer);
static void _aux_lock_timer_handler(shark_timer_t *t);
static void _aux_unlock_timer_handler(shark_timer_t *t);
static bms_health_t _health;
static debounce_timer_t _load_detect_timer;
static debounce_timer_t _charger_detect_timer;
void health_init(void){
	/* 5238如果有异常情况，比如短路，负载移除，通过这个handler上报 */
	ml5238_register_notify_handler(check_ml5238_state);
	init_detect_timer();
	set_log_level(MOD_HEALTH, L_debug);
	for (int i = 0; i < CELLS_NUM; i++){
		_health.internal_resistance[i] = 5;//毫欧,暂时用一个固定数据，后期需要计算R0=(U2-U1)/(I1-I2) - R1(R1为电路上的等效电阻+采样电阻)
	}
	_health.is_work_temp_lower = 1;
}

bms_health_t *bms_health(){
	return &_health;
}

static void init_detect_timer(void){
	_load_detect_timer._timer.handler = load_delect_handler;
	_load_detect_timer.max_count = 100;
	_load_detect_timer.interval = 10;

	_charger_detect_timer._timer.handler = charger_detect_handler;
	_charger_detect_timer.max_count = 100;
	_charger_detect_timer.interval = 10;	
}

static void load_delect_handler(shark_timer_t *timer){
	if (ml5238_is_load_disconnect()){
		_load_detect_timer.count ++;
	}else {
		_load_detect_timer.count = 0;
	}
	if (_load_detect_timer.count >= _load_detect_timer.max_count) {
		_health.load_current_short = 0; //负载移除，clear load current short
		ml5238_enable_load_detect(0);
		health_debug("load disconnect\n");
	}else {		
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}
}

static void charger_detect_handler(shark_timer_t *timer){
	if (!io_state()->charger_detect) {
		_charger_detect_timer.count ++;
	}else {
		_charger_detect_timer.count = 0;
	}
	if (_charger_detect_timer.count >= _charger_detect_timer.max_count){
		_health.charger_over_current = 0;
	}else {
		shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
	}
}

static void check_ml5238_state(int event){
	health_warning("ml5238 event=0x%x\n", event);
	if (event == ML5238_Event_Charger_Over_Current){
		shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
	}else if (event == ML5238_Event_Short_Current) { //ml5238触发短路保护,充放电mos全部关闭
		_health.load_current_short = 1;
		ml5238_enable_load_detect(1); //打开负载检测
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}else if (event == ML5238_Event_Load_Disconnect) {
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}
}

static void debug_health(void){
	uint32_t *value = (uint32_t *)&_health;
	if (*value != 0){
		//health_error("health value = 0x%x\n", *value);
	}
}

/* 检测电流情况，看是否过流等 */
static debounce_t _charger_over_current = {
	.count = 0,
	.max_count = 10
};
void check_current_state(void){
	//判断是否过流充电,放电过流通过5238来检测
	if (bms_state_get()->charging && !_health.charger_over_current) {
		float current = measure_value()->load_current;
		if (current > MAX_CURRENT_FOR_CHARGER) {
			_charger_over_current.count ++;
		}else {
			_charger_over_current.count = 0;
		}
		if (_charger_over_current.count >= _charger_over_current.max_count){
			_health.charger_over_current = 1;
			_charger_over_current.count = 0;
			shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
		}
	}
	
	debug_health();
}

/* 检测pack电压，cell电压，pack电压过低触发powerdown*/
static debounce_t _discharger_lower_voltage = {.count = 0, .max_count = 20, .init_count = 10};
static debounce_t _power_down_voltage = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _sigle_cell_discharger_lower_vol = {.count = 0, .max_count = 10, .init_count = 5};
static debounce_t _sigle_cell_charger_max_vol = {.count = 0, .max_count = 30, .init_count = 15};
static debounce_t _shut_discharger_lower_voltage = {.count = 0, .max_count = 20,};
static debounce_t _shut_discharger_cell_lower_voltage = {.count = 0, .max_count = 20,};

static int judge_debounce(int input, debounce_t *d){
	if (input) {
		d->count ++;
		if (d->count >= d->max_count){
			d->count = 0;
			return 1;
		}
		return 0;
	}else {
		d->count = d->init_count;
		return 0;
	}
}

static int _can_powerdown(void){
	if (io_state()->charger_detect || bms_state_get()->charging){
		return 0;
	}
	if ((bms_state_get()->pack_voltage <= min_discharger_pdown_vol[_health.is_work_temp_lower] ||
			bms_state_get()->cell_min_vol <= min_discharger_pdown_cell_vol[_health.is_work_temp_lower])){
		return 1;
	}
	return 0;
}


void check_voltage_state(void) {
	if (bms_state_get()->charging){ //check sigle cell's voltage for charger
		if ((bms_state_get()->cell_max_vol>= SIGLE_CELL_MAX_CHARGER_VOLTAGE)){
			if (judge_debounce(!_health.sigle_cell_over_voltage, &_sigle_cell_charger_max_vol)){
				_health.sigle_cell_over_voltage = 1;
			}
		}else if ((bms_state_get()->cell_max_vol < SIGLE_CELL_MAX_CHARGER_VOLTAGE)){
			if (judge_debounce(_health.sigle_cell_over_voltage, &_sigle_cell_charger_max_vol)){
				_health.sigle_cell_over_voltage = 0;
			}
		}
	}else{ 
		//check sigle cell's voltage for discharger
		if ((bms_state_get()->cell_min_vol <= min_discharger_cell_vol[_health.is_work_temp_lower])){
			if (judge_debounce(!_health.sigle_cell_lower_voltage, &_sigle_cell_discharger_lower_vol)){
				_health.sigle_cell_lower_voltage = 1;
			}
		}else if ((bms_state_get()->cell_min_vol >= min_discharger_cell_recovery_vol[_health.is_work_temp_lower])){
			if (judge_debounce(_health.sigle_cell_lower_voltage, &_sigle_cell_discharger_lower_vol)){
				_health.sigle_cell_lower_voltage = 0;
			}
		}
		//check sigle pack's voltage for discharger
		if (bms_state_get()->pack_voltage <= min_discharger_vol[_health.is_work_temp_lower]){
			if (judge_debounce(!_health.discharger_lower_voltage, &_discharger_lower_voltage)){
				_health.discharger_lower_voltage = 1;
			}
		}else if (bms_state_get()->pack_voltage >= min_discharger_recovery_vol[_health.is_work_temp_lower]){
			if (judge_debounce(_health.discharger_lower_voltage, &_discharger_lower_voltage)){
				_health.discharger_lower_voltage = 0;
			}
		}
		//check for shutdown power
		if ((bms_state_get()->cell_min_vol <= min_discharger_power_cell_vol[_health.is_work_temp_lower])){
			if (judge_debounce(!_health.discharger_cell_shutpower_voltage, &_shut_discharger_cell_lower_voltage)){
				_health.discharger_cell_shutpower_voltage = 1;
			}
		}else if ((bms_state_get()->cell_min_vol >= min_discharger_power_recovery_cell_vol[_health.is_work_temp_lower])){
			if (judge_debounce(_health.discharger_cell_shutpower_voltage, &_shut_discharger_cell_lower_voltage)){
				_health.discharger_cell_shutpower_voltage = 0;
			}
		}
		if ((bms_state_get()->cell_min_vol <= min_discharger_power_vol[_health.is_work_temp_lower])){
			if (judge_debounce(!_health.discharger_cell_shutpower_voltage, &_shut_discharger_lower_voltage)){
				_health.discharger_shutpower_voltage = 1;
			}
		}else if ((bms_state_get()->cell_min_vol >= min_discharger_power_recovery_vol[_health.is_work_temp_lower])){
			if (judge_debounce(_health.discharger_cell_shutpower_voltage, &_shut_discharger_lower_voltage)){
				_health.discharger_shutpower_voltage = 0;
			}
		}		
	}	

	/* check for power down */
	if (_can_powerdown()){
		if (judge_debounce(!_health.powerdown_lower_voltage, &_power_down_voltage)) {
			/*
			 * no need to clear powerdown(bms is shutdown), when charger insert, 
			 * system will power on with powerdown_lower_voltage cleared
			*/			
			_health.powerdown_lower_voltage = 1;
		}
	}
	debug_health();
}


/* 检测温度情况，看是否过高温，或者过低温 */

static debounce_t _charger_over_temp      = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _charger_lower_temp     = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _charger_normal_temp    = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _discharger_over_temp   = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _discharger_lower_temp  = {.count = 0, .max_count = 10, .init_count = 0};
static debounce_t _discharger_normal_temp = {.count = 0, .max_count = 10, .init_count = 0};

static int _is_over_temp(int8_t *temps){
	for (int i = 0; i < PACK_TEMPS_NUM; i++){
		if (measure_value()->pack_temp[i] >= temps[i]){
			return 1;
		}
	}
	return 0;
}

static int _is_low_temp(int8_t *temps){
	for (int i = 0; i < PACK_TEMPS_NUM; i++){
		if (measure_value()->pack_temp[i] <= temps[i]){
			return 1;
		}
	}
	return 0;
}

static uint8_t small_power_detect_count = 0;
static shark_timer_t _aux_lock_timer = {.handler = _aux_lock_timer_handler};
static shark_timer_t _aux_unlock_timer = {.handler = _aux_unlock_timer_handler};

static void _aux_lock_timer_handler(shark_timer_t *t){
	AUX_VOL_OPEN(1);
	shark_timer_post( &_aux_unlock_timer, 200);
	health_debug("open aux[re-enable], %lld\n", shark_get_mseconds());
	if (++small_power_detect_count >= 10){
		delay_us(1000);
		//端口电压小于阈值，判断为小电流短路
		if (get_small_current_voltage() < SMALL_CURRENT_MIN_VOLTAGE){//real short
			bms_health()->small_current_short = 1;
			AUX_VOL_OPEN(0);
			small_power_detect_count = 0;
			shark_timer_post( &_aux_lock_timer, 30 * 1000); //30s后再次尝试打开
			shark_timer_cancel(&_aux_unlock_timer);
			health_debug("set aux short current, and retry after 30s\n");
		}
	}
}

static void _aux_unlock_timer_handler(shark_timer_t *t){
	if (!io_state()->aux_lock_detect){
		health_debug("unlock aux detect\n");
		small_power_detect_count = 0;
		bms_health()->small_current_short = 0;
		AUX_VOL_OPEN(1);
	}
}


void health_process_aux_lock(void){
	if (io_state()->aux_lock_detect) {
		if (AUX_VOL_IS_OPEN()){
			AUX_VOL_OPEN(0);
			health_debug("close aux[locked], %lld\n", shark_get_mseconds());
			shark_timer_post( &_aux_lock_timer, 1);
			shark_timer_cancel(&_aux_unlock_timer);
		}
	}else {
		if (AUX_VOL_IS_OPEN()) {
			shark_timer_post( &_aux_unlock_timer, 500);
			shark_timer_cancel(&_aux_lock_timer);
		}
	}
}


void check_temp_state(void){
	if (bms_state_get()->charging){
		if (!_health.charger_over_temp){
			if (_is_over_temp(charger_higher_high_temp)) {//超过允许的最高温度
				debounce_inc(_charger_over_temp);
			}else {
				debounce_reset(_charger_over_temp);
			}
			if (debounce_reach_max(_charger_over_temp)){
				_health.charger_over_temp = 1;
				debounce_reset(_charger_over_temp);
			}			
		}
		if (!_health.charger_lower_temp){
			if (_is_low_temp(charger_lower_low_temp)) {//低于允许的最低温度
				debounce_inc(_charger_lower_temp);
			}else {
				debounce_reset(_charger_lower_temp);
			}
			if (debounce_reach_max(_charger_lower_temp)) {
				_health.charger_lower_temp = 1;
				debounce_reset(_charger_lower_temp);
			}
		}
		if (_health.charger_over_temp || _health.charger_lower_temp) {
			if (!_is_over_temp(charger_normal_high_temp) && !_is_low_temp(charger_normal_low_temp)){
				debounce_inc(_charger_normal_temp);
			}else {
				debounce_reset(_charger_normal_temp);
			}
			if (debounce_reach_max(_charger_normal_temp)){
				_health.charger_over_temp = 0;
				_health.charger_lower_temp = 0;
				debounce_reset(_charger_normal_temp);
			}	
		}
	}else {
		if (!_health.discharger_over_temp){
			if (_is_over_temp(discharger_higher_high_temp)) {//超过允许的最高温度
				debounce_inc(_discharger_over_temp);
			}else {
				debounce_reset(_discharger_over_temp);
			}
			if (debounce_reach_max(_discharger_over_temp)){
				_health.discharger_over_temp = 1;
				debounce_reset(_discharger_over_temp);
			}			
		}
		if (!_health.discharger_lower_temp){
			if (_is_low_temp(discharger_lower_low_temp)) {//低于允许的最低温度
				debounce_inc(_discharger_lower_temp);
			}else {
				debounce_reset(_discharger_lower_temp);
			}
			if (debounce_reach_max(_discharger_lower_temp)) {
				_health.discharger_lower_temp = 1;
				debounce_reset(_discharger_lower_temp);
			}
		}
		if (_health.discharger_over_temp || _health.discharger_lower_temp) {
			if (!_is_over_temp(discharger_normal_high_temp) && !_is_low_temp(discharger_normal_low_temp)){
				debounce_inc(_discharger_normal_temp);
			}else {
				debounce_reset(_discharger_normal_temp);
			}
			if (debounce_reach_max(_discharger_normal_temp)){
				_health.charger_over_temp = 0;
				_health.charger_lower_temp = 0;
				debounce_reset(_discharger_normal_temp);
			}	
		}

	}
	debug_health();
}