/*

Parse GPS data.

Copyright (C) 2019, ENSIMAG students
This file is based on LoRaMAC-node from Semtech, under Revised BSD License.

*/
#ifdef GPS

#include "gps.h"

#include <mutex.h>

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


// Various type of NMEA data we can receive with the GPS.
static const char NmeaDataTypeGPGGA[] = "GPGGA";
static const char NmeaDataTypeGPRMC[] = "GPRMC";
// TODO process messages GPGLL : Latitude, longitude, UTC time of position fix and status.
// TODO process messages GPGSA : GPS receiver operating mode, satellites used in the position solution, and DOP values.
// TODO process messages GPGSV : The number of GPS satellites in view satellite ID numbers, elevation, azimuth, and SNR values.
// TODO process messages GPMSS : Signal-to-noise ratio, signal strength, frequency, and bit rate from a radio-beacon receiver.
// TODO process messages GPRMC : Time, date, position, course and speed data.
// TODO process messages GPVTG : Course and speed information relative to the ground.
// TODO process messages GPZDA : PPS timing message (synchronized to PPS).


// TODO process messages from BD ou GB - Beidou ; GA - Galileo ; GL - GLONASS.


// Value used for the conversion of the position from DMS to decimal.
static const int32_t MaxNorthPosition = 8388607;  // 2^23 - 1
static const int32_t MaxSouthPosition = 8388608;  // -2^23
static const int32_t MaxEastPosition  = 8388607;  // 2^23 - 1
static const int32_t MaxWestPosition  = 8388608;  // -2^23

// GPS data in ASCII and numrical formats.
gps_nmea_t gps_nmea;
gps_data_t gps_data;

// Mutex that protect GPS data.
static mutex_t gps_mutex = MUTEX_INIT;


// Convert a nibble to hex char.
static int8_t nibble_to_hex(uint8_t a)
{
    if (a < 10)
        return '0' + a;
    else if (a < 16)
        return 'A' + (a - 10);
    else
        return '?';
}



// Convert GPS positions from double to binary values.
static void positions_to_binary(void)
{
    long double temp;

    if (gps_data.latitude >= 0) { // North
        temp = gps_data.latitude * MaxNorthPosition;
        gps_data.latitude_bin = temp / 90;
    } else {                      // South
        temp = gps_data.latitude * MaxSouthPosition;
        gps_data.latitude_bin = temp / 90;
    }

    if (gps_data.longitude >= 0) { // East
        temp = gps_data.longitude * MaxEastPosition;
        gps_data.longitude_bin = temp / 180;
    } else {                       // West
        temp = gps_data.longitude * MaxWestPosition;
        gps_data.longitude_bin = temp / 180;
    }
}


// Convert GPS positions from ASCII to double values.
static void positions_to_double(void)
{
    double valueTmp1, valueTmp2, valueTmp3, valueTmp4;

    // Convert the latitude from ASCII to uint8_t values.
    for (int i = 0 ; i < 10 ; i++ )
        gps_nmea.latitude[i] = gps_nmea.latitude[i] & 0xF;

    // Convert latitude from degree/minute/second (DMS) format into decimal.
    valueTmp1 = (double)gps_nmea.latitude[0] * 10.0 + (double)gps_nmea.latitude[1];
    valueTmp2 = (double)gps_nmea.latitude[2] * 10.0 + (double)gps_nmea.latitude[3];
    valueTmp3 = (double)gps_nmea.latitude[5] * 1000.0 + (double)gps_nmea.latitude[6] * 100.0 +
                (double)gps_nmea.latitude[7] * 10.0 + (double)gps_nmea.latitude[8];

    gps_data.latitude = valueTmp1 + ((valueTmp2 + (valueTmp3 * 0.0001)) / 60.0);
    if (gps_nmea.latitude_pole[0] == 'S')
        gps_data.latitude *= -1;

    // Convert the longitude from ASCII to uint8_t values.
    for (int i = 0 ; i < 10 ; i++)
        gps_nmea.longitude[i] = gps_nmea.longitude[i] & 0xF;

    // Convert longitude from degree/minute/second (DMS) format into decimal.
    valueTmp1 = (double)gps_nmea.longitude[0] * 100.0 + (double)gps_nmea.longitude[1] * 10.0 + (double)gps_nmea.longitude[2];
    valueTmp2 = (double)gps_nmea.longitude[3] * 10.0 + (double)gps_nmea.longitude[4];
    valueTmp3 = (double)gps_nmea.longitude[6] * 1000.0 + (double)gps_nmea.longitude[7] * 100;
    valueTmp4 = (double)gps_nmea.longitude[8] * 10.0 + (double)gps_nmea.longitude[9];

    gps_data.longitude = valueTmp1 + (valueTmp2 / 60.0) + (((valueTmp3 + valueTmp4) * 0.0001) / 60.0);
    if (gps_nmea.longitude_pole[0] == 'W')
        gps_data.longitude *= -1;
}



// Calculates the checksum for a NMEA sentence (and return the position of the
// checksum in the sentence).
static int32_t nmea_checksum(int8_t *nmeaStr, int32_t nmeaStrSize, int8_t *checksum)
{
    int i = 0;
    uint8_t checkNum = 0;

    if ((nmeaStr == NULL) || (checksum == NULL) || (nmeaStrSize <= 1))
        return -1;

    if (nmeaStr[i] == '$')
        i += 1;  // Skip the first '$' if necessary.

    while (nmeaStr[i] != '*') {
        checkNum ^= nmeaStr[i];  // XOR until '*' or max length is reached.
        i += 1;
        if (i >= nmeaStrSize)
            return -1;
    }

    // Convert checksum value to 2 hexadecimal characters.
    checksum[0] = nibble_to_hex(checkNum / 16); // upper nibble
    checksum[1] = nibble_to_hex(checkNum % 16); // lower nibble
    return i + 1;
}


// Calculate the checksum of a NMEA frame and compare it to the checksum that
// is present at the end of it (return true if it matches).
static bool nmea_validate_checksum(int8_t *serialBuff, int32_t buffSize)
{
    int32_t checksumIndex;
    int8_t checksum[2]; // 2 characters to calculate NMEA checksum.

    checksumIndex = nmea_checksum(serialBuff, buffSize, checksum);
    if (checksumIndex < 0)
        return false;

    if (checksumIndex >= (buffSize - 2))
        return false;  // Not enough char in the serial buffer to read checksum.

    return  // Check the checksum.
        (serialBuff[checksumIndex + 0] == checksum[0]) &&
        (serialBuff[checksumIndex + 1] == checksum[1]);
}


// Format GPS data.
static void format_gps_data(void)
{
    positions_to_double();
    positions_to_binary();

    if (gps_data.has_fix)
        gps_data.altitude = atoi(gps_nmea.altitude);
}



// Read a field from RX buffer.
#define READ_FIELD(field, i, rxBuffer, maxSize)       \
{                                                     \
    uint8_t __fs = 0;                                 \
    while ((rxBuffer)[(i) + __fs++] != ',')           \
        if (__fs > (maxSize)) return GPS_FAIL;        \
    for (uint8_t __j = 0; __j < __fs; __j++, (i)++)   \
        (field)[__j] = (rxBuffer)[i];                 \
}

// Read a field from RX buffer.
#define SKIP_FIELD(i, rxBuffer, maxSize)              \
{                                                     \
    uint8_t __fs = 0;                                 \
    while ((rxBuffer)[(i) + __fs++] != ',')           \
        if (__fs > (maxSize)) return GPS_FAIL;        \
    (i) += __fs;                                      \
}


// Parse a GPGGA message.
static uint8_t parse_GPGGA(uint8_t i, int8_t *rxBuffer)
{
    // NmeaUtcTime.
    SKIP_FIELD(i, rxBuffer, 11);
    // NmeaLatitude.
    READ_FIELD(gps_nmea.latitude, i, rxBuffer, 10);
    // NmeaLatitudePole.
    READ_FIELD(gps_nmea.latitude_pole, i, rxBuffer, 2);
    // NmeaLongitude.
    READ_FIELD(gps_nmea.longitude, i, rxBuffer, 11);
    // NmeaLongitudePole.
    READ_FIELD(gps_nmea.longitude_pole, i, rxBuffer, 2);
    // NmeaFixQuality.
    READ_FIELD(gps_nmea.fix_quality, i, rxBuffer, 2);
    // NmeaSatelliteTracked.
    SKIP_FIELD(i, rxBuffer, 3);
    // NmeaHorizontalDilution.
    SKIP_FIELD(i, rxBuffer, 6);
    // NmeaAltitude.
    READ_FIELD(gps_nmea.altitude, i, rxBuffer, 8);
    // NmeaAltitudeUnit.
    SKIP_FIELD(i, rxBuffer, 2);
    // NmeaHeightGeoid.
    SKIP_FIELD(i, rxBuffer, 8);
    // NmeaHeightGeoidUnit.
    SKIP_FIELD(i, rxBuffer, 2);

    gps_data.has_fix = (gps_nmea.fix_quality[0] > 0x30);
    format_gps_data();

    return GPS_SUCCESS;
}


// Parse a GPRMC message.
static uint8_t parse_GPRMC(uint8_t i, int8_t *rxBuffer)
{
    // NmeaUtcTime.
    SKIP_FIELD(i, rxBuffer, 11);
    // NmeaDataStatus.
    READ_FIELD(gps_nmea.fix_quality, i, rxBuffer, 2);
    // NmeaLatitude.
    READ_FIELD(gps_nmea.latitude, i, rxBuffer, 10);
    // NmeaLatitudePole.
    READ_FIELD(gps_nmea.latitude_pole, i, rxBuffer, 2);
    // NmeaLongitude.
    READ_FIELD(gps_nmea.longitude, i, rxBuffer, 11);
    // NmeaLongitudePole.
    READ_FIELD(gps_nmea.longitude_pole, i, rxBuffer, 2);
    // NmeaSpeed.
    SKIP_FIELD(i, rxBuffer, 8);
    // NmeaDetectionAngle.
    SKIP_FIELD(i, rxBuffer, 8);
    // NmeaDate.
    SKIP_FIELD(i, rxBuffer, 8);

    gps_data.has_fix = (gps_nmea.fix_quality[0] == 0x41);
    format_gps_data();

    return GPS_SUCCESS;
}


// Parse GPS data.
uint8_t gps_parse_data(int8_t *rxBuffer, int32_t rxBufferSize)
{
    if (!nmea_validate_checksum(rxBuffer, rxBufferSize))
        return GPS_FAIL;

    uint8_t i = 1;
    READ_FIELD(gps_nmea.data_type, i, rxBuffer, 6);

    if (strncmp(gps_nmea.data_type, NmeaDataTypeGPGGA, 5) == 0)
        return parse_GPGGA(i, rxBuffer);
    else if (strncmp(gps_nmea.data_type, NmeaDataTypeGPRMC, 5) == 0)
        return parse_GPRMC(i, rxBuffer);
    else
        return GPS_FAIL;
}


// Get the lastest GPS position in binary format.
uint8_t gps_get_binary(int32_t *lat, int32_t *lon, int16_t *alt)
{
    mutex_lock(&gps_mutex);

    uint8_t status = gps_data.has_fix ? GPS_SUCCESS : GPS_FAIL;
    if (!gps_data.has_fix)
        gps_reset_data();

    *lat = gps_data.latitude_bin;
    *lon = gps_data.longitude_bin;
    *alt = gps_data.altitude;

    mutex_unlock(&gps_mutex);
    return status;
}


// Reset GPS data.
void gps_reset_data(void)
{
    gps_data.has_fix = false;
    gps_data.altitude = 0xFFFF;

    gps_data.latitude = 0;
    gps_data.longitude = 0;

    gps_data.latitude_bin = 0;
    gps_data.longitude_bin = 0;
}

#endif