#include "usr_config.h"
#include "hal_battery.h"
#include "nrf_drv_saadc.h"
#include "bsp_time.h"
#include "system.h"
#include "hal_led.h"
#include "hal_ser_imu_mode_manage.h"
#include "app_overturn.h"
#include "hal_wearshoes.h"


#include "ble_comm.h"

#define SLIDE_WINDOW_LEN 5
				

typedef struct
{
	int16_t max_temp;
	uint16_t time_count;
}ACC_TIME_TYPE;

typedef struct
{
	ACC_TIME_TYPE slide_window[SLIDE_WINDOW_LEN];
	
	int window_length;
	
}SLIDE_WINDOW;

static uint8_t flag_move =0;

void sort_silde_window(SLIDE_WINDOW *max_slide_window, SLIDE_WINDOW *min_slide_window, int16_t acc_val, uint16_t	time_count)
{
		ACC_TIME_TYPE acc_time_type_temp = {acc_val, time_count};
//	  DEBUG_LOG("max_slide_window->slide_window[0]:0%2x,0%2x\n",max_slide_window->slide_window[0],&max_slide_window->slide_window[0]);
		
	//检测当前的值是是否是最大值
	if(max_slide_window->window_length == 0)
	{
		max_slide_window->slide_window[0] = acc_time_type_temp;
		
		max_slide_window->window_length = 1;
	}
	else
	{
		//先判断长度是否溢出来了
		DEBUG_LOG("sort_silde_window>>>>>>>>:%d,%d\n",time_count,max_slide_window->slide_window[0].time_count);
		while(time_count - max_slide_window->slide_window[0].time_count >= SLIDE_WINDOW_LEN)
		{
			memcpy(max_slide_window->slide_window, max_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE));
			max_slide_window->window_length --;
			DEBUG_LOG("111111 window_length:%d\n",max_slide_window->window_length);
		}
		
		DEBUG_LOG("22222 window_length:%d,time_count:%d\n",max_slide_window->window_length,time_count);
//		//再检测新的元素需要前面的元素小
//		while(max_slide_window->window_length > 0)
//		{
//			DEBUG_LOG("33333 window_length:%d\n",max_slide_window->window_length);
//			if(max_slide_window->slide_window[max_slide_window->window_length - 1].max_temp < acc_val)
//			{
//				DEBUG_LOG("44444 window_length:%d\n",max_slide_window->window_length);
//				max_slide_window->window_length --;
//			}
//			else
//			{
//				break;
//			}
//		}
		DEBUG_LOG("5555 window_length:%d\n",max_slide_window->window_length);
		max_slide_window->slide_window[max_slide_window->window_length] = acc_time_type_temp;
		
		max_slide_window->window_length ++;
		
		DEBUG_LOG("<<<<<<<<<<<sort_silde_window end :%d\n",max_slide_window->window_length);
	}
	
	//检测当前的值是是否是最小值
	if(min_slide_window->window_length == 0)
	{
		min_slide_window->slide_window[0] = acc_time_type_temp;
		min_slide_window->window_length = 1;
	}
	else
	{
		//先判断长度是否溢出来了
		while(time_count - min_slide_window->slide_window[0].time_count >= SLIDE_WINDOW_LEN)
		{
			memcpy(min_slide_window->slide_window, min_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE));
			min_slide_window->window_length --;
		}
		
		//再检测新的元素需要前面的元素小
		while(min_slide_window->window_length > 0)
		{
			if(min_slide_window->slide_window[min_slide_window->window_length - 1].max_temp > acc_val)
			{
				min_slide_window->window_length --;
			}
			else
			{
				break;
			}
		}
		
		min_slide_window->slide_window[min_slide_window->window_length] = acc_time_type_temp;
		
		min_slide_window->window_length ++;
	}
}

/************************ 踮脚显示电量 ***********************************/

//没穿鞋的情况下

void app_BatDispaly_Process_N(void)        
{
		ser_imu_data_t data= {0};
	  static uint8_t lED_LIFE_SIGNAL = 0;
    static uint8_t lED_LIFE_TIME = 0;
		
//	DEBUG_LOG("app_BatDispaly_Process_N;\n");
		
		if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY))
		{
			return;//停止状态
		}

		//获取最新一组前脚加速度
		int16_t data_size = 0;
		
		data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		
	  if(data_size > 0){
			
				hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size -1 , &data);
				
				int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]);
				
				if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536)
				{
					lED_LIFE_SIGNAL = 3;
				}
				else if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --;
				
				if(abs(acc_norm - 4194304) < 362144 && data.acc[2] < -1024)
				{ 
					lED_LIFE_SIGNAL = 0;
				}
				
				DEBUG_LOG("000000 data.acc[2]:%d\n",data.acc[2]);
				
//				DEBUG_LOG("acc_norm:%d,lED_LIFE_SIGNAL:%d,data.acc2:%d,lED_LIFE_TIME:%d\n",abs(acc_norm - 4194304),lED_LIFE_SIGNAL,data.acc[2],lED_LIFE_TIME);
				if(lED_LIFE_SIGNAL > 0)
				{
//					DEBUG_LOG("unwearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n");
					  if(lED_LIFE_TIME == 0){
								lED_LIFE_TIME = (10000/StandByPower_Interval);
								if(GetBatteryPersent()>20){
									LED_Start(LED_OVERTURN,COLOR_GREEN);
								}else{
									LED_Start(LED_OVERTURN,COLOR_ORANGE);
								}
								Process_SetHoldOn(app_BatDispaly_Process_N,1);
						 }
						 else if (2 == lED_LIFE_TIME){
							  LED_Stop(LED_OVERTURN);
						    Process_SetHoldOn(app_BatDispaly_Process_N,0);
						 }
						 if (lED_LIFE_TIME > 1)lED_LIFE_TIME--;
				}
				else
				{
//					DEBUG_LOG("unwearshoes LED_Stop(LED_OVERTURN);\n");
					if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0;
					   LED_Stop(LED_OVERTURN);
					   Process_SetHoldOn(app_BatDispaly_Process_N,0);
					}
					if(1 == flag_move){flag_move =0;
					 LED_Stop(LED_OVERTURN);
					 Process_SetHoldOn(app_BatDispaly_Process_N,0);
				  }
				}
	 }
}

//穿鞋的情况下
void app_BatDispaly_Process(void)
{
	  ser_imu_data_t data={0};
		
		static uint8_t	time_count = 0;
		
		static uint16_t station_count =0;
		static uint16_t overturn_180_count = 0;
		static uint8_t lED_LIFE_SIGNAL = 0;
    static uint8_t lED_LIFE_TIME = 0;
		
		static SLIDE_WINDOW max_slide_window = { {-3096,0},0 };
		static SLIDE_WINDOW min_slide_window = { {3096,0},0 };
		
	
		if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL))
		{
			return;//走路状态
		}			
//		DEBUG_LOG("app_BatDispaly_Process;\n");
		//获取最新一组前脚加速度, 有fifo
		int16_t data_size = 0;
		
		data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT);
		
	  if(data_size > 0){
			
//					DEBUG_LOG("data_size : %d,  hal_wearshoes_is_wearshoes: %d\n", data_size,hal_wearshoes_is_wearshoes());
		     
					hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size-1 , &data);
				  
					//耗时较少的计算滑动窗口的最大值、最小值
					sort_silde_window(&max_slide_window, &min_slide_window,  data.acc[0], time_count);
					
					if(time_count > SLIDE_WINDOW_LEN)
					{
						if(data.acc[0] - min_slide_window.slide_window[0].max_temp < 256 &&  max_slide_window.slide_window[0].max_temp -  data.acc[0] < 256
							&& max_slide_window.slide_window[0].max_temp  - min_slide_window.slide_window[0].max_temp < 256 &&  data.acc[0] < -1500)
						{
							
							station_count++;
						}
						else station_count =0;
					}
					
					time_count++;

//					DEBUG_LOG("111111 %d:%d:%d:%d\n",(data.acc[0] - min_slide_window.slide_window[0].max_temp),
//							                                 (max_slide_window.slide_window[0].max_temp -  data.acc[0]),
//							                                 (max_slide_window.slide_window[0].max_temp  - min_slide_window.slide_window[0].max_temp),data.acc[0]);
					//在穿鞋状态下，被翻转了
//					DEBUG_LOG("111111 data.acc[2]:%d\n",data.acc[2]);
					int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]);
					if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536)
					{
						//DEBUG_LOG(" wear shoes if(abs(acc_norm - 4194304) < 262144 && data.acc[2] > 1536);\n");
						overturn_180_count ++;
					}
					else
					{
						overturn_180_count = 0;
					}
					
					if(overturn_180_count >= 5 || station_count > 4)
					{
//						 DEBUG_LOG(" overturn_180_count > 5;\n");
						lED_LIFE_SIGNAL = 6;
					}
					
					if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --;
					
					if(lED_LIFE_SIGNAL > 0)
					{
//						DEBUG_LOG("wearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n");
						 if(lED_LIFE_TIME == 0){
								lED_LIFE_TIME = (10000/LowPower_Interval);
								if(GetBatteryPersent()>20){
									LED_Start(LED_OVERTURN,COLOR_GREEN);
								}else{
									LED_Start(LED_OVERTURN,COLOR_ORANGE);
								}
								Process_SetHoldOn(app_BatDispaly_Process,1);
						 }
						 else if (2 == lED_LIFE_TIME){
							  LED_Stop(LED_OVERTURN);
						    Process_SetHoldOn(app_BatDispaly_Process,0);
						 }
						 if (lED_LIFE_TIME > 1)lED_LIFE_TIME--;
					}
					else
					{
//						DEBUG_LOG("wearshoes LED_Stop(LED_OVERTURN);\n");
						if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0;
					    LED_Stop(LED_OVERTURN);
					    Process_SetHoldOn(app_BatDispaly_Process,0);
					  }
						if(0 == flag_move){flag_move =1;
							 LED_Stop(LED_OVERTURN);
							 Process_SetHoldOn(app_BatDispaly_Process_N,0);
				    }
					}
		}
		
}


void app_overturn_Init(void)
{
	Process_Start(LowPower_Interval,"BatDispaly",app_BatDispaly_Process);
	Process_Start(StandByPower_Interval,"BatDispaly_N",app_BatDispaly_Process_N);
}