#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 "app_overturn.h"
#include "hal_wearshoes.h"
#include "bll_imu.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;

int cal_unsigned_D_value(uint16_t left_value, uint16_t right_value)
{
	if(left_value <= right_value)
	{
		return right_value - left_value;
	}
	else
	{
		return 65535-left_value + 1 + right_value;
	}
}

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};
	
	//检测当前的值是是否是最大值
	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(cal_unsigned_D_value(max_slide_window->slide_window[0].time_count, 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(cal_unsigned_D_value(min_slide_window->slide_window[0].time_count, 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)        
{
		bll_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(hal_wearshoes_is_wearshoes())return;
			
		//获取最新一组前脚加速度
		int16_t data_size = 0;
		
		data_size = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
		
		if(data_size > 0){
			
				bll_imu_get_data(BLL_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("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)
{
	  bll_imu_data_t data={0};
		
		static uint16_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(!hal_wearshoes_is_wearshoes())return;
		
//		DEBUG_LOG("app_BatDispaly_Process;\n");
		//获取最新一组前脚加速度, 有fifo
		int16_t data_size = 0;
		
		data_size = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
		
		if(data_size > 0){
			
//					DEBUG_LOG("data_size : %d,  hal_wearshoes_is_wearshoes: %d\n", data_size,hal_wearshoes_is_wearshoes());
		
					bll_imu_get_data(BLL_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] < -1400)
						{
							station_count++;
						}
						else station_count =0;
					}
					
					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)
					{
						//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);
}