#include "bsp/gpio.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_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_voltage_task();
	init_temp_task();
	set_log_level(MOD_MEASURE, L_debug);
}

measure_value_t * measure_value(void){
	return &_measure_value;
}

void measure_log(void){
	measure_debug("Current %.4f -- %.4f\n", (float)_measure_value.load_current/1000.0f, (float)_measure_value.current_5238/1000.0f);
	for (int i = 0; i < CELLS_NUM; i++){
		//measure_debug("Cell[%d]: %.3fv\n", i, _measure_value.cell_vol[i]/1000.0f);
	}
	measure_debug("Gain:%f, Off %f\n", get_ml5238_gain(), get_ml5238_vos());
}
/*
 * 测量电芯电压，计算出总电压, 测量总电流，放电和充电
*/
static struct means_task _current_voltage_task;
static u32 current_voltage_task_handler(void);
static void init_current_voltage_task(void){
	_current_voltage_task._task.handler = current_voltage_task_handler;
	_current_voltage_task.delay = 15;
	_current_voltage_task.index = CELLS_NUM;
	_measure_value.load_current = get_pack_current(&_measure_value.current_5238);
	for (int i = 0; i < CELLS_NUM; i++){
		_measure_value.cell_vol[i] = get_cell_voltage(i);
	}
	shark_task_add(&_current_voltage_task._task);
}

static u32 current_voltage_task_handler(void){
	/* 测量电流 */
	_measure_value.load_current = get_pack_current(&_measure_value.current_5238);
	_current_notify();//通知bms state 有新的电流数据

	if (bms_state_get()->pack_balancing){ //if balance, do'nt sample cell voltage
		return _current_voltage_task.delay;
	}	
	//用内阻对cell的电压进行补偿，充电减，放电加
	_current_voltage_task.index = (_current_voltage_task.index + 1) % CELLS_NUM;
	_measure_value.cell_vol[_current_voltage_task.index] = get_cell_voltage(_current_voltage_task.index) - bms_health()->internal_resistance[_current_voltage_task.index] * _measure_value.load_current/1000;
	_voltage_notify();//通知bms state 有新的电压数据
	return _current_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;
}