/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{ 
 * @addtogroup GSPModule GPS Module
 * @brief Process GPS information
 * @{ 
 *
 * @file       GPS.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      GPS module, handles GPS and NMEA stream
 * @see        The GNU Public License (GPL) Version 3
 *
 *****************************************************************************/
/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "openpilot.h"
#include "buffer.h"
#include "GPS.h"
#include <stdbool.h>
#include "NMEA.h"
#include "gpsposition.h"
#include "homelocation.h"
#include "gpstime.h"
#include "WorldMagModel.h"
#include "CoordinateConversions.h"

// constants/macros/typdefs
#define NMEA_BUFFERSIZE		128

#define GPS_TIMEOUT_MS 500

// Private functions

static bool GPS_copy_sentence_from_cbuffer(char *dest, uint32_t dest_len, cBuffer * rxBuffer);
static void gpsTask(void *parameters);
static void setHomeLocation(GPSPositionData * gpsData);

// Global variables

// Private constants
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE configMINIMAL_STACK_SIZE + 1500
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)

// Private types

// Private variables
static uint8_t gpsPort;
static xTaskHandle gpsTaskHandle;
static cBuffer gpsRxBuffer;
static char gpsRxData[512];
static char NmeaPacket[NMEA_BUFFERSIZE];

static uint32_t timeOfLastUpdateMs;
static uint32_t numUpdates;
static uint32_t numChecksumErrors;
static uint32_t numParsingErrors;

/**
 * Initialise the gps module
 * \return -1 if initialisation failed
 * \return 0 on success
 */
int32_t GPSInitialize(void)
{
	signed portBASE_TYPE xReturn;

	// TODO: Get gps settings object
	gpsPort = PIOS_COM_GPS;

	// Init input buffer size 512
	bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);

	// Start gps task
	xReturn = xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);

	return 0;
}

/**
 * gps task. Processes input buffer. It does not return.
 */
static void gpsTask(void *parameters)
{
	int32_t gpsRxOverflow = 0;
	char c;
	portTickType xDelay = 100 / portTICK_RATE_MS;
	GPSPositionData GpsData;
	uint32_t timeNowMs;

	// Loop forever
	while (1) {
		/* This blocks the task until there is something on the buffer */
		while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) {
			c = PIOS_COM_ReceiveBuffer(gpsPort);
			if (!bufferAddToEnd(&gpsRxBuffer, c)) {
				/* 
				 * The buffer is already full and we haven't found a valid NMEA sentence.
				 * Flush the buffer and note the overflow event.
				 */
				gpsRxOverflow++;
				bufferFlush(&gpsRxBuffer);
			} else {
				/* Grab the next available complete NMEA sentence from the Rx buffer */
				if (GPS_copy_sentence_from_cbuffer(NmeaPacket, sizeof(NmeaPacket), &gpsRxBuffer)) {
					/* Validate the checksum over the sentence */
					if (!NMEA_checksum(NmeaPacket)) {
						/* Invalid checksum.  May indicate dropped characters on Rx. */
						++numChecksumErrors;
					} else {
						/* Valid checksum, use this packet to update the GPS position */
						if (!NMEA_update_position(NmeaPacket)) {
							++numParsingErrors;
						} else {
							++numUpdates;
						}
						timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
					}
				}
			}
		}

		// Check for GPS timeout
		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
		if ((timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS) {
			GPSPositionGet(&GpsData);
			GpsData.Status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionSet(&GpsData);
		} else {
			// Had an update
			HomeLocationData home;
			HomeLocationGet(&home);

			GPSPositionGet(&GpsData);
			if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) {
				setHomeLocation(&GpsData);
			}
		}

		// Block task until next update
		vTaskDelay(xDelay);
	}
}

static bool GPS_copy_sentence_from_cbuffer(char *dest, uint32_t dest_len, cBuffer * rxBuffer)
{
	/* Throw away all initial characters from the Rx buffer that are not the NMEA start flag '$' */
	bool startFlag = false;
	while (rxBuffer->datalength && !startFlag) {
		if (bufferGetAtIndex(rxBuffer, 0) == '$') {
			startFlag = true;
		} else {
			bufferGetFromFront(rxBuffer);
		}
	}

	if (!startFlag) {
		/* No start of sentence located, bail out */
		return false;
	}

	/* rxBuffer is now positioned at the start of sentence marker */

	/* Start of sentence located, look for an end of sentence marker '\r\n' */
	bool endFlag = false;
	uint16_t end_pos;
	for (end_pos = 2; end_pos < rxBuffer->datalength; end_pos++) {
		// check for end of NMEA sentence '\r\n'
		if ((bufferGetAtIndex(rxBuffer, end_pos - 1) == '\r') && (bufferGetAtIndex(rxBuffer, end_pos) == '\n')) {
			/* Found the end of the packet */
			endFlag = true;
		}
	}

	if (!endFlag) {
		/* No end of sentence located, bail out and wait for more data */
		return false;
	}

	/*
	 * Our rxBuffer must look like this now:
	 *   [0]           = '$'
	 *   ...           = zero or more bytes of sentence payload
	 *   [end_pos - 1] = '\r'
	 *   [end_pos]     = '\n'
	 *
	 * Prepare to consume the sentence from the buffer
	 */
	uint32_t payload_len = end_pos - 2;	/* not counting the '$' or '\r\n' */

	/* Drop the start flag '$' */
	(void)bufferGetFromFront(rxBuffer);

	bool truncated = false;
	while (payload_len--) {
		if (dest_len > 1) {
			/* Put the payload data into the buffer as long as there is room */
			*dest = bufferGetFromFront(rxBuffer);
			dest++;
			dest_len--;
		} else {
			/* 
			 * No more room in the dest buffer.  Note that the sentence was truncated and
			 * continue to flush the rxBuffer.
			 */
			truncated = true;
			(void)bufferGetFromFront(rxBuffer);
		}
	}

	/* Drop the end marker '\r\n' */
	(void)bufferGetFromFront(rxBuffer);
	(void)bufferGetFromFront(rxBuffer);

	/* NULL terminate the dest buffer.  We left 1 byte at the end (see 'dest_len > 1' above) */
	*dest = '\0';

	return (!truncated);
}

static void setHomeLocation(GPSPositionData * gpsData)
{
	HomeLocationData home;
	HomeLocationGet(&home);
	GPSTimeData gps;
	GPSTimeGet(&gps);

	if (gps.Year >= 2000) {
		// Store LLA
		home.Latitude = gpsData->Latitude;
		home.Longitude = gpsData->Longitude;
		home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation;

		// Compute home ECEF coordinates and the rotation matrix into NED
		double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) };
		double ECEF[3];
		RneFromLLA(LLA, (float (*)[3])home.RNE);
		LLA2ECEF(LLA, ECEF);
		// TODO: Currently UAVTalk only supports float but these conversions use double
		// need to find out if they require that precision and if so extend UAVTAlk
		home.ECEF[0] = (int32_t) (ECEF[0] * 100);
		home.ECEF[1] = (int32_t) (ECEF[1] * 100);
		home.ECEF[2] = (int32_t) (ECEF[2] * 100);

		// Compute magnetic flux direction at home location
		WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]);

		home.Set = HOMELOCATION_SET_TRUE;
		HomeLocationSet(&home);
	}
}

/** 
  * @}
  * @}
  */