//
// Created by xemon on 01.09.22.
//
#include "Nmea0183Parser.h"
#include "AsciiStringAssmeblingUtils.h"
#include "AsciiStringParsingUtils.h"
#include "time.h"
#include <string.h>
//#include "printf.h"

//double specifAtCommandGNSS;
/*
static char *strnstr(const char *haystack, const char *needle, size_t len)
{
    int i;
    size_t needle_len;

    if (0 == (needle_len = strnlen(needle, len)))
        return (char *)haystack;

    for (i=0; i<=(int)(len-needle_len); i++)
    {
        if ((haystack[0] == needle[0]) &&
            (0 == strncmp(haystack, needle, needle_len)))
            return (char *)haystack;

        haystack++;
    }
    return NULL;
}
*/
#define Nmea0183ParseShortCharsDecimalNumber(STR, LEN) (uint16_t) iAsciiStringParseUnsignedLongDecimalNumber(STR,((STR)+(LEN)))

#define Nmea0183ParseDouble(STR, LEN) dAsciiStringParseDouble(STR,((STR)+(LEN)))

#define Nmea0183SkipSpace(STR, STR_END)  xAsciiStringSkipSpace(STR,STR_END)

#define Nmea0183SkipToChar(STR, STR_END, SYMBOL) xAsciiStringSeekChar(STR,STR_END,SYMBOL)

#define Nmea0183ParseNextPortion(BEGIN, DIV, STR_END) iAsciiStringMoveToNextParsingBlock(BEGIN,DIV,STR_END,',')

#define Nmea0183ParseHexByte(STR) iAsciiStringParseHexByte(STR)

bool Nmea0183ParseTime(char *utcString, char const *utcStringEnd, tNmeaTimeRmc *time) {

    uint32_t len = utcStringEnd - utcString;
    uint8_t charsLeft = len - 7;

    if (len > 10 || len < 9) {
        return false;
    }

    if (utcString[6] != '.') {
        return false;
    }

    time->hour = Nmea0183ParseShortCharsDecimalNumber(utcString + 0, 2);
    time->minute = Nmea0183ParseShortCharsDecimalNumber(utcString + 2, 2);
    time->second = Nmea0183ParseShortCharsDecimalNumber(utcString + 4, 2);

    time->millisecond = Nmea0183ParseShortCharsDecimalNumber(utcString + 7, charsLeft);

    charsLeft = 3 - charsLeft;

    while (charsLeft) {
        time->millisecond *= 10;
        --charsLeft;
    }

    return true;
}

bool Nmea0183ParseDate(char *utcString, char const *utcStringEnd, tNmeaDateRmc *date) {

    uint32_t len = utcStringEnd - utcString;

    if (len != 6) {
        return false;
    }

    date->day = Nmea0183ParseShortCharsDecimalNumber(utcString + 0, 2);
    date->month = Nmea0183ParseShortCharsDecimalNumber(utcString + 2, 2);
    date->year = Nmea0183ParseShortCharsDecimalNumber(utcString + 4, 2);

    return true;
}

uint8_t uNmea0183Checksum(const char *str, size_t len) {
    char *end = (char *) (str + len);

    unsigned char result;
    result = 0;
    str++;

    while ((str != end) && (*str != '*') && (*str != '\0'))
        result ^= *str++;

    return result;
}


void vNmea0183Sign(char *str, size_t *strLen) {
    uint8_t crc = uNmea0183Checksum(str, *strLen);
    vAsciiStringAddChar(str, strLen, '*');
    vAsciiStringAddByteAsHex(str, strLen, crc);
}

bool bNmea0183IsRmcString(char *nmeaString, size_t len) {
    if (nmeaString[0] != '$') {
        return false;
    }

    return strnstr(nmeaString, "RMC", len);
}

uint32_t iNmea0183TimestampFromRmc(tNmeaRmc *env) {
    struct tm time;
    time.tm_hour = env->time.hour;
    time.tm_min = env->time.minute;
    time.tm_sec = env->time.second;

    time.tm_mday = env->date.day;
    time.tm_mon = env->date.month - 1;
    if(env->date.year>60){
        time.tm_year = (1900 - 1900) + env->date.year;
    }else{
        time.tm_year = (2000 - 1900) + env->date.year;
    }

    return mktime(&time);
}

static double nmeaLocationToDeg(double dec) {
    double _dec = dec;
    int deg = (int) (_dec / 100);
    int min = (int) (_dec) - (deg * 100);

    double sec = (double) (_dec - min - 100 * deg) * 60.0;

    return deg + min / 60.0 + sec / 3600.0;
}


tLocationPointInDegDouble iNmea0183_DegLocationDoubleFromRmc(tNmeaRmc *env) {
    tLocationPointInDegDouble result;
    result.lat = nmeaLocationToDeg(env->location.latitude);
    result.lon = nmeaLocationToDeg(env->location.longitude);

    if (env->location.nsIndicator == 'S' || env->location.nsIndicator == 's') {
        result.lat *= -1.0;
    }

    if (env->location.ewIndicator == 'W' || env->location.ewIndicator == 'w') {
        result.lon *= -1.0;
    }
    return result;
}

tLocationPointInDeg iNmea0183_DegLocationFromRmc(tNmeaRmc *env) {
    tLocationPointInDeg result;
    result.lat = (float) nmeaLocationToDeg(env->location.latitude);
    result.lon = (float) nmeaLocationToDeg(env->location.longitude);

    if (env->location.nsIndicator == 'S' || env->location.nsIndicator == 's') {
        result.lat *= -1.0f;
    }

    if (env->location.ewIndicator == 'W' || env->location.ewIndicator == 'w') {
        result.lon *= -1.0f;
    }
    return result;
}


bool bNmea0183IsValidString(char *nmeaString, size_t len) {
        char *dataEndPos = strnstr((char *) nmeaString, "*", len);
        uint8_t checksumCalculated = uNmea0183Checksum(nmeaString, dataEndPos - nmeaString);
        ++dataEndPos;
        uint8_t checksumInString = Nmea0183ParseHexByte(dataEndPos);
        return checksumCalculated == checksumInString;
}

bool bNmea0183ParseRMC(char *rmcString, size_t len, tNmeaRmc *result) {

    char *end = rmcString + len;
    char *front = rmcString;
    char *nextDivider = front - 1;

    result->status = 'V';
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        if (!Nmea0183ParseTime(front, nextDivider, &result->time)) {
            return false;
        }
    } else {
        result->time.second = 0;
        result->time.minute = 0;
        result->time.hour = 0;
        result->time.millisecond = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->status = *front;
    } else {
        result->status = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.latitude = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->location.latitude = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.nsIndicator = *front;
    } else {
        result->location.nsIndicator = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.longitude = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->location.longitude = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.ewIndicator = *front;
    } else {
        result->location.ewIndicator = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->knotVelocity = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->knotVelocity = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->headingAngle = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->headingAngle = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        if (!Nmea0183ParseDate(front, nextDivider, &result->date)) {
            return false;
        }
    } else {
        result->date.year = 0;
        result->date.month = 0;
        result->date.day = 0;
    }


    return true;
}

bool bNmea0183IsGgaString(char *nmeaString, size_t len) {
    if (nmeaString[0] != '$') {
        return false;
    }

    return strnstr(nmeaString, "GGA", len);
}

bool bNmea0183ParseGGA(char *ggaString, size_t len, tNmeaGga *result){
    char *end = ggaString + len;
    char *front = ggaString;
    char *nextDivider = front-1;
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        if (!Nmea0183ParseTime(front, nextDivider, &result->time)) {
            return false;
        }
    } else {
        result->time.second = 0;
        result->time.minute = 0;
        result->time.hour = 0;
        result->time.millisecond = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.latitude = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->location.latitude = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.nsIndicator = *front;
    } else {
        result->location.nsIndicator = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.longitude = Nmea0183ParseDouble(front, nextDivider - front);
    } else {
        result->location.longitude = 0;
    }

    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->location.ewIndicator = *front;
    } else {
        result->location.ewIndicator = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->qual = Nmea0183ParseShortCharsDecimalNumber(front,nextDivider - front);
    } else {
        result->qual = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->sats = Nmea0183ParseShortCharsDecimalNumber(front,nextDivider - front);
    } else {
        result->sats = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->hdop = Nmea0183ParseDouble(front,nextDivider - front);
    } else {
        result->hdop = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->altitude = Nmea0183ParseDouble(front,nextDivider - front);
    } else {
        result->altitude = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->altitudeUnit = *front;
    } else {
        result->altitude = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->undulation = Nmea0183ParseDouble(front,nextDivider - front);
    } else {
        result->undulation = 0;
    }
    if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
        result->undulationUnit = *front;
    } else {
        result->undulationUnit = 0;
    }
    if(result->qual == DIFF){
        if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
            result->ageSec = Nmea0183ParseShortCharsDecimalNumber(front,nextDivider - front);
        } else {
            result->ageSec = 0;
        }
        if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) {
            result->stnID = Nmea0183ParseShortCharsDecimalNumber(front,nextDivider - front);
        } else {
            result->stnID = 0;
        }
    } else{
        result->ageSec = 0;
        result->stnID = 0;
    }
    return true;
}