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

static measure_value_t _measure_value;
static measure_notify _current_notify;
static measure_notify _voltage_notify;
static measure_notify _temperature_notify;

/* 测量task，用来定时测量电流，电压，温度等adc数据 */
struct means_task {
	shark_task_t _task;
	u32 delay;
	u8  index;
};

static void init_current_task(void);
static void init_voltage_task(void);
static void init_temp_task(void);

void measure_task_init(measure_notify cn, measure_notify vn, measure_notify tn){
	_current_notify = cn;
	_voltage_notify = vn;
	_temperature_notify = tn;
	measure_adc_init();
	init_current_task();
	init_voltage_task();
	init_temp_task();
	set_log_level(MOD_MEASURE, L_debug);
}

measure_value_t * measure_value(void){
	return &_measure_value;
}
extern uint32_t check_gain_error;
extern int gd32_adc_error;
extern u32 dgain_switch_count, cgain_switch_count;
void measure_log(void){
	measure_debug("Current %.4f -- %.4f\n", (float)_measure_value.load_current/1000.0f, (float)_measure_value.current_5238/1000.0f);
	measure_debug("Gain:%f, Off %f, check error = %d, %d\n", get_ml5238_gain(), get_ml5238_vos(), check_gain_error, gd32_adc_error);
	measure_debug("Gain switch: %d, %d\n", dgain_switch_count, cgain_switch_count);
}
/*
 * 测量电芯电压，计算出总电压, 测量总电流，放电和充电
*/
static struct means_task _current_task;
static struct means_task _voltage_task;

static u32 current_task_handler(void);
static u32 voltage_task_handler(void);
static void init_current_task(void){
	_current_task._task.handler = current_task_handler;
	_current_task.delay = 25;
	_measure_value.load_current = get_pack_current(&_measure_value.current_5238);

	shark_task_add(&_current_task._task);
}

static u32 current_task_handler(void){
	if (bms_work_is_calibrating()){
		return _current_task.delay;
	}
	/* 测量电流 */
	_measure_value.load_current = get_pack_current(&_measure_value.current_5238);
	_current_notify();//通知bms state 有新的电流数据

	if (abs(_measure_value.load_current) >= MIN_CURRENT_FOR_CS1180) {
		return 5;
	}
	return _current_task.delay;
}

static void init_voltage_task(void){
	_voltage_task._task.handler = voltage_task_handler;
	_voltage_task.delay = 50;
	_voltage_task.index = CELLS_NUM;
	_measure_value.min_vol = 10000;
	_measure_value.max_vol = 0;
	for (int i = 0; i < CELLS_NUM; i++){
		_measure_value.cell_vol[i] = get_cell_voltage(i);
		if (_measure_value.cell_vol[i] > _measure_value.max_vol) {
			_measure_value.max_vol = _measure_value.cell_vol[i];
		}
		if (_measure_value.cell_vol[i] < _measure_value.min_vol) {
			_measure_value.min_vol = _measure_value.cell_vol[i];
		}		
	}
	shark_task_add(&_voltage_task._task);
}

static u32 voltage_task_handler(void){
	if (bms_work_is_calibrating()){
		return _voltage_task.delay;
	}

	if (bms_state_get()->pack_balancing){ //if balance, do'nt sample cell voltage
		return _voltage_task.delay;
	}	
	//用内阻对cell的电压进行补偿，充电减，放电加
	_voltage_task.index = (_voltage_task.index + 1) % CELLS_NUM;
	_measure_value.cell_vol[_voltage_task.index] = get_cell_voltage(_voltage_task.index) - bms_health()->internal_resistance[_voltage_task.index] * _measure_value.load_current/1000;
	if (_measure_value.cell_vol[_voltage_task.index] > _measure_value.max_vol) {
		_measure_value.max_vol = _measure_value.cell_vol[_voltage_task.index];
	}
	if (_measure_value.cell_vol[_voltage_task.index] < _measure_value.min_vol) {
		_measure_value.min_vol = _measure_value.cell_vol[_voltage_task.index];
	}
	_voltage_notify();//通知bms state 有新的电压数据
	//接近过压或者欠压，加快采集速度
	if ((_measure_value.min_vol < 2300) || (_measure_value.max_vol > 3600)) {
		if (abs(_measure_value.load_current) >= MIN_CURRENT_FOR_CS1180){
			return 5;
		}
	}
	return _voltage_task.delay;
}


/*
 * 测量4个温度
*/
static struct means_task _temp_task;
static u32 temp_task_handler(void);
static void init_temp_task(void){
	_temp_task._task.handler = temp_task_handler;
	_temp_task.delay = 500;
	_temp_task.index = 0;
	for (int i = 0; i < PACK_TEMPS_NUM; i++){
		_measure_value.pack_temp[i] = get_pack_temperature(i);
	}
	shark_task_add(&_temp_task._task);
}
static u32 temp_task_handler(void){
	_measure_value.pack_temp[_temp_task.index] = get_pack_temperature(_temp_task.index);
	_temp_task.index = (_temp_task.index + 1) % (PACK_TEMPS_NUM);
	_temperature_notify();//通知bms state 有新的温度数据
	return _temp_task.delay;
}