/*Includes ------------------------------------------------------*/
#include "exception.h"
#include "system.h"
#include "hal_monitor.h"
#include "hal_ser_imu_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		charge_vol_max;				//充电期间最大的电压值
	
	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;		//上一次的后脚地磁计值错误累计
	
	int16_t		suspend_mode_err_sum;	//异常挂起模式持续错误累计
	
	uint32_t	last_tim;							//上一次的时间

} Hal_Monitor_t;

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

/*Local Functions ----------------------------------------------*/
static void monitor_sensor_data(int16_t *f_acc, int16_t *f_gry, int16_t *f_mag, int16_t *b_mag)
{
	/*前脚加速度*/
	if(f_acc != NULL)
	{
		if(
				ob_monitor.last_f_acc[0] == f_acc[0] && \
				ob_monitor.last_f_acc[1] == f_acc[1] && \
				ob_monitor.last_f_acc[2] == f_acc[2]
			)
		{
			ob_monitor.last_f_acc_err_sum++;
			
			if(ob_monitor.last_f_acc_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_ACC);
			}
			
		}else{
			ob_monitor.last_f_acc_err_sum = 0;
		}
		
		ob_monitor.last_f_acc[0] = f_acc[0];
		ob_monitor.last_f_acc[1] = f_acc[1];
		ob_monitor.last_f_acc[2] = f_acc[2];
	}
	/*前脚陀螺仪*/
	if(f_gry != NULL)
	{
		if(
				ob_monitor.last_f_gry[0] == f_gry[0] && \
				ob_monitor.last_f_gry[1] == f_gry[1] && \
				ob_monitor.last_f_gry[2] == f_gry[2]
			)
		{
			ob_monitor.last_f_gry_err_sum++;
			
			if(ob_monitor.last_f_gry_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_GRY);
			}
				
		}else{
			ob_monitor.last_f_gry_err_sum = 0;
		}
		
		ob_monitor.last_f_gry[0] = f_gry[0];
		ob_monitor.last_f_gry[1] = f_gry[1];
		ob_monitor.last_f_gry[2] = f_gry[2];
	}
	/*前脚地磁计*/
	if(f_mag != NULL)
	{
		if(
				ob_monitor.last_f_mag[0] == f_mag[0] && \
				ob_monitor.last_f_mag[1] == f_mag[1] && \
				ob_monitor.last_f_mag[2] == f_mag[2]
			)
		{
			ob_monitor.last_f_mag_err_sum++;
			
			if(ob_monitor.last_f_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_FRONT_MAG);
			}
				
		}else{
			ob_monitor.last_f_mag_err_sum = 0;
		}
		
		ob_monitor.last_f_mag[0] = f_mag[0];
		ob_monitor.last_f_mag[1] = f_mag[1];
		ob_monitor.last_f_mag[2] = f_mag[2];
	}
	/*后脚地磁计*/
	if(b_mag != NULL)
	{
		if(
				ob_monitor.last_b_mag[0] == b_mag[0] && \
				ob_monitor.last_b_mag[1] == b_mag[1] && \
				ob_monitor.last_b_mag[2] == b_mag[2]
			)
		{
			ob_monitor.last_b_mag_err_sum++;
			
			if(ob_monitor.last_b_mag_err_sum >= MONITOR_DATA_ERR_SUM_MAX){
				Except_SetExceptype(EXCEPT_DATA_BACK_MAG);
			}
				
		}else{
			ob_monitor.last_b_mag_err_sum = 0;
		}
		
		ob_monitor.last_b_mag[0] = b_mag[0];
		ob_monitor.last_b_mag[1] = b_mag[1];
		ob_monitor.last_b_mag[2] = b_mag[2];
	}
}
static void monitor_battery_charge_data(void)
{
	int16_t					bat_vol;
	int16_t					charge_vol;
	int16_t					charge_threshold;
	uint32_t 				ch = nrf_gpio_pin_read(PIN_CHARGING);
	static uint32_t charge_cycle = 50;
	
	if(!ch)//没充电
	{		
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		ob_monitor.before_charge_vol = ob_monitor.before_charge_vol > charge_vol ? charge_vol : ob_monitor.before_charge_vol;
		ob_monitor.charge_vol_max = 0;
		charge_cycle = 50;
	}else	//充电
	{			
		/* 过筛50轮 */
		if(charge_cycle != 0){
			charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
			ob_monitor.charge_vol_max = ob_monitor.charge_vol_max < charge_vol ? charge_vol : ob_monitor.charge_vol_max;
			charge_cycle--;
			return;
		}
		charge_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		ob_monitor.charge_vol_max = ob_monitor.charge_vol_max < charge_vol ? charge_vol : ob_monitor.charge_vol_max;		
		bat_vol    = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_BAT_CHANNEL))*5/3;
		
		//当电池电量没满，充电（经测试，电量没满的充电电压跟电池电压有关，最小充电电压100+mv）
		charge_threshold = 	ob_monitor.charge_vol_max - ob_monitor.before_charge_vol;	//充电前和充电期间的充电电压变化值
		if(charge_threshold < CHARGE_VOL_THRESHOLD)
		{		
			Except_SetExceptype(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_SetExceptype(EXCEPT_DATA_BATTERY);
		}
	}
}
static void monitor_suspend_mode_overflow(void)
{
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_SELF_CHECK) == -1 && \
		 hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) == -1 && \
		 hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) == -1 && \
		 hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) == -1 && \
		 hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) == -1)
	{
		ob_monitor.suspend_mode_err_sum++;
		if(ob_monitor.suspend_mode_err_sum >= MONITOR_SUSPEND_MODE_OVERFLOW_ERR_SUM_MAX){
			Except_SetExceptype(EXCEPT_MODE_SUSPEND_OVERFLOW);			
		}
	}
	else
	{
		ob_monitor.suspend_mode_err_sum = 0;
	}
}

static void hal_monitor_process(void)
{
	int 							i = 0;
	int16_t						group_num = 0;
	static int16_t 		f_acc[3];
	static int16_t		f_gry[3];
	static int16_t		f_mag[3];
	static int16_t		b_mag[3];
	ser_imu_data_t		data;
	
	if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_SELF_CHECK) != -1 || \
		 hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_GAME) != -1 || \
			hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_REALSTEP) != -1)
	{
		if(TIME_GetTicks()-ob_monitor.last_tim>=FullPower_Interval)				//监测前脚传感器数据（acc + gry + mag）+ 监测后脚传感器数据（mag）	
		{
			ob_monitor.last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
			
			for(i=0;i<group_num;i++)
			{
				hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
				f_gry[0] = data.gry[0];f_gry[1] = data.gry[1];f_gry[2] = data.gry[2];
				f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
				f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
				
				monitor_sensor_data(f_acc, f_gry, f_mag, NULL);
			}
			if(hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_BACK) >= 1)hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_BACK, 0, &data);
			b_mag[0] = data.mag[0];b_mag[1] = data.mag[1];b_mag[2] = data.mag[2];
			
			monitor_sensor_data(NULL, NULL, NULL, b_mag);
		}
	}
	else if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL) != -1)
	{
		if(TIME_GetTicks()-ob_monitor.last_tim>=LowPower_Interval)				//监测前脚传感器数据（acc + mag）
		{
			ob_monitor.last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
			
			for(i=0;i<group_num;i++)
			{
				hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
				f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
				f_mag[0] = data.mag[0];f_mag[1] = data.mag[1];f_mag[2] = data.mag[2];
				
				monitor_sensor_data(f_acc, NULL, f_mag, NULL);		
			}
		}
	}
	else if(hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY) != -1)
	{
		if(TIME_GetTicks()-ob_monitor.last_tim>=StandByPower_Interval)		//监测前脚传感器数据（acc）
		{
			ob_monitor.last_tim = TIME_GetTicks();
			
			group_num = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
			
			for(i=0;i<group_num;i++)
			{
				hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, i, &data);
				f_acc[0] = data.acc[0];f_acc[1] = data.acc[1];f_acc[2] = data.acc[2];
				
				monitor_sensor_data(f_acc, NULL, NULL, NULL);		
			}
		}
	}

	//监测电池和充电数据
	monitor_battery_charge_data();
	
	//监测异常挂起模式是否持续时间超过临界点
	monitor_suspend_mode_overflow();

}

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