#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;

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};
	
	//检测当前的值是是否是最大值
	if(max_slide_window->window_length == 0)
	{
		max_slide_window->slide_window[0] = acc_time_type_temp;
		
		max_slide_window->window_length = 1;
	}
	else
	{
		//先判断长度是否溢出来了
		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 --;
		}
		
		//再检测新的元素需要前面的元素小
		while(max_slide_window->window_length > 0)
		{
			if(max_slide_window->slide_window[max_slide_window->window_length - 1].max_temp < acc_val)
			{
				max_slide_window->window_length --;
			}
			else
			{
				break;
			}
		}
		
		max_slide_window->slide_window[max_slide_window->window_length] = acc_time_type_temp;
		
		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;
	
		static uint8_t	led_light_count  = -1;
		
		static uint8_t station_count =0;
	
		static int LED_LIVE_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) < 262144  && data.acc[2] > 1536)
//					station_count =1;
//				else 
//					station_count =0;
//				
//				if(abs(acc_norm - 4194304) < 262144 && data.acc[2] < -1024)
//					station_count =0;
			
			  if(data.acc[2] > 1200)
					station_count =1;
				else 
					station_count =0;
				
          
				DEBUG_LOG("acc_norm:%d,station_count:%d,data.acc2:%d\n",abs(acc_norm - 4194304),station_count,data.acc[2]);
				if(station_count > 0){
						if(0 == LED_LIVE_TIME){
							LED_LIVE_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_LIVE_TIME){
							LED_Stop(LED_OVERTURN);
						  Process_SetHoldOn(app_BatDispaly_Process_N,0);
						}
						if (LED_LIVE_TIME > 1)LED_LIVE_TIME--;
				}
				else if (LED_LIVE_TIME > 0 && 0 == station_count){LED_LIVE_TIME=0;
						LED_Stop(LED_OVERTURN);
						Process_SetHoldOn(app_BatDispaly_Process_N,0);
				}
	 }
}

//穿鞋的情况下
void app_BatDispaly_Process(void)
{
	  ser_imu_data_t data={0};
		
		static uint16_t	time_count = 0;
		
		static uint16_t station_count= 0;
		
		static SLIDE_WINDOW max_slide_window = { {-3096,0},0 };
		static SLIDE_WINDOW min_slide_window = { {3096,0},0 };
		
		static uint16_t overturn_180_count = 0;
		
		static uint8_t lED_LIFE_TIME = 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] < -308){
							station_count =6;
						}
						else if(station_count > 0)station_count--;
					}
					time_count++;
					
					//在穿鞋状态下，被翻转了
				  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)
						overturn_180_count ++;
					else 
						overturn_180_count = 0;
		
					DEBUG_LOG("acc_norm:%d,station_count:%d,overturn_180_count:%d\n",abs(acc_norm - 4194304),station_count,overturn_180_count);
          if(station_count > 0 || overturn_180_count >= 5){
						if(0 == lED_LIFE_TIME){
							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 if (lED_LIFE_TIME > 0 && (0 == station_count && 0 == overturn_180_count)){lED_LIFE_TIME=0;
						LED_Stop(LED_OVERTURN);
						Process_SetHoldOn(app_BatDispaly_Process,0);
					}
		}
		
}


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