/*
    Copyright   Novatek Microelectronics Corp. 2018.  All rights reserved.

    @file       GPS.c
    @ingroup

    @brief      Execute command via UART interface.
                Execute command and output message via UART interface.

    @note       Nothing.

    @version    V1.00.000
*/

#include <stdio.h>
#include <string.h>
#include <kwrap/stdio.h>
#include <stdlib.h>
//#include "Debug.h"
//#include "SysKer.h"
//#include "kernel.h"
#include <kwrap/semaphore.h>
#include <kwrap/flag.h>
#include <kwrap/error_no.h>
#include <uart.h>
#include "GPS.h"

//@{
#define __MODULE__					GPS
#define __DBGLVL__          2 // 0=FATAL, 1=ERR, 2=WRN, 3=UNIT, 4=FUNC, 5=IND, 6=MSG, 7=VALUE, 8=USER
#define __DBGFLT__          "*" //*=All, [mark]=CustomClass
#include <kwrap/debug.h>

#define _GPS_CheckSum_Enable		0   //Enable to open check sum mechanism

//Declare functions
void GPSRec_NMEAParser(CHAR *NMEA_str, UINT32 Len, NMEATYPE NMEAtype);


//Declare variables
GSAINFO GSAInfo;
RMCINFO RMCInfo;
GGAINFO GGAInfo;
GSVINFO GSVInfo;
_ALIGNED(4) NMEASENTENCE NMEASentence={0};

// global variables for parsing NMEA code
CHAR    type[NMEA_TYPE_SIZE], RemainStr[NMEA_SENTENCE_SIZE], PostPtr[NMEA_SENTENCE_SIZE + 20], *pPostPrt;
UINT32  PostPtrLen;
UINT32  CheckSum;
//GSA used
CHAR    SelMode;
UINT32  FixMode, SatUsed01, SatUsed02, SatUsed03, SatUsed04, SatUsed05, SatUsed06, SatUsed07, SatUsed08, SatUsed09, SatUsed10, SatUsed11, SatUsed12;
double  PDOP, HDOP, VDOP;
//RMC used
UINT32  Date;
CHAR    Status,MagVarInd;
double  Speed, Angle, MagVar;
//GGA used
double  UTCTime;
UINT32  PosFixInd, SatNumber, Empty1, Empty2;
double  Lat, Lon, Alt, MSL;
CHAR    NSInd, EWInd, M1, M2;
//GSV used
UINT32  NumOfSen, SenNum, SatInView, SatNum1, Elev1, Azi1, SNR1, SatNum2, Elev2, Azi2, SNR2, SatNum3, Elev3, Azi3, SNR3, SatNum4, Elev4, Azi4, SNR4;

//GPS heartbeat
static UINT32 GPSHeartBeat = 0;         //#NT#2015/10/02#KCHong#Add GPS heartbeat to detect GPS status

static GPS_UART_OBJ GpsUart = {
	.open		= uart_open,
	.close		= uart_close,
	.init		= uart_initHW,
	.getString	= uart_getString,
};

extern void GPSRecTsk(void);

void GPSRec_TransformData(CHAR *NMEA_str, UINT32 Len, CHAR *pPostPrt, UINT32 *PostPtrLen);
BOOL GPSRec_CheckSum(CHAR *NMEA_str, UINT32 Len, UINT32 CheckSum);
BOOL GPSRec_GetUTCDate(UINT32 *Year, UINT32 *Month, UINT32 *Day);
BOOL GPSRec_GetUTCTime(UINT32 *Hour, UINT32 *Minute, UINT32 *Second);

THREAD_HANDLE GPSRECEIVE_ID = 0;
ID FLG_ID_GPS = 0;
ID FGPS_SEM_ID = 0;

#define PRI_GPSRECEIVE            10
#define STKSIZE_GPSRECEIVE        6144//64KB stack size

static BOOL     g_bGPSreceiveTskOpened = FALSE;
static BOOL     g_bGPSreceiveTskClosing = FALSE;


void GPSreceive_InstallID(void)
{
	T_CFLG cflg ;
	//OS_CONFIG_TASK(MOVIESTAMPTSK_ID, PRI_MOVIESTAMP, STKSIZE_MOVIESTAMP, MovieStampTsk);
	#if 0
	OS_CONFIG_FLAG(FLG_ID_GPS);
	OS_CONFIG_SEMPHORE(FGPS_SEM_ID, 0, 1, 1);
	#else
	vos_flag_create(&FLG_ID_GPS, &cflg, "GPS receive FLG");
	vos_sem_create(&FGPS_SEM_ID, 1, "GPS receive SEM");
	#endif
}
void GPSreceive_UnInstallID(void)
{
	//OS_CONFIG_TASK(MOVIESTAMPTSK_ID, PRI_MOVIESTAMP, STKSIZE_MOVIESTAMP, MovieStampTsk);
	vos_flag_destroy(FLG_ID_GPS);
	vos_sem_destroy(FGPS_SEM_ID);
}

ER GPSreceiveTsk_Open(void)
{
	if (g_bGPSreceiveTskOpened) {
		return E_SYS;
	}
	GPSreceive_InstallID();
	vos_flag_clr(FLG_ID_GPS, GPS_FLAG_ALL);
	g_bGPSreceiveTskOpened = TRUE;
	g_bGPSreceiveTskClosing = FALSE;
	//sta_tsk(MOVIESTAMPTSK_ID, 0);
	GPSRECEIVE_ID=vos_task_create(GPSRecTsk,  0, "GPSreceiveTsk",   PRI_GPSRECEIVE,	STKSIZE_GPSRECEIVE);
	vos_task_resume(GPSRECEIVE_ID);
	return E_OK;
}

ER GPSreceiveTsk_Close(void)
{
	FLGPTN  FlgPtn;

	if (!g_bGPSreceiveTskOpened) {
		return E_SYS;
	}

	g_bGPSreceiveTskClosing = TRUE;
	vos_flag_set(FLG_ID_GPS, GPS_FLAG_DATAVALID);//add to close tsk
	CHKPNT;
	vos_flag_wait(&FlgPtn, FLG_ID_GPS, GPS_FLAG_DATAVALID, TWF_ORW);
	CHKPNT;
	vos_task_destroy(GPSRECEIVE_ID);
	GPSreceive_UnInstallID();
	g_bGPSreceiveTskOpened = FALSE;

	return E_OK;
}

#if 0
void GPS_SetUartObj(GPS_UART_OBJ *UartObj)
{
	memcpy(&GpsUart, UartObj, sizeof(GPS_UART_OBJ));
}
#endif

static BOOL bGPSRecOpened = FALSE;
ER GPSRec_Open(UART_BAUDRATE BaudRate)
{
	UART_INIT_PARA  UartParaBasic;

    if (bGPSRecOpened)
        return E_SYS;

    if (GpsUart.open() == E_OK)
    {
        DBG_WRN("UART open success\r\n");
		UartParaBasic.BaudRate       = BaudRate;//UART_BAUDRATE_9600;
		//UartParaBasic.BaudRate       = 9600;
		UartParaBasic.Length         = UART_LEN_L8_S1;
		UartParaBasic.Parity         = UART_PARITY_NONE;
		UartParaBasic.FlowCtrlMode   = UART_FC_MODE_NONE;
		UartParaBasic.DataTransMode  = UART_DATA_TRANS_PIO;
        GpsUart.init(&UartParaBasic);
    }
    else
    {
        DBG_ERR("UART open fail!\r\n");
        return E_SYS;
    }

    //DBG_DUMP("Start GPS receive task\r\n");
    //sta_tsk(GPSRECEIVE_ID,0);
    GPSreceiveTsk_Open();

    //loc_cpu();
    bGPSRecOpened = TRUE;
    //unl_cpu();

    return E_OK;
}

ER GPSRec_Close(void)
{
    if (!bGPSRecOpened)
        return E_SYS;

    if (GpsUart.close() == E_OK)
    {
        DBG_WRN("UART close success\r\n");
    }
    else
    {
        DBG_ERR("UART close fail!\r\n");
    }

    //DBG_DUMP("Terminate GPS receive task\r\n");

    //ter_tsk(GPSRECEIVE_ID);
    GPSreceiveTsk_Close();

    //loc_cpu();
    bGPSRecOpened = FALSE;
    //unl_cpu();

    return E_OK;
}

void GPSRecTsk(void)
{
    UINT32  uiLen;
    UINT16  CheckSentenceType1, CheckSentenceType2;
    UINT32  CheckSentenceType3;
    CHAR    RecSentence[NMEA_SENTENCE_SIZE];
    CHAR    *pSentenceData;

    //kent_tsk();

    //clr_flg(FLG_ID_GPS, GPS_FLAG_DATAVALID);

	THREAD_ENTRY();

	vos_flag_clr(FLG_ID_GPS, GPS_FLAG_DATAVALID);

    while(GPSRECEIVE_ID)
    {
        uiLen = NMEA_SENTENCE_SIZE;
		CHKPNT;
        if (GpsUart.getString(RecSentence, &uiLen) == E_PAR)
        {
            DBG_ERR("UART parameter error!\r\n");
        }
		CHKPNT;
        ////DBG_DUMP("%s\r\n", RecSentence);
        //Each NMEA sentence begins with '$' (0x24)
        DBGH(RecSentence[0]);
        if (RecSentence[0] == 0x24)
        {
            GPSHeartBeat ++;        //#NT#2015/10/02#KCHong#Add GPS heartbeat to detect GPS status
            pSentenceData = RecSentence;
            CheckSentenceType1 = *(UINT16 *)(pSentenceData + 2);
            CheckSentenceType2 = *(UINT16 *)(pSentenceData + 4);
            CheckSentenceType3 = ((UINT)CheckSentenceType2 << 16) | (UINT)CheckSentenceType1;

            switch(CheckSentenceType3)
            {
            case GSA_SENTENCE:
                NMEASentence.GSA = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, GSA);
                break;

            case RMC_SENTENCE:
                NMEASentence.RMC = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, RMC);
                break;

            case GGA_SENTENCE:
                NMEASentence.GGA = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, GGA);
                break;

            case GSV_SENTENCE:
                NMEASentence.GSV = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, GSV);
                break;

            case GLL_SENTENCE:
                NMEASentence.GLL = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, GLL);
                break;

            case VTG_SENTENCE:
                NMEASentence.VTG = RecSentence;
                GPSRec_NMEAParser(RecSentence, uiLen, VTG);
                break;

            default:
                DBG_ERR("GPS: Can't identify this sentence 0x%X \r\n",CheckSentenceType3);
                break;
            }

        }

    }

	THREAD_RETURN(0);
}

ER GPSRec_Reset(void)
{
    return E_OK;
}

BOOL GPSRec_CheckData(void)
{
    //DBG_DUMP("GPS: DATA VALID: 0x%X\r\n", vos_flag_chk(FLG_ID_GPS, GPS_FLAG_DATAVALID));
    if (vos_flag_chk(FLG_ID_GPS, GPS_FLAG_DATAVALID))
        return TRUE;
    else
        return FALSE;

}

void GPSRec_TransformData(CHAR *NMEA_str, UINT32 Len, CHAR *pPostPrt, UINT32 *PostPtrLen)
{
    UINT32 i;

    //Process sentence, change ',' as ' '.
    //If message is null, change to corresponding value.
    for (i = 0; i <= Len; i++)
    {
        *pPostPrt = *NMEA_str;

        if (*NMEA_str == ',')
        {
            *pPostPrt = 0x20;   //Instead of ' '
            if ((*(NMEA_str+1) == ',')|(*(NMEA_str+1) == '*'))
            {
                *++pPostPrt = 0x30;  //Insert '0'
                *PostPtrLen = *PostPtrLen + 1;
            }
        }
        else if ((*NMEA_str == '*')|(i == Len))
        {
            *pPostPrt = 0x20;   //Instead of ' '
        }

        NMEA_str++;
        pPostPrt++;

    }

    NMEA_str -= Len;
    pPostPrt -= *PostPtrLen;

}


BOOL GPSRec_CheckSum(CHAR *NMEA_str, UINT32 Len, UINT32 CheckSum)
{
    UINT32 i, LenTemp = 1;
    CHAR XORData;
    XORData = *(NMEA_str++);

    //Doing 8 bit exclusive OR of all characters between, but not including, the '$' and '*'.
    for (i = 1; i < Len; i++)
    {
        NMEA_str++;
        if (*NMEA_str == '*')
            break;

        LenTemp++;
        XORData ^= *NMEA_str;
    }
    NMEA_str -= LenTemp;

    if (XORData == (CHAR)CheckSum)
    {
        return TRUE;
    }
    else
    {
        DBG_ERR("GPS: check sum error! 0x%X != 0x%X\r\n",CheckSum, XORData);
        return FALSE;
    }

}

void GPSRec_NMEAParser(CHAR *NMEA_str, UINT32 Len, NMEATYPE NMEAtype)
{
    UINT32 i;

    pPostPrt = PostPtr;
    PostPtrLen = Len;

    GPSRec_TransformData(NMEA_str, Len, pPostPrt, &PostPtrLen);
    switch(NMEAtype)
    {
    case GSA:
        sscanf(pPostPrt, "%s %c %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lf %lf %lf %lX\n", type, &SelMode, &FixMode, &SatUsed01, &SatUsed02, &SatUsed03, &SatUsed04, &SatUsed05, &SatUsed06, &SatUsed07, &SatUsed08, &SatUsed09, &SatUsed10, &SatUsed11, &SatUsed12, &PDOP, &HDOP, &VDOP, &CheckSum);
        #if (_GPS_CheckSum_Enable)
            GPSRec_CheckSum(NMEA_str, Len, CheckSum);
        #endif

        if (FixMode != No_Fix)
        {
            GSAInfo.SelMode = SelMode;
            GSAInfo.FixMode = FixMode;
            GSAInfo.SatUsed01 = SatUsed01;
            GSAInfo.SatUsed02 = SatUsed02;
            GSAInfo.SatUsed03 = SatUsed03;
            GSAInfo.SatUsed04 = SatUsed04;
            GSAInfo.SatUsed05 = SatUsed05;
            GSAInfo.SatUsed06 = SatUsed06;
            GSAInfo.SatUsed07 = SatUsed07;
            GSAInfo.SatUsed08 = SatUsed08;
            GSAInfo.SatUsed09 = SatUsed09;
            GSAInfo.SatUsed10 = SatUsed10;
            GSAInfo.SatUsed11 = SatUsed11;
            GSAInfo.SatUsed12 = SatUsed12;
            GSAInfo.PDOP = PDOP;
            GSAInfo.HDOP = HDOP;
            GSAInfo.VDOP = VDOP;
            //DBG_DUMP(("#GSA SelMode = %c, FixMode = %d \r\n",GSAInfo.SelMode,GSAInfo.FixMode));
            //DBG_DUMP(("#GSA SelMode = %c, FixMode = %d \r\n",GSAInfo.SelMode,GSAInfo.FixMode));
            //DBG_DUMP(("#GSA SatUsed1 = %d, %d, %d, %d \r\n",GSAInfo.SatUsed01,GSAInfo.SatUsed02, GSAInfo.SatUsed03, GSAInfo.SatUsed04));
            //DBG_DUMP(("#GSA SatUsed2 = %d, %d, %d, %d \r\n",GSAInfo.SatUsed05,GSAInfo.SatUsed06, GSAInfo.SatUsed07, GSAInfo.SatUsed08));
            //DBG_DUMP(("#GSA SatUsed3 = %d, %d, %d, %d \r\n",GSAInfo.SatUsed09,GSAInfo.SatUsed10, GSAInfo.SatUsed11, GSAInfo.SatUsed12));
            //DBG_DUMP(("#PDOP = %.1f, HDOP = %.1f, VDOP = %.1f CheckSum = %X\r\n",GSAInfo.PDOP,GSAInfo.HDOP, GSAInfo.VDOP,CheckSum));
        }

        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));

        //Check processed sentence
        /*
        if (PostPtrLen)
        {
            for (i = 0; i < PostPtrLen; i++)
                DBG_WRN(("%c",*pPostPrt++));
        }
        DBG_WRN(("\r\n"));
        */
        break;

    case RMC:
        sscanf(pPostPrt, "%s %lf %c %lf %c %lf %c %lf %lf %lu %lf %c %lX\n", type, &UTCTime, &Status, &Lat, &NSInd, &Lon, &EWInd, &Speed, &Angle, &Date, &MagVar, &MagVarInd,&CheckSum);
        #if (_GPS_CheckSum_Enable)
            GPSRec_CheckSum(NMEA_str, Len, CheckSum);
        #endif

        {
            RMCInfo.Hour =      (UINT32)UTCTime / 10000;
            RMCInfo.Minute =    ((UINT32)UTCTime % 10000) / 100;
            RMCInfo.Second =    ((UINT32)UTCTime % 100);
            RMCInfo.Status =    Status;
            RMCInfo.Latitude =  Lat;
            RMCInfo.NSInd =     NSInd;
            RMCInfo.Longitude = Lon;
            RMCInfo.EWInd =     EWInd;
            RMCInfo.Speed =     Speed;
            RMCInfo.Angle =     Angle;
            RMCInfo.Day =       Date / 10000;
            RMCInfo.Month =     (Date % 10000) / 100;
            RMCInfo.Year =      (Date % 100);
        }

        if (Status == 'A')   //Data valid
        {
            set_flg(FLG_ID_GPS, GPS_FLAG_DATAVALID);

            //DBG_DUMP(("\x1b[34m"));   //Blue
            //DBG_DUMP(("#RMC: Status = %c, Speed = %.1f, Angle = %.1f \r\n",RMCInfo.Status,RMCInfo.Speed,RMCInfo.Angle));
            //DBG_DUMP(("#RMC: Date = %d\\%d\\%d CheckSum = %X\r\n",RMCInfo.Year,RMCInfo.Month,RMCInfo.Day,CheckSum));
            //DBG_DUMP(("\x1b[30m"));   //Black
        }
        else if (Status == 'V')
        {
            clr_flg(FLG_ID_GPS, GPS_FLAG_DATAVALID);
        }

        DBG_IND(("\x1b[34m"));   //Blue
        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));
        //Check processed sentence
        /*
        if (PostPtrLen)
        {
            for (i = 0; i < PostPtrLen; i++)
                DBG_WRN(("%c",*pPostPrt++));
        }
        DBG_WRN(("\r\n"));
        */
        DBG_IND(("\x1b[30m"));   //Black
        break;

    case GGA:
        sscanf(pPostPrt, "%s %lf %lf %c %lf %c %lu %lu %lf %lf %c %lf %c %lu %lu %lX\n", type, &UTCTime, &Lat, &NSInd, &Lon, &EWInd,  &PosFixInd, &SatNumber, &HDOP, &Alt, &M1, &MSL, &M2, &Empty1, &Empty2, &CheckSum);
        #if (_GPS_CheckSum_Enable)
            GPSRec_CheckSum(NMEA_str, Len, CheckSum);
        #endif

        if (PosFixInd == 1)  //Data valid
        {
            GGAInfo.Hour = (UINT32)UTCTime / 10000;
            GGAInfo.Minute = ((UINT32)UTCTime % 10000) / 100;
            GGAInfo.Second = ((UINT32)UTCTime % 100);
            GGAInfo.Latitude = Lat;
            GGAInfo.NSInd = NSInd;
            GGAInfo.Longitude = Lon;
            GGAInfo.EWInd = EWInd;
            GGAInfo.PosFixInd = PosFixInd;
            GGAInfo.SatNumber = SatNumber;
            GGAInfo.HDOP = HDOP;
            GGAInfo.Altitude = Alt;
            GGAInfo.MSL = MSL;
            //DBG_DUMP(("\x1b[31m"));   //Red
            //DBG_DUMP(("#GGA: Time = %d:%d:%d CheckSum = %X\r\n", GGAInfo.Hour, GGAInfo.Minute, GGAInfo.Second,CheckSum));
            //DBG_DUMP(("#GGA: Lat = %1f %c, Lon = %1f %c \r\n", GGAInfo.Latitude, NSInd, GGAInfo.Longitude, EWInd));
            //DBG_DUMP(("#GGA: PosFixInd = %d, SatNum = %d, HDOP = %2.1f, Alt = %.1f, MSL = %.1f \r\n", GGAInfo.PosFixInd, GGAInfo.SatNumber, GGAInfo.HDOP, GGAInfo.Altitude, GGAInfo.MSL));
            //DBG_DUMP(("\x1b[30m"));   //Black

        }

        DBG_IND(("\x1b[31m"));   //Red
        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));

        //Check processed sentence
        /*
        if (PostPtrLen)
        {
            for (i = 0; i < PostPtrLen; i++)
                DBG_WRN(("%c",*pPostPrt++));
        }
        DBG_WRN(("\r\n"));
        */
        DBG_IND(("\x1b[30m"));   //Black
        break;

    case GSV:
        sscanf(pPostPrt, "%s %lu %lu %lu %s\n", type, &NumOfSen, &SenNum, &SatInView, RemainStr);
        if (NumOfSen == SenNum)
        {
            if ((SatInView % 4) == 1)
                sscanf(pPostPrt, "%s %lu %lu %lu %lu %lu %lu %lu %lX\n", type, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &CheckSum);
            else if ((SatInView % 4) == 2)
                sscanf(pPostPrt, "%s %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lX\n", type, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &SatNum2, &Elev2, &Azi2, &SNR2, &CheckSum);
            else if ((SatInView % 4) == 3)
                sscanf(pPostPrt, "%s %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lX\n", type, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &SatNum2, &Elev2, &Azi2, &SNR2, &SatNum3, &Elev3, &Azi3, &SNR3, &CheckSum);
            else
                sscanf(pPostPrt, "%s %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lX\n", type, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &SatNum2, &Elev2, &Azi2, &SNR2, &SatNum3, &Elev3, &Azi3, &SNR3, &SatNum4, &Elev4, &Azi4, &SNR4, &CheckSum);
        }
        else
        {
//            sscanf_s(pPostPrt, "%s %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %X\n", type, NMEA_TYPE_SIZE, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &SatNum2, &Elev2, &Azi2, &SNR2, &SatNum3, &Elev3, &Azi3, &SNR3, &SatNum4, &Elev4, &Azi4, &SNR4, &CheckSum);
            sscanf(pPostPrt, "%s %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lu %lX\n", type, &NumOfSen, &SenNum, &SatInView, &SatNum1, &Elev1, &Azi1, &SNR1, &SatNum2, &Elev2, &Azi2, &SNR2, &SatNum3, &Elev3, &Azi3, &SNR3, &SatNum4, &Elev4, &Azi4, &SNR4, &CheckSum);
        }

        #if (_GPS_CheckSum_Enable)
            GPSRec_CheckSum(NMEA_str, Len, CheckSum);
        #endif

        if (SenNum == 1)   //GSV1
        {
            GSVInfo.NumOfSen =     NumOfSen;
            GSVInfo.SatInView =    SatInView;
            GSVInfo.SAT01.SatNum = SatNum1;
            GSVInfo.SAT01.Elev =   Elev1;
            GSVInfo.SAT01.Azi =    Azi1;
            GSVInfo.SAT01.SNR =    SNR1;
            GSVInfo.SAT02.SatNum = SatNum2;
            GSVInfo.SAT02.Elev =   Elev2;
            GSVInfo.SAT02.Azi =    Azi2;
            GSVInfo.SAT02.SNR =    SNR2;
            GSVInfo.SAT03.SatNum = SatNum3;
            GSVInfo.SAT03.Elev =   Elev3;
            GSVInfo.SAT03.Azi =    Azi3;
            GSVInfo.SAT03.SNR =    SNR3;
            GSVInfo.SAT04.SatNum = SatNum4;
            GSVInfo.SAT04.Elev =   Elev4;
            GSVInfo.SAT04.Azi =    Azi4;
            GSVInfo.SAT04.SNR =    SNR4;
            //DBG_DUMP(("\x1b[35m"));   //Violet
            //DBG_DUMP(("#GSV%d SatInView = %d, CheckSum = %X\r\n",SenNum, SatInView, CheckSum));
            //DBG_DUMP(("#GSV%d Sat1 = %d, %d, %d, %d, Sat2 = %d, %d, %d, %d\r\n",SenNum, SatNum1, Elev1, Azi1, SNR1, SatNum2, Elev2, Azi2, SNR2));
            //DBG_DUMP(("#GSV%d Sat3 = %d, %d, %d, %d, Sat4 = %d, %d, %d, %d\r\n",SenNum, SatNum3, Elev3, Azi3, SNR3, SatNum4, Elev4, Azi4, SNR4));
            //DBG_DUMP(("\x1b[30m"));      //Black

        }
        else if (SenNum == 2)   //GSV2
        {
            GSVInfo.NumOfSen =     NumOfSen;
            GSVInfo.SatInView =    SatInView;
            GSVInfo.SAT05.SatNum = SatNum1;
            GSVInfo.SAT05.Elev =   Elev1;
            GSVInfo.SAT05.Azi =    Azi1;
            GSVInfo.SAT05.SNR =    SNR1;
            GSVInfo.SAT06.SatNum = SatNum2;
            GSVInfo.SAT06.Elev =   Elev2;
            GSVInfo.SAT06.Azi =    Azi2;
            GSVInfo.SAT06.SNR =    SNR2;
            GSVInfo.SAT07.SatNum = SatNum3;
            GSVInfo.SAT07.Elev =   Elev3;
            GSVInfo.SAT07.Azi =    Azi3;
            GSVInfo.SAT07.SNR =    SNR3;
            GSVInfo.SAT08.SatNum = SatNum4;
            GSVInfo.SAT08.Elev =   Elev4;
            GSVInfo.SAT08.Azi =    Azi4;
            GSVInfo.SAT08.SNR =    SNR4;
            //DBG_DUMP(("\x1b[35m"));   //Violet
            //DBG_DUMP(("#GSV%d SatInView = %d, CheckSum = %X\r\n",SenNum, SatInView, CheckSum));
            //DBG_DUMP(("#GSV%d Sat5 = %d, %d, %d, %d, Sat6 = %d, %d, %d, %d\r\n",SenNum, SatNum1, Elev1, Azi1, SNR1, SatNum2, Elev2, Azi2, SNR2));
            //DBG_DUMP(("#GSV%d Sat7 = %d, %d, %d, %d, Sat8 = %d, %d, %d, %d\r\n",SenNum, SatNum3, Elev3, Azi3, SNR3, SatNum4, Elev4, Azi4, SNR4));
            //DBG_DUMP(("\x1b[30m"));      //Black

        }
        else if (SenNum == 3)   //GSV3
        {
            GSVInfo.NumOfSen =     NumOfSen;
            GSVInfo.SatInView =    SatInView;
            GSVInfo.SAT09.SatNum = SatNum1;
            GSVInfo.SAT09.Elev =   Elev1;
            GSVInfo.SAT09.Azi =    Azi1;
            GSVInfo.SAT09.SNR =    SNR1;
            GSVInfo.SAT10.SatNum = SatNum2;
            GSVInfo.SAT10.Elev =   Elev2;
            GSVInfo.SAT10.Azi =    Azi2;
            GSVInfo.SAT10.SNR =    SNR2;
            GSVInfo.SAT11.SatNum = SatNum3;
            GSVInfo.SAT11.Elev =   Elev3;
            GSVInfo.SAT11.Azi =    Azi3;
            GSVInfo.SAT11.SNR =    SNR3;
            GSVInfo.SAT12.SatNum = SatNum4;
            GSVInfo.SAT12.Elev =   Elev4;
            GSVInfo.SAT12.Azi =    Azi4;
            GSVInfo.SAT12.SNR =    SNR4;
            //DBG_DUMP(("\x1b[35m"));   //Violet
            //DBG_DUMP(("#GSV%d SatInView = %d, CheckSum = %X\r\n",SenNum, SatInView, CheckSum));
            //DBG_DUMP(("#GSV%d Sat9 = %d, %d, %d, %d,  Sat10 = %d, %d, %d, %d\r\n",SenNum, SatNum1, Elev1, Azi1, SNR1, SatNum2, Elev2, Azi2, SNR2));
            //DBG_DUMP(("#GSV%d Sat11 = %d, %d, %d, %d, Sat12 = %d, %d, %d, %d\r\n",SenNum, SatNum3, Elev3, Azi3, SNR3, SatNum4, Elev4, Azi4, SNR4));
            //DBG_DUMP(("\x1b[30m"));      //Black

        }
        else
        {
            DBG_ERR("GPS: Invalid number of GSV");
        }

        DBG_IND(("\x1b[35m"));   //Violet
        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));
        //Check processed sentence
        /*
        if (PostPtrLen)
        {
            for (i = 0; i < PostPtrLen; i++)
                DBG_WRN(("%c",*pPostPrt++));
        }
        DBG_WRN(("\r\n"));
        */
        DBG_IND(("\x1b[30m"));      //Black
        break;

    //Do not support GLL and VTG yet
    case GLL:
        DBG_IND(("\x1b[36m"));   //Blue
        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));
        DBG_IND(("\x1b[30m"));   //Black
        break;

    case VTG:
        DBG_IND(("\x1b[32m"));   //Green
        if (Len)
        {
            for (i = 0; i < Len; i++)
                DBG_IND(("%c",*NMEA_str++));
        }
        DBG_IND(("\r\n"));
        DBG_IND(("\x1b[30m"));   //Black
        break;

    default:
        break;
    }

}

BOOL GPSRec_GetRMCDate(RMCINFO *pRMCInfo)
{
    memcpy(pRMCInfo,&RMCInfo,sizeof(RMCINFO));
    if (RMCInfo.Status == 'A')  //Data valid
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

BOOL GPSRec_GetUTCDate(UINT32 *Year, UINT32 *Month, UINT32 *Day)
{
    if (RMCInfo.Status == 'A')  //Data valid
    {
        *Year   = RMCInfo.Year;
        *Month  = RMCInfo.Month;
        *Day    = RMCInfo.Day;
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

BOOL GPSRec_GetUTCTime(UINT32 *Hour, UINT32 *Minute, UINT32 *Second)
{
    if (RMCInfo.Status == 'A')   //Data valid
    {
        *Hour   = RMCInfo.Hour;
        *Minute = RMCInfo.Minute;
        *Second = RMCInfo.Second;
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

BOOL GPSRec_GetGGADate(GGAINFO *pGGAInfo)
{
    memcpy(pGGAInfo,&GGAInfo,sizeof(GGAINFO));
    if (PosFixInd == 1)  //Data valid
    {
        return TRUE;
    }
    else
        return FALSE;
}

FLOAT GPSRec_GetGPS_Speed(void)
{
    static FLOAT CurSpeed = 0;

    CurSpeed = RMCInfo.Speed * 1.852;

    return CurSpeed;
}

//#NT#2015/10/02#KCHong -begin
//#NT#Add GPS heartbeat to detect GPS status
BOOL GPSRec_GetSpeed(FLOAT *Speed)
{
    static float CurSpeed = 0;

    if (RMCInfo.Status == 'A')   //Data valid
    {
    	CurSpeed = RMCInfo.Speed * 1.852;
        *Speed = CurSpeed;
        return TRUE;
    }
    else
    {
        *Speed = 0.0;
        return FALSE;
    }
}

UINT32 GetGPSHeartBeat(void)
{
    return GPSHeartBeat;
}
//#NT#2015/10/02#KCHong -end