/****************************************************************************
 *
 * Copy right:   Qx.Chen jie
 * File name:    AppTaskGps.c
 * Description:  Gps处理任务和三轴数据处理及获取
 * History:      2021-03-07 2021-07-15
 * Version:      V3.0
 ****************************************************************************/
#include "AppTaskGps.h"
static QueueHandle_t norGpsHandle = NULL;
static osThreadId_t gpsTaskHandle = NULL;
static StaticTask_t gpsTask = NULL;
static UINT8 gpsTaskStack[GPS_TASK_STACK_SIZE];
extern void GsensorInit(void);
extern void GsensorI2CHandler(ARM_I2C_SignalEvent_t cb_event);
extern void GsensorI2CCallback(UINT32 event);

static process_gps gProcess_Gps_Task = PROCESS_GPS_STATE_IDLE;
#define PROC_GPS_STATE_SWITCH(a) (gProcess_Gps_Task = a)
UINT32 location_handle(char *in1);
void strdel(char *str, char c);
static void GpsTask(void *arg)
{
	/*三轴加速度初始化*/
	GsensorI2CHandler(GsensorI2CCallback);
	GsensorInit();
	if (GpsRecvHandle == NULL)
	{
		GpsRecvHandle = osMessageQueueNew(1, sizeof(GPSInfo), NULL);
	}
	gpsReqMsg msg;
	char *p = NULL;
	CHAR *p2 = NULL;
	const char *delim = "\n";
	char *databuffer[20];
	UINT32 speedtemp;
	UINT32 latitude;
	UINT32 longitude;
	UINT16 direction;
	UINT16 GpsNoDataCounter = 0;
	UINT8 index = 0;
	UINT8 xyzCounter = 0;
	INT16 xData[3] = {0};
	INT16 yData[3] = {0};
	INT16 zData[3] = {0};
	INT16 xyzDataBuffer[3] = {0};
	GPSInfo GpsInfoData;
	PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_INIT);
	while (1)
	{
		switch (gProcess_Gps_Task)
		{
		case PROCESS_GPS_STATE_INIT:
		{

			osDelay(100);
			posGGAServiceStart(norGpsHandle);
			posGGAReset();
			PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_WORK);
			break;
		}
		case PROCESS_GPS_STATE_IDLE:
		{

			osDelay(100);
			if (gProcess_app == WORK)
			{
				PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_WORK);
				break;
			}
			else
			{
				PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_SLEEP);
				break;
			}
			break;
		}
		case PROCESS_GPS_STATE_WORK:
		{
			if (GpsNoDataCounter >= 60 * 10)
			{
				memset((UINT8 *)&GpsInfoData, 0x00, sizeof(GPSInfo));
				GpsNoDataCounter = 0;
				osDelay(100);
				posGGAServiceStart(norGpsHandle);
				posGGAReset();
			}
			osStatus_t ret = osMessageQueueGet(norGpsHandle, &msg, 0, 1000);
			if (ret == 0)
			{
				if (msg.dataPtr)
				{
					// char temp[] = "$GNRMC,082626.000,A,2939.91801,N,10637.09500,E,0.543,30.254,261120,,,A,V*17";
					memset((UINT8 *)&GpsInfoData, 0x00, sizeof(GPSInfo));
					p = strtok(msg.dataPtr, delim); //将信息进行分割
					p2 = strtok(NULL, delim);
#ifdef USING_PRINTF1
					printf("\nP msgptr data:%s\r\n", p);
#endif
					p = strtok(p, ","); //只取第1行的信息RMC
					// p = strtok(temp,",");//模拟测试
					if (strcmp(p, "$GNRMC") == 0)
					{
						index = 0;
						while (p)
						{
							databuffer[index] = p;
							p = strtok(NULL, ",");
							index++;
						}
						if (index > 5 && strcmp(databuffer[2], "A") == 0)
						{
							GpsNoDataCounter = 0;
							GpsInfoData.locateMark = 0x01; //有效，东经，北纬写定
							GpsFlag = 3;
							strdel(databuffer[3], '.');
							strdel(databuffer[5], '.');
							strdel(databuffer[7], '.');
							speedtemp = atol(databuffer[7]) * 1852 / 1e5; //节换算单位，1节=1.852km每小时
							GpsInfoData.speed[0] = (speedtemp >> 8) & 0xFF;
							GpsInfoData.speed[1] = speedtemp & 0xFF;
							latitude = location_handle(databuffer[3]);
							GpsInfoData.latitude[0] = latitude >> 24;
							GpsInfoData.latitude[1] = latitude >> 16;
							GpsInfoData.latitude[2] = latitude >> 8;
							GpsInfoData.latitude[3] = latitude;
							longitude = location_handle(databuffer[5]);
							GpsInfoData.longitude[0] = longitude >> 24;
							GpsInfoData.longitude[1] = longitude >> 16;
							GpsInfoData.longitude[2] = longitude >> 8;
							GpsInfoData.longitude[3] = longitude;
							if (speedtemp >= 50) //大于5km/h才输出方位
							{
								direction = atol(databuffer[8]);
								GpsInfoData.direction[0] = direction >> 8;
								GpsInfoData.direction[1] = direction;
							}
							else
							{
								GpsInfoData.direction[0] = 0xff;
								GpsInfoData.direction[1] = 0xff;
							}
							if (speedtemp >= 30 && speedtemp <= 1500 && BattWorkStateDelay == 0x01)
							{
								AppDataInfo.appDataModify = true;
								AppDataInfo.AccMileage = speedtemp / 36 + AppDataInfo.AccMileage;
								if (AppDataInfo.AccMileage >= 0xfffffffe)
								{
									AppDataInfo.AccMileage = 0;
								}
							}
						}
						GpsInfoData.AccMileage[0] = AppDataInfo.AccMileage >> 24;
						GpsInfoData.AccMileage[1] = AppDataInfo.AccMileage >> 16;
						GpsInfoData.AccMileage[2] = AppDataInfo.AccMileage >> 8;
						GpsInfoData.AccMileage[3] = AppDataInfo.AccMileage;
					}
					p2 = strtok(p2, ","); //只取第2行的信息GGA
					memset(databuffer, 0x30, 20);
					if (strcmp(p2, "$GNGGA") == 0)
					{
						index = 0;
						while (p2)
						{
							databuffer[index] = p2;
							p2 = strtok(NULL, ",");
							index++;
						}
						if (index > 9 && (strcmp(databuffer[6], "1") == 0 || strcmp(databuffer[6], "2") == 0))
						{
							GpsInfoData.satelliteNum = atol(databuffer[7]); //卫星数目写入
							GpsFlag = GpsInfoData.satelliteNum;
							strdel(databuffer[9], '.');
							UINT16 alt = 0;
							alt = atol(databuffer[9]) / 10 + 1000;
							GpsInfoData.altitude[0] = alt >> 8; //海拔
							GpsInfoData.altitude[1] = alt;
							strdel(databuffer[2], '.');
							strdel(databuffer[4], '.');
							latitude = location_handle(databuffer[2]);
							GpsInfoData.latitude[0] = latitude >> 24;
							GpsInfoData.latitude[1] = latitude >> 16;
							GpsInfoData.latitude[2] = latitude >> 8;
							GpsInfoData.latitude[3] = latitude;
							longitude = location_handle(databuffer[4]);
							GpsInfoData.longitude[0] = longitude >> 24;
							GpsInfoData.longitude[1] = longitude >> 16;
							GpsInfoData.longitude[2] = longitude >> 8;
							GpsInfoData.longitude[3] = longitude;
						}
						else
						{
							GpsFlag = 0;
							GpsNoDataCounter++;
						}
					}
					osMessageQueueReset(GpsRecvHandle);
					osMessageQueuePut(GpsRecvHandle, (UINT8 *)&GpsInfoData, 0, 1000);
				}
				if (msg.dataPtr)
					free(msg.dataPtr);
				msg.dataPtr = NULL;
			}
			if (getbit(PadInterrupt, 3) == 1 || getbit(PadInterrupt, 4) == 1 || TimeCounter % 50 == 0)
			{
				//SL_SC7A20_Reg_read_all();
				UINT8 ret = SL_SC7A20_Read_XYZ_Data(xyzDataBuffer);
				xData[xyzCounter] = xyzDataBuffer[0];
				yData[xyzCounter] = xyzDataBuffer[1];
				zData[xyzCounter] = xyzDataBuffer[2];
				xyzCounter++;
				if (xyzCounter > 2)
				{
					xyzCounter = 0;
				}
				xyzData[0] = (xData[0] + xData[1] + xData[2]) / 3;
				xyzData[1] = (yData[0] + yData[1] + yData[2]) / 3;
				xyzData[2] = (zData[0] + zData[1] + zData[2]) / 3;
#ifdef USING_PRINTF
				printf("\n%d %d %d \r\n", xyzData[0], xyzData[1], xyzData[2]);
#endif
			}

			PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_IDLE);
			break;
		}
		case PROCESS_GPS_STATE_SLEEP:
		{
			posGGAServiceStop();
			while (TRUE)
			{
				osDelay(100);
				if (gProcess_app == WORK)
				{
					PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_INIT);
					break;
				}
			}
			break;
		}
		}
	}
}
void AppTaskGpsInit(void *arg)
{

	if (norGpsHandle == NULL)
	{
		norGpsHandle = osMessageQueueNew(1, sizeof(gpsReqMsg), NULL);
		if (norGpsHandle == NULL)
			return;
	}
	if (gpsTaskHandle == NULL)
	{
		osThreadAttr_t task_attr;
		memset(&task_attr, 0, sizeof(task_attr));
		task_attr.name = "GPSTask";
		task_attr.priority = osPriorityBelowNormal6;
		task_attr.cb_mem = &gpsTask;
		task_attr.cb_size = sizeof(StaticTask_t);
		task_attr.stack_mem = gpsTaskStack;
		task_attr.stack_size = GPS_TASK_STACK_SIZE;
		memset(&gpsTaskStack, 0xa5, GPS_TASK_STACK_SIZE);
		gpsTaskHandle = osThreadNew(GpsTask, NULL, &task_attr);
		if (gpsTaskHandle == NULL)
			return;
	}
}
void AppTaskGpsDeInit(void *arg)
{
	osThreadTerminate(gpsTaskHandle);
	gpsTaskHandle = NULL;
}
UINT32 location_handle(char *in1)
{
	UINT32 location_temp;
	UINT32 location_degree;
	UINT32 location_min;
	location_temp = atol(in1);
	location_degree = location_temp / (1e7);
	location_degree = location_degree * (1e6);
	location_min = location_temp - location_degree * 10;
	location_min = location_min / 6;
	location_temp = location_degree + location_min;
	return location_temp;
}
void strdel(char *str, char c)
{
	char *p = str;
	while (*str)
	{
		if (*str != c)
			*p++ = *str;
		str++;
	}
	*p = '\0';
}