#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "nmeaparser.h"

const float NMEAParser::knots2kmh = 1.854;

int NMEAParser::parseGGA(char* buff, int& ts, double& lat, double& lon, int& nosat, double& hdop)
{
    char x[10];
    double cL, cB;
    char *p;
    
    int l = strlen(buff);
    p = strtok(buff, ",");            // $GPGGA
    p = strtok(NULL, ",");            // time stamp

    ts = (p) ? atoi(p) : 0;
    if (l < 40) 
        return gpsInvalidFormat;
    p = strtok(NULL, ",");                // latitude
    if (p) 
    {
        strncpy(x, p, 2); 
        x[2] = 0;
        cB = atof(x);
        strcpy(x, &p[2]);
        cB += atof(x)/60;
        p = strtok(NULL, ",");
        if (*p == 'S') // southern latitudes
            cB = -cB;
        lat = cB;
    }
    p = strtok(NULL, ",");                // longitude
    if (p) 
    {
        strncpy(x, p, 3); x[3] = 0;
        cL = atof(x);
        strcpy(x, &p[3]);
        cL += atof(x)/60;
        p = strtok(NULL, ",");
        if (*p == 'W') // western longitudes
            cL = -cL;
        lon = cL;
    }
    p = strtok(NULL, ",");     // drop "Fix Quality"
    p = strtok(NULL, ",");     // Number of satellites
    nosat = (p) ? atoi(p) : 0;
    
    p = strtok(NULL, ",");     // HDOP
    hdop = (p) ? atof(p) : 0;

    return gpsOK;
}

int NMEAParser::parseRMC(char* buff, int& ts, double& lat, double& lon, double& vel, double& course, 
    char* ds)
{
    char x[256];
    double cL, cB;
    char *p;
    bool validPos;

    int l = strlen(buff);
    p = strtok(buff, ",");            // $GPRMC
    if (!p || strcmp(p, "$GPRMC")) 
        return gpsInvalidFormat;

    p = strtok(NULL, ",");            // time stamp
    ts = (p && strlen(p) >= 6) ? atoi(p) : 0;
    if (!p) 
        return gpsInvalidFormat;
    
    p = strtok(NULL, ",");            // recv warning V -> invalid data
    if (!p || strlen(p) != 1)
        return gpsInvalidFormat;
        
    validPos = (*p == 'A');
    if (l < 40)
        return gpsInvalidFormat;
        
    p = strtok(NULL, ",");                // latitude
    if (p) 
    {
        if (strlen(p) < 6)
            return gpsInvalidFormat;
        strncpy(x, p, 2); 
        x[2] = 0;
        cB = atof(x);
        strcpy(x, &p[2]);
        cB += atof(x)/60;
        p = strtok(NULL, ",");
        if (!p || strlen(p) != 1)
            return gpsInvalidFormat;
        if (*p == 'S') // southern latitudes
            cB = -cB;
        else if (*p != 'N')
            return gpsInvalidFormat;
        lat = cB;
    } else
        return gpsInvalidFormat;
    
    p = strtok(NULL, ",");                // longitude
    if (p) 
    {
        if (strlen(p) < 7)
            return gpsInvalidFormat;
        strncpy(x, p, 3); 
        x[3] = 0;
        cL = atof(x);
        strcpy(x, &p[3]);
        cL += atof(x)/60;
        p = strtok(NULL, ",");
        if (!p || strlen(p) != 1)
            return gpsInvalidFormat;
        if (*p == 'W') // western longitudes
            cL = -cL;
        else if (*p != 'E')
            return gpsInvalidFormat;
        lon = cL;
    } else
        return gpsInvalidFormat;
        
    // validity check
    if (!validPos) 
    {
        if (fabs(lat) > 88)
            return gpsNullPosition;
        else
            return gpsInvalidPosition;
    }
    
    p = strtok(NULL, ",");     // velocity kts
    if (p)
        vel = atof(p) * knots2kmh;
    else 
        return gpsInvalidFormat;
    
    p = strtok(NULL, ",");     // course
    if (p)
        course = atof(p);
    else
        return gpsInvalidFormat;
    
    p = strtok(NULL, ",");    // date
    if (!p || strlen(p) != 6) 
        return gpsInvalidFormat;
    if (ds) 
    {
        strncpy(ds, p, 6);
        ds[6] = 0;
    }
    
    return gpsOK;
}


