#include "bsp/gpio.h"
#include "bsp/ml5238.h"
#include "app/sox/measure.h"
#include "app/sox/measure_task.h"
#include "libs/shark_task.h"
#include "health.h"
#include "state.h"
#include "iostate.h"

static bms_state_t _bms_state;
static void _current_notify(void);
static void _voltage_notify(void);
static void _temperature_notify(void);

void bms_state_init(void){
	_bms_state.cell_index_of_max_vol = 0xff;
	measure_task_init(_current_notify, _voltage_notify, _temperature_notify);
	io_state_init();
	health_init();
}

bms_state_t *bms_state_get(void){
	return &_bms_state;
}

static debounce_t _charging_detect = {.count = 0, .max_count = 10};
static void check_charging(){
	if (bms_health()->charger_over_current || bms_health()->load_current_short) {
		ml5238_enable_discharger_mosfet(0);
		ml5238_enable_charger_mosfet(0); //disable charger mosfet
		_bms_state.charging = 0;
		return;
	}	
	if (!_bms_state.charging) {
		if (measure_value()->load_current >= MIN_START_CHARGER_CURRENT) {
			debounce_inc(_charging_detect);
		}else {
			debounce_reset(_charging_detect);
		}
		if (debounce_reach_max(&_charging_detect)){
			_bms_state.charging = 1;
			_bms_state.discharging = 0;
			debounce_reset(_charging_detect);
		}
	}else {
		if (measure_value()->load_current <= MIN_START_LOADING_CURRENT) {
			debounce_inc(_charging_detect);
		}else {
			debounce_reset(_charging_detect);
		}
		if (debounce_reach_max(_charging_detect)){
			_bms_state.charging = 0;
			_bms_state.discharging = 1;
			debounce_reset(_charging_detect);
		}
	}

}

static void _current_notify(void){
	check_current_state(); //check health of current
	check_charging();
}

/* 充电电流小于一定值，认为充满的时候，需要检查电芯的电压，如果发现有电芯电压过高，需要开启被动均衡 
 * 充电过程中考虑balance,主要是希望cell 电压扩散后，保证1. 单电芯不能过压， 2. 单电芯不能比平均电压过低,导致
 * 木桶效应,目标是电压最高的那个cell，尽量压制，不让电压再升高，或者升高的尽量慢一些
*/
static debounce_t _cell_balance = {.count = 10, .max_count = 20};
static void check_cell_balance(void){
	if (!_bms_state.charging){ //not charging, need not do balance
		if (_bms_state.pack_balancing){
			_bms_state.pack_balancing = 0;
			_cell_balance.count = 10;
			ml5238_cell_start_balance(0);
		}
		return;
	}

	if (_bms_state.cell_max_vol - _bms_state.cell_min_vol >= MAX_DIFF_BETWEEN_MIN_MAX_CELL){
		debounce_inc(_cell_balance);
	}else {
		debounce_dec(_cell_balance);
	}
	if (!_bms_state.pack_balancing && debounce_reach_max(_cell_balance)){
		_bms_state.pack_balancing = 1;
	}else if (_bms_state.pack_balancing && debounce_reach_zero(_cell_balance)){
		_bms_state.pack_balancing = 0;
		ml5238_cell_start_balance(0);
	}
}

static void _voltage_notify(void){
	uint16_t voltage = 0;
	uint16_t max_cell = 0;
	uint16_t min_cell = 0xf000;
	uint8_t max_index = 0;
	for (int i = 0; i < CELLS_NUM; i++){
		voltage += measure_value()->cell_vol[i];
		if (max_cell > measure_value()->cell_vol[i]){
			max_cell = measure_value()->cell_vol[i];
			max_index = i;
		}
		if (min_cell < measure_value()->cell_vol[i]){
			min_cell = measure_value()->cell_vol[i];
		}		
	}
	_bms_state.pack_voltage = voltage;
	_bms_state.cell_max_vol = max_cell;
	_bms_state.cell_min_vol = min_cell;	
	check_voltage_state(); //check health of cell voltage
	check_cell_balance();
	if (_bms_state.pack_balancing && (max_index != _bms_state.cell_index_of_max_vol)){
		ml5238_cell_start_balance(BIT(max_index));
	}
	_bms_state.cell_index_of_max_vol = max_index;
}

static void _temperature_notify(void){
	check_temp_state(); //check health of cell/pcb temperature
}