#include "app_self_checking.h"
#include "usr_config.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_ble_client.h"
#include "hal_ble_host.h"
#include "hal_mt.h"
#include "nrf_delay.h"
#include "app_charge.h"
#include "hal_imu.h"
#include "nrf_gpio.h"
#include "hal_battery.h"
#include "hal_ble_uart0.h"
#include "hal_led.h"
#include "ble_comm.h"
#include "tool.h"

#define TEST_SUCCESS													0															//测试成功
#define TEST_FAIL															1															//测试失败

#define TEST_ITEMS														4															//非人工测试项数（前脚传感器、后脚传感器、电量、充电电流）

#define ENTER_SELF_CHECK											1															//进入自检标志位
#define QUITE_SELF_CHECK											0															//退出自检标志位

#define TEST_RESULT_LED_SUCCESS_GREEN					0
#define TEST_RESULT_LED_ERR_RED								(TEST_RESULT_LED_SUCCESS_GREEN+0x01)
#define TEST_RESULT_LED_ERR_BLUE							(TEST_RESULT_LED_SUCCESS_GREEN+0x02)
#define TEST_RESULT_LED_ERR_PURPLE						(TEST_RESULT_LED_SUCCESS_GREEN+0x03)
#define TEST_RESULT_LED_ERR_YELLOW						(TEST_RESULT_LED_SUCCESS_GREEN+0x04)

#define DISPLAY_TEST_RESULT_LED_TIME					1000												//用灯显示测试结果的时间,单位ms

#define DISPLAY_TEST_RESULT_CONTINUE_TIME			3000												//显示测试结果的总时间，单位ms

#define	SELF_CHECK_RECIVE_ORDER_TIMES					1														//允许接受到的自检指令最大次数

#define BATTERY_TEST_VALUE										2500												//电池必须大于2.5V，否则算异常

#define BATTERY_CHARGE_CHANGE_VALUE						80													//自检前和自检期间的充电电压变化值,单位mv

static uint8_t  self_check_state = QUITE_SELF_CHECK;

static uint8_t  self_check_result_buff[TEST_ITEMS];

static uint8_t	self_check_recive_order_times = 0;												//允许接受到的自检指令最大次数

static int16_t	before_check_charge_vol = 0;															//自检前的充电电压

//发送非人工检测结果，只有在插上充电时，才配置串口
static void app_self_check_send_result(uint8_t *buf, uint8_t datalen)
{
	uint32_t txd,rxd;
	UART0_GetPinConfig(&txd, &rxd);
	UART0_Initialize(PIN_TXD_BLE,PIN_RXD_BLE,9600);
	UART0_Tx_Send(0,UART0_T_SELF_CHECK_ACK,buf,datalen);
	UART0_Initialize(txd,rxd,9600);
}

static void app_self_checking_ack_process(void)
{
	app_self_check_send_result(self_check_result_buff, TEST_ITEMS);
//	SEGGER_RTT_printf(0,"app_self_checking_ack_process\n");
}


static void app_self_checking_err_led_process(void)
{
	nrf_gpio_pin_toggle(PIN_LED_RUN);
}

static void app_self_checking_artificial(void)								//人工测试（电机、指示灯）
{
	uint32_t 				temp_result = 0;
	static uint8_t 	state = 0;
	static uint32_t display_times = 0;
	
	switch(state){
		case 0:
			Process_SetHoldOn(app_self_checking_artificial,1);
			//测试电机是否正常
			MT_Run(100);
			//显示其余的非人工测试结果
			for(int i=0;i<TEST_ITEMS;i++)temp_result +=self_check_result_buff[i];
//			SEGGER_RTT_printf(0,"temp_result=%d\n",temp_result);
			if(temp_result == TEST_SUCCESS){																			//如果全部测试通过
				LED_SetColor(LED_SELF_CHECK,COLOR_GREEN);
				LED_Start(LED_SELF_CHECK);
				nrf_gpio_pin_write(PIN_LED_RUN,LED_SMALL_ENABLE);
				Process_UpdatePeroid(app_self_checking_artificial,DISPLAY_TEST_RESULT_LED_TIME);
				state = 1;
				break;
			}else{																																//没有全部通过
				static uint8_t loop = 0;
				if(self_check_result_buff[loop++] != TEST_SUCCESS){
					switch(loop)
					{
						case TEST_RESULT_LED_ERR_RED:
							LED_SetColor(LED_SELF_CHECK,COLOR_RED);
						break;
						case TEST_RESULT_LED_ERR_BLUE:
							LED_SetColor(LED_SELF_CHECK,COLOR_BLUE);
						break;
						case TEST_RESULT_LED_ERR_PURPLE:
							LED_SetColor(LED_SELF_CHECK,COLOR_PURPLE);
						break;
						case TEST_RESULT_LED_ERR_YELLOW:
							LED_SetColor(LED_SELF_CHECK,COLOR_YELLOW);
						break;				
					}
					Process_UpdatePeroid(app_self_checking_artificial,DISPLAY_TEST_RESULT_LED_TIME);
					LED_Start(LED_SELF_CHECK); 
//					//开启外设损坏小灯闪烁线程，100ms翻转一次
//					Process_Start(100,"app_self_checking_err_led_process",app_self_checking_err_led_process);	
				}else{
					Process_UpdatePeroid(app_self_checking_artificial,0);						//通过的某项就跳过等待时间。
				}
				if(loop == TEST_ITEMS){
					loop = 0;
					state = 1;
				}
			}
			break;
		case 1:
			for(int i=0;i<TEST_ITEMS;i++)temp_result +=self_check_result_buff[i];
			temp_result = (temp_result == 0) ? DISPLAY_TEST_RESULT_LED_TIME : (temp_result * DISPLAY_TEST_RESULT_LED_TIME);//跑完一次case0的时间
			if((temp_result * ++display_times) <= DISPLAY_TEST_RESULT_CONTINUE_TIME){
				state = 0;
				break;
			}
			display_times = 0;
			Process_UpdatePeroid(app_self_checking_artificial,0);
			LED_Stop(LED_SELF_CHECK);
			nrf_gpio_pin_write(PIN_LED_RUN,LED_SMALL_DISABLE);
			state = 0;
			Process_SetHoldOn(app_self_checking_artificial,0);
			Process_Stop(app_self_checking_artificial);
			Process_Stop(app_self_checking_err_led_process);
			break;
		default:state=0;Process_UpdatePeroid(app_self_checking_artificial,0);break;
	}
	
}

//测试前脚IMU和地磁是否正常
static void app_checking_front_sensor(uint8_t *check_result)
{
	static int16_t 	acc_front[IMU_BUFF_SIZE][3];
	static int16_t  gry_front[IMU_BUFF_SIZE][3];
	static int16_t	mag6310_front[IMU_BUFF_SIZE][3];
	static int32_t	timestamp_front[IMU_BUFF_SIZE];
	
	int16_t 				group_num = 0;
	int16_t 				front_index = 0;
	
	group_num = IMU_Get_Front_Update_Data_GroupNum();
	group_num = group_num >= IMU_BUFF_SIZE?IMU_BUFF_SIZE:group_num;
	front_index = IMU_Get_Front_Full_Power_Data((int16_t*)gry_front, (int16_t*)acc_front, (int16_t*)mag6310_front, timestamp_front, group_num);
	
	for(int i=0; i < front_index; i++)
	{
		if( (gry_front[i][0] == 0 && gry_front[i][1] == 0 && gry_front[i][2] == 0) || \
				(acc_front[i][0] == 0 && acc_front[i][1] == 0 && acc_front[i][2] == 0) || \
				(mag6310_front[i][0] == 0 && mag6310_front[i][1] == 0 && mag6310_front[i][2] == 0) ||
				(timestamp_front[i] == 0))
		{
			*check_result = TEST_FAIL;
		}
	}
}

//测试后脚地磁是否正常
static void app_checking_back_sensor(uint8_t *check_result)
{
	static int16_t	mag6310_back[3];
	
	IMU_Get_Back_Mag(mag6310_back);
	if(mag6310_back[0] == 0 && mag6310_back[1] == 0 && mag6310_back[2] == 0)*check_result = TEST_FAIL;
}

//测试电量检测是否正常
static void app_checking_bat(uint8_t *check_result)
{
	int16_t cur_vol;
	int16_t	bat;
	
	cur_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
	bat = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_BAT_CHANNEL))*5/3;
	SEGGER_RTT_printf(0,"app_checking_bat cur_vol:%d  before_check_charge_vol:%d  cur_vol-before_check_charge_vol:%d\r\n",cur_vol,before_check_charge_vol,cur_vol-before_check_charge_vol);
	//电池小于2.5V或充电电压变化小于阈值且电池电压大于4V
	if(bat <= BATTERY_TEST_VALUE || ((cur_vol-before_check_charge_vol)<BATTERY_CHARGE_CHANGE_VALUE && bat > 4000))*check_result = TEST_FAIL;
}

//测试充电电流是否正常
static void app_checking_charge(uint8_t *check_result)
{
	int16_t cur_vol;
	
	cur_vol = ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL);
	cur_vol = ADC_RESULT_IN_MILLI_VOLTS(cur_vol);
	SEGGER_RTT_printf(0,"app_checking_charge cur_vol:%d  before_check_charge_vol:%d  cur_vol-before_check_charge_vol:%d\r\n",cur_vol,before_check_charge_vol,cur_vol-before_check_charge_vol);
	//当电池电量满了，不充电（经测试，哪怕电量满了，充电电压也有10+mv）
	//当电池电量没满，充电（经测试，电量没满的充电电压跟电池电压有关，最小充电电压100+mv）	
	if((cur_vol-before_check_charge_vol)<BATTERY_CHARGE_CHANGE_VALUE)*check_result = TEST_FAIL;
}

void app_self_checking_Process(void)
{
	static uint8_t wait_times = IMU_INIT_TIMES + 1;					// 预防IMU初始化失败，重复几次才初始化成功的情况，所以等待时间为初始化失败上限+1
	
	if(self_check_state == ENTER_SELF_CHECK){
		
		if(wait_times-- != 0)return;
		
		wait_times = IMU_INIT_TIMES + 1;
	
		memset(self_check_result_buff,TEST_SUCCESS,TEST_ITEMS);
		
		if(app_charge_Getstate()!=BLE_Client_T_CHARGE_PULLOUT){
			//如果插上充电，测试充电电流是否正常
			app_checking_charge(&self_check_result_buff[3]);
		}
		
		if(IMU_GetCurrentMode() == STATE_SELF_CHECK_MODE){
			//测试前脚IMU和地磁是否正常
			app_checking_front_sensor(&self_check_result_buff[0]);
		}else{
			//前脚传感器损坏
			self_check_result_buff[0] = TEST_FAIL;
		}
		
		//测试后脚地磁是否正常
		app_checking_back_sensor(&self_check_result_buff[1]);
		
		//测试电量检测是否正常
		app_checking_bat(&self_check_result_buff[2]);

		
		//发送非人工检测结果，只有在插上充电时，才配置串口
		if(app_charge_Getstate()!=BLE_Client_T_CHARGE_PULLOUT){
			Process_Start(50,"app_self_checking_ack_process",app_self_checking_ack_process);
		}
		
		//关闭自检模式
		IMU_SetSelfCheckMode(0);	
		self_check_state = QUITE_SELF_CHECK;
		
//		self_check_result_buff[0] = TEST_FAIL;
//		self_check_result_buff[1] = TEST_FAIL;
//		self_check_result_buff[2] = TEST_FAIL;
//		self_check_result_buff[3] = TEST_FAIL;
		
//		self_check_result_buff[0] = TEST_SUCCESS;
//		self_check_result_buff[1] = TEST_SUCCESS;
//		self_check_result_buff[2] = TEST_SUCCESS;
//		self_check_result_buff[3] = TEST_SUCCESS;
		
		
		/*-----------------上面是不需要人工检测的项目，正常亮小灯，下面的只能靠人工来观察是否正常----------------------------*/
		
		//开启需要人工去测试的线程
		Process_Start(0,"app_self_checking_artificial",app_self_checking_artificial);																								

	}
	
	else if(app_charge_Getstate()==BLE_Client_T_CHARGE_PULLOUT){
		//更新可接受的自检指令最大次数
		self_check_recive_order_times = SELF_CHECK_RECIVE_ORDER_TIMES;
		//用于判断充电芯片和电池是否断线
		int16_t	temp_vol = ADC_RESULT_IN_MILLI_VOLTS(ADC_GetValue(PIN_ADC_CHARGMEASURE_CHANNEL));
		before_check_charge_vol = before_check_charge_vol > temp_vol ? temp_vol : before_check_charge_vol;
		//发送自检结果线程停止
		Process_Stop(app_self_checking_ack_process);			
	}

	
	//地磁触发自检
	else if((IMU_GetCurrentMode() == STATE_LOW_POWER_MODE) && app_charge_Getstate()!=BLE_Client_T_CHARGE_PULLOUT){
		int16_t mag[3];
		IMU_Get_Index_Front_Low_Power_Data(NULL, mag, NULL, IMU_Get_Front_Update_Data_GroupNum());
		int32_t front_mag_norm;
		front_mag_norm  = (int32_t)(sqrt((float) (mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2])));
		if(front_mag_norm>=20000){
			IMU_SetSelfCheckMode(1);															//开机，开启自检模式
			self_check_state = ENTER_SELF_CHECK;
		}
	}
	
//	//用于测量充电电压、电池电压、电量百分比
//	int16_t bat;
//	int16_t	vol;
//	if(app_charge_Getstate()!=BLE_Client_T_CHARGE_PULLOUT){
//		bat = ADC_GetValue(PIN_ADC_BAT_CHANNEL);
//		bat = ADC_RESULT_IN_MILLI_VOLTS(bat)*5/3;
//		SEGGER_RTT_printf(0,"charge!!! %d %d %d\r\n",before_check_charge_vol,bat,GetBatteryPersent());
//	}else{
//		bat = ADC_GetValue(PIN_ADC_BAT_CHANNEL);
//		bat = ADC_RESULT_IN_MILLI_VOLTS(bat)*5/3;
//		SEGGER_RTT_printf(0,"no charge!!! %d %d %d\r\n",before_check_charge_vol,bat,GetBatteryPersent());
//	}

}

void app_self_checking_ready_Process(void)
{
	char advname_buf[100];
	int adv_len;
	
	if(app_charge_Getstate()!=BLE_Client_T_CHARGE_PULLOUT && self_check_recive_order_times){
		slave_get_advname_len(&adv_len);
		slave_get_advname(advname_buf, adv_len);
		uint32_t txd,rxd;
		UART0_GetPinConfig(&txd, &rxd);
		UART0_Initialize(PIN_TXD_BLE,UART0_INVALID_PIN,9600);
		UART0_Tx_Send(0,UART0_T_SELF_CHECK_RDY,(uint8_t*)advname_buf,adv_len);
//		SEGGER_RTT_printf(0,"advname_buf:%s  len:%d\n",advname_buf,adv_len);
		UART0_Initialize(txd,rxd,9600);
	}
}

void cb_UART0_R_SELF_CHECK_ASK(void* handle)
{
	if(self_check_recive_order_times){
		IMU_SetSelfCheckMode(1);															//串口接收自检指令，开启自检模式
		self_check_state = ENTER_SELF_CHECK;		
		self_check_recive_order_times--;
	}
}

void cb_UART0_R_SELF_CHECK_END(void* handle)
{
	Process_Stop(app_self_checking_ack_process);			//发送自检结果线程停止
}

void app_self_checking_Init(void)
{
	UART0_Rx_Regist(UART0_R_SELF_CHECK_ASK,cb_UART0_R_SELF_CHECK_ASK);
	UART0_Rx_Regist(UART0_R_SELF_CHECK_END,cb_UART0_R_SELF_CHECK_END);
	Process_Start(10,"app_self_checking_Process",app_self_checking_Process);
	Process_Start(50,"app_self_checking_ready_Process",app_self_checking_ready_Process);
}