#include "fm_parse_gps.h"

bool FMParseGPS::ParseRMC(char szSentence[], NMEA_INFO* data, int nPacketSize)
{
    Q_UNUSED(nPacketSize);

    // qDebug() << szSentence;

    char szItem[NMEA_TOKEN_SIZE]={0,};
    long nTemp;

    // 의미없으니 하지 말자
    // DWORD dwCheckSum = 0;
    //for (i = 1; i < strlen(szSentence) && szSentence[i] != '*'; i++)
    //	dwCheckSum ^= szSentence[i];

    //NMEA_INFO* data = &_gpsData[index];

    //0th
    szSentence = _GetNextToken(szSentence, szItem);

    // 1th
    char	szUtc[11] = {0,};
    szSentence = _GetNextToken(szSentence, szItem);
    strncpy(szUtc, szItem, sizeof(szUtc));

    // 2th
    szSentence = _GetNextToken(szSentence, szItem);
    char cStatus = szItem[0];

    // 3th
    szSentence = _GetNextToken(szSentence, szItem);
    data->Latitude = atof(szItem);

    // 4th
    szSentence = _GetNextToken(szSentence, szItem);
    unsigned char cLatitudeDir = szItem[0];

    // 5th
    szSentence = _GetNextToken(szSentence, szItem);
    data->Longitude = atof(szItem);

    // 6th
    szSentence = _GetNextToken(szSentence, szItem);
    unsigned char cLongitudeDir = szItem[0];

    // 7th
    szSentence = _GetNextToken(szSentence, szItem);
    double speed = atof(szItem);

    // 8th
    szSentence = _GetNextToken(szSentence, szItem);
    double track = atof(szItem);

    // 9th
    char szDate[7] = {0,};
    szSentence = _GetNextToken(szSentence, szItem);
    strncpy(szDate, szItem, sizeof(szDate));

    // 10th
    szSentence = _GetNextToken(szSentence, szItem);
    // double Mag = atof(szItem); // Magnetic variation

    // 11th
    szSentence = _GetNextToken(szSentence, szItem);
    // unsigned char cMagDir = szItem[0];

    // Set GPS Info
    if (strlen(szUtc) >= 6)
    {
        data->nHour     = (szUtc[0] - 48) * 10 + (szUtc[1] - 48);
        data->nMin      = (szUtc[2] - 48) * 10 + (szUtc[3] - 48);
        data->nSec      = (szUtc[4] - 48) * 10 + (szUtc[5] - 48);
    }
    if (strlen(szDate) >= 6)
    {
        data->nDay      = (szDate[0] - 48) * 10 + (szDate[1] - 48);
        data->nMonth    = (szDate[2] - 48) * 10 + (szDate[3] - 48);
        data->nYear     = (szDate[4] - 48) * 10 + (szDate[5] - 48);
        if (data->nYear > 80)
        {
            data->nYear += 1900;
        }
        else
        {
            data->nYear += 2000;
        }
    }
    // printf("date:%d-%d-%d %d:%d:%d\n",data->nYear,data->nMonth,data->nDay,data->nHour,data->nMin,data->nSec);
    data->nStatus = (cStatus == 'A') ? 1 : 0;

    data->Latitude = data->Latitude / 100.0;
    nTemp = (long)data->Latitude;
    data->Latitude = (double)nTemp + (double)(data->Latitude - nTemp) * 100.0 / 60.0;
    if (cLatitudeDir == 'S')
    {
        data->Latitude *= -1;
    }

    data->Longitude = data->Longitude / 100.0;
    nTemp = (long)data->Longitude;
    data->Longitude = (double)nTemp + (double)(data->Longitude - nTemp) * 100.0 / 60.0;

    if (cLongitudeDir == 'W')
    {
        data->Longitude *= -1;
    }

    data->Speed = speed * 1.852; // 1 knots는 1해리를 1시간에 가는 속력으로 1해리는 1852m

    data->nAngle = (uint8_t) track;
//    //	data->nAngle = (long) track;
//    if((data->nAngle-track>5 || data->nAngle-track<-5)	||
//       (_nLastAngle[1]==_nLastAngle[0] && _nLastAngle[0] == track ))
//    {
//        data->nAngle = (uint8_t) track;
//    }
//    else
//    {
//        _nLastAngle[1] = _nLastAngle[0];
//        _nLastAngle[0] = (long)track;
//    }

    return true;
}
char* FMParseGPS::_GetNextToken(char* lpSentence, char* lpToken, int iTokenSize)
{
    lpToken[0] = '\0';
    if (lpSentence == NULL || lpSentence[0] == '\0')
    {
        return NULL;
    }
    if (lpSentence[0] == ',')
    {
        return lpSentence + 1;
    }
    iTokenSize--;

    while(	*lpSentence != ',' &&
            *lpSentence != '\0' &&
            *lpSentence != '*'&& iTokenSize > 0)
    {
        *lpToken = *lpSentence;
        lpToken++;
        lpSentence++;
        iTokenSize--;
    }
    lpSentence++;
    *lpToken = '\0';
    return lpSentence;
}