/*Includes ------------------------------------------------------*/
#include "exception.h"
#include "system.h"
#include "hal_monitor.h"
#include "hal_imu.h"
#include "hal_mode_manage.h"
#include "hal_battery.h"
#include "bsp_time.h"

/*Private macro ------------------------------------------------*/
#define MONITOR_DATA_ERR_SUM_MAX			5				//数据监测错误累计最大值
#define BATTERY_VOL_THRESHOLD_MIN			2500		//2.5V
#define BATTERY_VOL_THRESHOLD_MAX			4000		//4V
#define CHARGE_VOL_THRESHOLD					80			//充电前和充电期间的充电电压变化值,单位mv

/*STRUCTION -----------------------------------------------------*/
typedef struct hal_monitor
{
	/*private member*/
	int16_t 	last_f_acc[3];				//上一次的前脚加速度值
	int16_t 	last_f_gry[3];				//上一次的前脚陀螺仪值
	int16_t 	last_f_mag[3];				//上一次的前脚地磁计值
	int16_t 	last_b_mag[3];				//上一次的后脚地磁计值
	
	int16_t		before_charge_vol;		//充电前的电压值
	
	int16_t		last_f_acc_err_sum;		//上一次的前脚加速度值错误累计
	int16_t 	last_f_gry_err_sum;		//上一次的前脚陀螺仪值错误累计
	int16_t 	last_f_mag_err_sum;		//上一次的前脚地磁计值错误累计
	int16_t 	last_b_mag_err_sum;		//上一次的后脚地磁计值错误累计
	
	uint32_t	last_tim;							//上一次的时间

} Hal_Monitor_t;

/*Local Variable ----------------------------------------------*/
static Hal_Monitor_t ob_monitor;

/*Local Functions ----------------------------------------------*/
static void monitor_sensor_data(Hal_Monitor_t	*p_monitor, int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag)
{
	if(p_monitor == NULL)return;
	
	/*前脚加速度*/
	if(f_acc != NULL)
	{
		if(
				p_monitor->last_f_acc[0] == f_acc[0] && \
				p_monitor->last_f_acc[1] == f_acc[1] && \
				p_monitor->last_f_acc[2] == f_acc[2]
			)
		{
			p_monitor->last_f_acc_err_sum++;
			
			if(p_monitor->last_f_acc_err_sum >= MONITOR_DATA_ERR_SUM_MAX)
				Except_SaveExceptype(EXCEPT_DATA_FRONT_ACC);
		}else{
			p_monitor->last_f_acc_err_sum = 0;
		}
		
		p_monitor->last_f_acc[0] = f_acc[0];
		p_monitor->last_f_acc[1] = f_acc[1];
		p_monitor->last_f_acc[2] = f_acc[2];
	}
	/*前脚陀螺仪*/
	if(f_gry != NULL)
	{
		if(
				p_monitor->last_f_gry[0] == f_gry[0] && \
				p_monitor->last_f_gry[1] == f_gry[1] && \
				p_monitor->last_f_gry[2] == f_gry[2]
			)
		{
			p_monitor->last_f_gry_err_sum++;
			
			if(p_monitor->last_f_gry_err_sum >= MONITOR_DATA_ERR_SUM_MAX)
				Except_SaveExceptype(EXCEPT_DATA_FRONT_GRY);
		}else{
			p_monitor->last_f_gry_err_sum = 0;
		}
		
		p_monitor->last_f_gry[0] = f_gry[0];
		p_monitor->last_f_gry[1] = f_gry[1];
		p_monitor->last_f_gry[2] = f_gry[2];
	}
	/*前脚地磁计*/
	if(f_mag != NULL)
	{
		if(
				p_monitor->last_f_mag[0] == f_mag[0] && \
				p_monitor->last_f_mag[1] == f_mag[1] && \
				p_monitor->last_f_mag[2] == f_mag[2]
			)
		{
			p_monitor->last_f_mag_err_sum++;
			
			if(p_monitor->last_f_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX)
				Except_SaveExceptype(EXCEPT_DATA_FRONT_MAG);
		}else{
			p_monitor->last_f_mag_err_sum = 0;
		}
		
		p_monitor->last_f_mag[0] = f_mag[0];
		p_monitor->last_f_mag[1] = f_mag[1];
		p_monitor->last_f_mag[2] = f_mag[2];
	}
	/*后脚地磁计*/
	if(b_mag != NULL)
	{
		if(
				p_monitor->last_b_mag[0] == b_mag[0] && \
				p_monitor->last_b_mag[1] == b_mag[1] && \
				p_monitor->last_b_mag[2] == b_mag[2]
			)
		{
			p_monitor->last_b_mag_err_sum++;
			
			if(p_monitor->last_b_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX)
				Except_SaveExceptype(EXCEPT_DATA_BACK_MAG);
		}else{
			p_monitor->last_b_mag_err_sum = 0;
		}
		
		p_monitor->last_b_mag[0] = b_mag[0];
		p_monitor->last_b_mag[1] = b_mag[1];
		p_monitor->last_b_mag[2] = b_mag[2];
	}	
}
static void monitor_battery_charge_data(Hal_Monitor_t	*p_monitor)
{
	int16_t		charge_vol;
	int16_t		bat_vol;
	int16_t		charge_threshold;
	uint32_t 	ch = nrf_gpio_pin_read(PIN_CHARGING);
	
	if(!ch){		//没充电
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		p_monitor->before_charge_vol = p_monitor->before_charge_vol > charge_vol ? charge_vol : p_monitor->before_charge_vol;
	}else{			//充电
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		bat_vol    = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_BAT_CHANNEL))*5/3;
		
		//当电池电量满了，不充电（经测试，哪怕电量满了，充电电压也有10+mv）
		//当电池电量没满，充电（经测试，电量没满的充电电压跟电池电压有关，最小充电电压100+mv）
		charge_threshold = 	charge_vol - p_monitor->before_charge_vol;	//充电前和充电期间的充电电压变化值
		if(charge_threshold < CHARGE_VOL_THRESHOLD)
		{
			Except_SaveExceptype(EXCEPT_DATA_CHARGE);
		}
		//电池小于2.5V或充电电压变化小于阈值且电池电压大于4V
		if(bat_vol <= BATTERY_VOL_THRESHOLD_MIN || (charge_threshold < CHARGE_VOL_THRESHOLD && bat_vol > BATTERY_VOL_THRESHOLD_MAX))
		{
			Except_SaveExceptype(EXCEPT_DATA_BATTERY);
		}
	}
}
static void hal_monitor_process(void)
{
	int 			i = 0;
	int16_t		group_num = 0;
	int16_t 	f_acc[3];
	int16_t		f_gry[3];
	int16_t		f_mag[3];
	int16_t		b_mag[3];

	//监测传感器数据,配置期间不处理
	if(IMU_IsNoSignal())
	{
		switch(hal_mode_get())
		{
			case HAL_MODE_SELF_CHECK:
			case HAL_MODE_GAME:
			case HAL_MODE_REALSTEP:
				if(TIME_GetTicks()-ob_monitor.last_tim>=FullPower_Interval)				//监测前脚传感器数据（acc + gry + mag）+ 监测后脚传感器数据（mag）	
				{
					ob_monitor.last_tim = TIME_GetTicks();
					
					group_num = IMU_Get_Front_Data_Num();
					for(i=0;i<group_num;i++)
					{
						IMU_Get_Front_Data(i, f_gry, f_acc, f_mag, NULL);
						monitor_sensor_data(&ob_monitor, f_acc, f_gry, f_mag, NULL);
					}
					IMU_Get_Back_Data(b_mag);
					monitor_sensor_data(&ob_monitor, NULL, NULL, NULL, b_mag);
				}
				break;
			
			case HAL_MODE_STANDBY:
				if(TIME_GetTicks()-ob_monitor.last_tim>=StandByPower_Interval)		//监测前脚传感器数据（acc）
				{
					ob_monitor.last_tim = TIME_GetTicks();
					
					group_num = IMU_Get_Front_Data_Num();
					for(i=0;i<group_num;i++)
					{
						IMU_Get_Front_Data(i, NULL, f_acc, NULL, NULL);
						monitor_sensor_data(&ob_monitor, f_acc, NULL, NULL, NULL);		
					}
				}
				break;
			
			case HAL_MODE_NORMAL:
				if(TIME_GetTicks()-ob_monitor.last_tim>=LowPower_Interval)				//监测前脚传感器数据（acc + mag）
				{
					ob_monitor.last_tim = TIME_GetTicks();
					
					group_num = IMU_Get_Front_Data_Num();
					for(i=0;i<group_num;i++)
					{
						IMU_Get_Front_Data(i, NULL, f_acc, f_mag, NULL);
						monitor_sensor_data(&ob_monitor, f_acc, NULL, f_mag, NULL);		
					}
				}
				break;
			
			default:
				break;
		}		
	}

	//监测电池和充电数据
	monitor_battery_charge_data(&ob_monitor);

}

/*API ---------------------------------------------------------*/
void hal_monitor_Init(void)
{
	ob_monitor.before_charge_vol = 5;
	
	Process_Start(0,"hal_monitor_process",hal_monitor_process);
}