#include "openpilot.h"
#include "pios.h"
#include "NMEA.h"
#include "gpsposition.h"
#include "gpstime.h"
#include "gpssatellites.h"

// Debugging
//#define GPSDEBUG
#ifdef GPSDEBUG
#define NMEA_DEBUG_PKT		///< define to enable debug of all NMEA messages
#define NMEA_DEBUG_GGA		///< define to enable debug of GGA messages
#define NMEA_DEBUG_VTG		///< define to enable debug of VTG messages
#define NMEA_DEBUG_RMC		///< define to enable debug of RMC messages
#define NMEA_DEBUG_GSA		///< define to enable debug of GSA messages
#define NMEA_DEBUG_GSV		///< define to enable debug of GSV messages
#define NMEA_DEBUG_ZDA		///< define to enable debug of ZDA messages
#endif

/* Utility functions */
static float NMEA_real_to_float(char *nmea_real);
static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon);

/* NMEA sentence parsers */

struct nmea_parser {
	const char *prefix;
	 bool(*handler) (GPSPositionData * GpsData, char *sentence);
};

static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence);
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence);

static struct nmea_parser nmea_parsers[] = {
	{
	 .prefix = "GPGGA",
	 .handler = nmeaProcessGPGGA,
	 },
	{
	 .prefix = "GPVTG",
	 .handler = nmeaProcessGPVTG,
	 },
	{
	 .prefix = "GPGSA",
	 .handler = nmeaProcessGPGSA,
	 },
	{
	 .prefix = "GPRMC",
	 .handler = nmeaProcessGPRMC,
	 },
	{
	 .prefix = "GPZDA",
	 .handler = nmeaProcessGPZDA,
	 },
	{
	 .prefix = "GPGSV",
	 .handler = nmeaProcessGPGSV,
	 },
};

static struct nmea_parser *NMEA_find_parser_by_prefix(char *prefix)
{
	if (!prefix) {
		return (NULL);
	}

	for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
		struct nmea_parser *parser = &nmea_parsers[i];

		/* Use strcmp to check for exact equality over the entire prefix */
		if (!strcmp(prefix, parser->prefix)) {
			/* Found an appropriate parser */
			return (parser);
		}
	}

	/* No matching parser for this prefix */
	return (NULL);
}

/**
 * Computes NMEA sentence checksum
 * \param[in] Buffer for parsed nmea sentence
 * \return false checksum not valid
 * \return true checksum valid
 */
bool NMEA_checksum(char *nmea_sentence)
{
	uint8_t checksum_computed = 0;
	uint8_t checksum_received;

	while (*nmea_sentence != '\0' && *nmea_sentence != '*') {
		checksum_computed ^= *nmea_sentence;
		nmea_sentence++;
	}

	/* Make sure we're now pointing at the checksum */
	if (*nmea_sentence == '\0') {
		/* Buffer ran out before we found a checksum marker */
		return false;
	}

	/* Load the checksum from the buffer */
	checksum_received = strtol(nmea_sentence + 1, NULL, 16);

	//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum_computed);

	return (checksum_computed == checksum_received);
}

/**
 * Parses a complete NMEA sentence and updates the GPSPosition UAVObject
 * \param[in] An NMEA sentence with a valid checksum
 * \return true if the sentence was successfully parsed
 * \return false if any errors were encountered with the parsing
 */
bool NMEA_update_position(char *nmea_sentence)
{
	char *sentence = nmea_sentence;
	struct nmea_parser *parser;
	char *prefix;

	/* Find out what kind of NMEA packet we're dealing with */
	prefix = strsep(&sentence, ",");

	/* Check if we have a parser for this packet type */
	parser = NMEA_find_parser_by_prefix(prefix);

	if (!parser) {
		/* Valid but unhandled packet type */
		return false;
	}

	/* Found a matching parser for this packet type */

	/* Reject empty (but valid) packets without parsing */
	if (sentence[0] == ',' && sentence[1] == ',' && sentence[2] == ',') {
		/* Nothing to parse,  */
		return false;
	}

	/* Parse the sentence and update the GpsData object */

	GPSPositionData GpsData;
	GPSPositionGet(&GpsData);
	if (!parser->handler(&GpsData, sentence)) {
		/* Parse failed for valid checksum.  Do not update the UAVObject. */
		return false;
	}
	GPSPositionSet(&GpsData);

	/* Tell the caller what kind of packet we just parsed */
	return true;
}

/**
 * Parse an NMEA GPGGA sentence and update the given UAVObject
 * \param[in] A pointer to a GPSPosition UAVObject to be updated.
 * \param[in] An NMEA sentence with a valid checksum
 */
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",*";

#ifdef NMEA_DEBUG_GGA
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	// get UTC time [hhmmss.sss]
	tokens = strsep(&next, delimiter);
	//strcpy(GpsInfo.TimeOfFix,tokens);

	// next field: latitude
	// get latitude [DDMM.mmmmm]
	tokens = strsep(&next, delimiter);
	if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, tokens)) {
		return false;
	}
	// next field: N/S indicator
	// correct latitute for N/S
	tokens = strsep(&next, delimiter);
	if (tokens[0] == 'S')
		GpsData->Latitude = -GpsData->Latitude;

	// next field: longitude
	// get longitude [dddmm.mmmmm]
	tokens = strsep(&next, delimiter);
	if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, tokens)) {
		return false;
	}
	// next field: E/W indicator
	// correct longitude for E/W
	tokens = strsep(&next, delimiter);
	if (tokens[0] == 'W')
		GpsData->Longitude = -GpsData->Longitude;

	// next field: position fix status
	// position fix status
	// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
	// check for good position fix
	tokens = strsep(&next, delimiter);
	//if((tokens[0] != '0') || (tokens[0] != 0))
	//    GpsData.Updates++;

	// next field: satellites used
	// get number of satellites used in GPS solution
	tokens = strsep(&next, delimiter);
	GpsData->Satellites = atoi(tokens);

	// next field: HDOP (horizontal dilution of precision)
	tokens = strsep(&next, delimiter);

	// next field: altitude
	// get altitude (in meters mm.m)
	tokens = strsep(&next, delimiter);
	GpsData->Altitude = NMEA_real_to_float(tokens);

	// next field: altitude units, always 'M'
	tokens = strsep(&next, delimiter);

	// next field: geoid separation
	tokens = strsep(&next, delimiter);
	GpsData->GeoidSeparation = NMEA_real_to_float(tokens);

	// next field: separation units
	tokens = strsep(&next, delimiter);

	// next field: DGPS age
	tokens = strsep(&next, delimiter);

	// next field: DGPS station ID
	tokens = strsep(&next, delimiter);

	// next field: checksum
	tokens = strsep(&next, delimiter);

	return true;
}

/**
 * Parse an NMEA GPRMC sentence and update the given UAVObject
 * \param[in] A pointer to a GPSPosition UAVObject to be updated.
 * \param[in] An NMEA sentence with a valid checksum
 */
static bool nmeaProcessGPRMC(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",*";
	GPSTimeData gpst;
	GPSTimeGet(&gpst);

#ifdef NMEA_DEBUG_RMC
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	// get UTC time [hhmmss.sss]
	tokens = strsep(&next, delimiter);
	float hms = NMEA_real_to_float(tokens);
	gpst.Second = (int)hms % 100;
	gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
	gpst.Hour = (int)hms / 10000;

	// next field: Navigation receiver warning A = OK, V = warning
	tokens = strsep(&next, delimiter);

	// next field: latitude
	// get latitude [ddmm.mmmmm]
	tokens = strsep(&next, delimiter);
	if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, tokens)) {
		return false;
	}
	// next field: N/S indicator
	// correct latitute for N/S
	tokens = strsep(&next, delimiter);
	if (tokens[0] == 'S')
		GpsData->Latitude = -GpsData->Latitude;

	// next field: longitude
	// get longitude [dddmm.mmmmm]
	tokens = strsep(&next, delimiter);
	if (!NMEA_latlon_to_fixed_point(&GpsData->Longitude, tokens)) {
		return false;
	}
	// next field: E/W indicator
	// correct latitute for E/W
	tokens = strsep(&next, delimiter);
	if (tokens[0] == 'W')
		GpsData->Longitude = -GpsData->Longitude;

	// next field: speed (knots)
	// get speed in knots
	tokens = strsep(&next, delimiter);
	GpsData->Groundspeed = NMEA_real_to_float(tokens);
	GpsData->Groundspeed *= 0.51444;

	// next field: True course
	// get True course
	tokens = strsep(&next, delimiter);
	GpsData->Heading = NMEA_real_to_float(tokens);

	// next field: Date of fix
	// get Date of fix
	tokens = strsep(&next, delimiter);
	// TODO: Should really not use a float here to be safe
	float date = NMEA_real_to_float(tokens);
	gpst.Year = (int)date % 100;
	gpst.Month = (((int)date - gpst.Year) / 100) % 100;
	gpst.Day = (int)(date / 10000);
	gpst.Year += 2000;

	// next field: Magnetic variation
	tokens = strsep(&next, delimiter);

	// next field: E or W
	tokens = strsep(&next, delimiter);

	// next field: Mode: A=autonomous, D=differential, E=Estimated, N=not valid, S=Simulator
	tokens = strsep(&next, delimiter);

	// next field: checksum
	tokens = strsep(&next, delimiter);

	GPSTimeSet(&gpst);
	return true;
}

/**
 * Parse an NMEA GPVTG sentence and update the given UAVObject
 * \param[in] A pointer to a GPSPosition UAVObject to be updated.
 * \param[in] An NMEA sentence with a valid checksum
 */
static bool nmeaProcessGPVTG(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",*";

#ifdef NMEA_DEBUG_VTG
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	// get course (true north ref) in degrees [ddd.dd]
	tokens = strsep(&next, delimiter);

	// next field: 'T'
	tokens = strsep(&next, delimiter);

	// next field: course (magnetic north)
	// get course (magnetic north ref) in degrees [ddd.dd]
	tokens = strsep(&next, delimiter);
	// next field: 'M'
	tokens = strsep(&next, delimiter);

	// next field: speed (knots)
	// get speed in knots
	tokens = strsep(&next, delimiter);
	// next field: 'N'
	tokens = strsep(&next, delimiter);

	// next field: speed (km/h)
	// get speed in km/h
	tokens = strsep(&next, delimiter);

	// next field: 'K'
	tokens = strsep(&next, delimiter);

	// next field: checksum
	tokens = strsep(&next, delimiter);

	return true;
}

/**
 * Parse an NMEA GPZDA sentence and update the @ref GPSTime object
 * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
 * \param[in] An NMEA sentence with a valid checksum
 */
static bool nmeaProcessGPZDA(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",*";

#ifdef NMEA_DEBUG_VZDA
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	GPSTimeData gpst;
	GPSTimeGet(&gpst);

	tokens = strsep(&next, delimiter);
	float hms = NMEA_real_to_float(tokens);

	gpst.Second = (int)hms % 100;
	gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
	gpst.Hour = (int)hms / 10000;

	tokens = strsep(&next, delimiter);
	gpst.Day = (uint8_t) NMEA_real_to_float(next);

	tokens = strsep(&next, delimiter);
	gpst.Month = (uint8_t) NMEA_real_to_float(next);

	tokens = strsep(&next, delimiter);
	gpst.Year = (uint16_t) NMEA_real_to_float(next);

	GPSTimeSet(&gpst);

	return true;
}

static GPSSatellitesData gsv_partial;
/* Bitmaps of which sentences we're looking for to allow us to handle out-of-order GSVs */
static uint8_t gsv_expected_mask;
static uint8_t gsv_processed_mask;
/* Error counters */
static uint16_t gsv_incomplete_error;
static uint16_t gsv_duplicate_error;

static bool nmeaProcessGPGSV(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",";

#ifdef NMEA_DEBUG_GSV
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	/* Drop the checksum */
	char *tmp = sentence;
	char *tmp_delim = "*";
	next = strsep(&tmp, tmp_delim);

	/* # of sentences in full GSV data set */
	tokens = strsep(&next, delimiter);
	uint8_t total_sentences = atoi(tokens);
	if ((total_sentences < 1) || (total_sentences > 8)) {
		return false;
	}

	/* Sentence number within the current GSV data set */
	tokens = strsep(&next, delimiter);
	uint8_t current_sentence = atoi(tokens);
	if (current_sentence < 1) {
		return false;
	}

	/* # of satellites currently in view */
	tokens = strsep(&next, delimiter);
	gsv_partial.SatsInView = atoi(tokens);

	/* Find out if this is the first sentence in the GSV set */
	if (current_sentence == 1) {
		if (gsv_expected_mask != gsv_processed_mask) {
			/* We are starting over when we haven't yet finished our previous GSV group */
			gsv_incomplete_error++;
		}

		/* First GSV sentence in the sequence, reset our expected_mask */
		gsv_expected_mask = (1 << total_sentences) - 1;
	}

	uint8_t current_sentence_id = (1 << (current_sentence - 1));
	if (gsv_processed_mask & current_sentence_id) {
		/* Duplicate sentence in this GSV set */
		gsv_duplicate_error++;
	} else {
		/* Note that we've seen this sentence */
		gsv_processed_mask |= current_sentence_id;
	}

	/* Make sure this sentence can fit in our GPSSatellites object */
	if ((current_sentence * 4) <= NELEMENTS(gsv_partial.PRN)) {
		/* Process 4 blocks of satellite info */
		for (uint8_t i = 0; next && i < 4; i++) {
			uint8_t sat_index = ((current_sentence - 1) * 4) + i;

			/* PRN number */
			tokens = strsep(&next, delimiter);
			gsv_partial.PRN[sat_index] = atoi(tokens);

			/* Elevation */
			tokens = strsep(&next, delimiter);
			gsv_partial.Elevation[sat_index] = NMEA_real_to_float(tokens);

			/* Azimuth */
			tokens = strsep(&next, delimiter);
			gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(tokens);

			/* SNR */
			tokens = strsep(&next, delimiter);
			gsv_partial.SNR[sat_index] = atoi(tokens);
		}
	}

	/* Find out if we're finished processing all GSV sentences in the set */
	if ((gsv_expected_mask != 0) && (gsv_processed_mask == gsv_expected_mask)) {
		/* GSV set has been fully processed.  Update the GPSSatellites object. */
		GPSSatellitesSet(&gsv_partial);
		memset((void *)&gsv_partial, 0, sizeof(gsv_partial));
		gsv_expected_mask = 0;
		gsv_processed_mask = 0;
	}

	return true;
}

/**
 * Parse an NMEA GPGSA sentence and update the given UAVObject
 * \param[in] A pointer to a GPSPosition UAVObject to be updated.
 * \param[in] An NMEA sentence with a valid checksum
 */
static bool nmeaProcessGPGSA(GPSPositionData * GpsData, char *sentence)
{
	char *next = sentence;
	char *tokens;
	char *delimiter = ",*";

#ifdef NMEA_DEBUG_GSA
	PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "$%s\r\n", sentence);
#endif

	// next field: Mode
	// Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
	tokens = strsep(&next, delimiter);

	// next field: Mode
	// Mode: 1=Fix not available, 2=2D, 3=3D
	tokens = strsep(&next, delimiter);
	switch (atoi(tokens)) {
	case 1:
		GpsData->Status = GPSPOSITION_STATUS_NOFIX;
		break;
	case 2:
		GpsData->Status = GPSPOSITION_STATUS_FIX2D;
		break;
	case 3:
		GpsData->Status = GPSPOSITION_STATUS_FIX3D;
		break;
	default:
		/* Unhandled */
		return false;
		break;
	}

	// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);
	tokens = strsep(&next, delimiter);

	// next field: PDOP
	tokens = strsep(&next, delimiter);
	GpsData->PDOP = NMEA_real_to_float(tokens);

	// next field: HDOP
	tokens = strsep(&next, delimiter);
	GpsData->HDOP = NMEA_real_to_float(tokens);

	// next field: VDOP
	tokens = strsep(&next, delimiter);
	GpsData->VDOP = NMEA_real_to_float(tokens);

	// next field: checksum
	tokens = strsep(&next, delimiter);

	return true;
}

/* Parse a number encoded in a string of the format:
 *   [-]NN.nnnnn
 * into a signed whole part and an unsigned fractional part.
 * The fract_units field indicates the units of the fractional part as
 *   1 whole = 10^fract_units fract
 */
static bool NMEA_parse_real(int32_t * whole, uint32_t * fract, uint8_t * fract_units, char *field)
{
	char *s = field;
	char *field_w;
	char *field_f;

	PIOS_DEBUG_Assert(whole);
	PIOS_DEBUG_Assert(fract);
	PIOS_DEBUG_Assert(fract_units);

	field_w = strsep(&s, ".");
	field_f = s;

	*whole = strtol(field_w, NULL, 10);

	if (field_w) {
		/* decimal was found so we may have a fractional part */
		*fract = strtoul(field_f, NULL, 10);
		*fract_units = strlen(field_f);
	} else {
		/* no decimal was found, fractional part is zero */
		*fract = 0;
		*fract_units = 0;
	}

	return true;
}

/*
 * This function only exists to deal with a linking
 * failure in the stdlib function strtof().  This
 * implementation does not rely on the _sbrk() syscall
 * like strtof() does.
 */

static float NMEA_real_to_float(char *nmea_real)
{
	int32_t whole;
	uint32_t fract;
	uint8_t fract_units;

	/* Sanity checks */
	PIOS_DEBUG_Assert(nmea_real);

	if (!NMEA_parse_real(&whole, &fract, &fract_units, nmea_real)) {
		return false;
	}

	/* Convert to float */
	return (((float)whole) + fract * pow(10, -fract_units));
}

/*
 * Parse a field in the format:
 *    DD[D]MM.mmmm[mm]
 * into a fixed-point representation in units of (degrees * 1e-7)
 */
static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon)
{
	int32_t num_DDDMM;
	uint32_t num_m;
	uint8_t units;

	/* Sanity checks */
	PIOS_DEBUG_Assert(nmea_latlon);
	PIOS_DEBUG_Assert(latlon);

	if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) {
		return false;
	}

	/* scale up the mmmm[mm] field apropriately depending on # of digits */
	switch (units) {
	case 0:
		/* no digits, value is zero so no scaling */
		break;
	case 1:		/* m       */
		num_m *= 1e6;	/* m000000 */
		break;
	case 2:		/* mm      */
		num_m *= 1e5;	/* mm00000 */
		break;
	case 3:		/* mmm     */
		num_m *= 1e4;	/* mmm0000 */
		break;
	case 4:		/* mmmm    */
		num_m *= 1e3;	/* mmmm000 */
		break;
	case 5:		/* mmmmm   */
		num_m *= 1e2;	/* mmmmm00 */
		break;
	case 6:		/* mmmmmm  */
		num_m *= 1e1;	/* mmmmmm0 */
		break;
	default:
		/* unhandled format */
		num_m = 0;
		break;
	}

	*latlon = (num_DDDMM / 100) * 1e7;	/* scale the whole degrees */
	*latlon += (num_DDDMM % 100) * 1e7 / 60;	/* add in the scaled decimal whole minutes */
	*latlon += num_m / 60;	/* add in the scaled decimal fractional minutes */

	return true;
}