/**
 * Author : Asanga Udugama (adu@comnets.uni-bremen.de)
 * Date : 14-Mar-2004
 * Function : 	- To read an output from a given file (device)
 *		- Assume this output is a GPS NMEA format
 *		- Get Longitude, Latitude and Altitude
 *		- Send a UDP packet with these values
 *
 * NMEA Format at http://www.grc.nasa.gov/WWW/MAEL/ag/agnmea1.htm
 *
 */
#include <sys/types.h>
#include <sys/socket.h>
#include <stdio.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <string.h>

#define TRUE	1
#define FALSE	0

FILE *fpGPSDevice;
int socketID;
char readBuffer[1024];
char packetBuffer[1024];
float longitude, latitude, altitude;
struct sockaddr_in serverIPAddr;

/**
 * Function to return the given token (by number) from a buffer where
 * each token is separated by a comma.
 */
char *getToken(char *buffer, int tokenNum, char *retBuffer, int retBufSize) {
	int i;
	char *ptr1, *ptr2;

	i = 0;
	ptr1 = buffer;
	while((ptr2 = strstr(ptr1, ",")) != NULL) {

		i++;

		if(i == tokenNum) {
			if((ptr2 - ptr1) > retBufSize) {
				return NULL;
			}
			strncpy(retBuffer, ptr1, (ptr2 - ptr1));
			*(retBuffer + (ptr2 - ptr1)) = '\0';
			return retBuffer;
		}

		if(i > tokenNum) {
			return NULL;
		}

		ptr1 = ptr2 + 1;
	}
	return NULL;
}

/**
 * Print the usage
 */
int printUsage(char *argv[]) {
	printf("\nTool to generate UDP packets that contain the\n");
	printf("GPS values (latitude, longitude and altitude)\n");
	printf("which are read from a file or GPS device. These\n");
	printf("values are assumed to be in the NMEA format.\n\n");
	printf("Usage : %s GPS-file IP-Addr IP-Port\n\n", argv[0]);
	printf("        where GPS-file is the GPS file or device,\n");
	printf("        IP-Addr and IP-Port are the IP address\n");
	printf("        and port to which the packet must be\n");
	printf("        sent.\n");
}

/**
 * Open the GPS file (device)
 */
int openGPSStream(int argc, char *argv[]) {
	if(argc != 4) {
		printf("Invald number of parameters\n");
		printUsage(argv);
		exit(1);
	}

	if((fpGPSDevice = fopen(argv[1], "r")) == NULL) {
		printf("Cannot open the device %s \n", argv[1]);
		printUsage(argv);
		exit(1);
	}
}

/**
 * Open the UDP socket
 */
int openUDPSocket(int argc, char *argv[])  {
	int portnum;

	if((socketID = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
		printf("Cannot open socket \n");
		printUsage(argv);
		fclose(fpGPSDevice);
		exit(1);
	}

	bzero(&serverIPAddr, sizeof(serverIPAddr));
  	serverIPAddr.sin_family = AF_INET;

  	if(inet_pton(AF_INET, argv[2], &serverIPAddr.sin_addr) < 0) {
		printf("Invalid IP address %s \n", argv[2]);
		printUsage(argv);
		close(socketID);
		fclose(fpGPSDevice);
		exit(1);
	}

  	if(sscanf(argv[3], "%d", &portnum) == 0) {
		printf("Invalid port number %s \n", argv[3]);
		printUsage(argv);
		fclose(fpGPSDevice);
		close(socketID);
		exit(1);
	}
	serverIPAddr.sin_port = htons(portnum);
}

/**
 * Read a line from the GPS file
 */
char *readGPSStream() {
	return (fgets(readBuffer, 1000, fpGPSDevice));
}

/**
 * extract the required GPS values from the buffer
 */
int extractGPSValues() {
	char extractBuffer[32];

	if(strstr(readBuffer, "GPGGA") == NULL) {
		return FALSE;
	}

	// get latitude
	if(getToken(readBuffer, 3, extractBuffer, 32) == NULL) {
		return FALSE;
	}
	if(sscanf(extractBuffer, "%f", &latitude) == 0) {
		return FALSE;
	}

	// get longitude
	if(getToken(readBuffer, 5, extractBuffer, 32) == NULL) {
		return FALSE;
	}
	if(sscanf(extractBuffer, "%f", &longitude) == 0) {
		return FALSE;
	}


	// get altitude
	if(getToken(readBuffer, 10, extractBuffer, 32) == NULL) {
		return FALSE;
	}
	if(sscanf(extractBuffer, "%f", &altitude) == 0) {
		return FALSE;
	}
	//printf("long %f lat %f alt %f\n", longitude, latitude, altitude);

	return TRUE;
}

/**
 * build the UDP packet
 */
int buildUDPPacket() {
	int rem, pad;
	char blanks[5] = "    ";

	sprintf(packetBuffer,"%08.06f %08.06f %08.06f ", latitude,
			longitude, altitude);
	if(( rem = strlen(packetBuffer) % 4) != 0) {
		pad = 4 - rem;
		strncpy(packetBuffer + strlen(packetBuffer), blanks, pad);
		*(packetBuffer + strlen(packetBuffer) + pad) = '\0';

	}
	//printf("String %s \n", packetBuffer);
}

/**
 * send the build UDP packet
 */
int sendUDPPacket() {
	if(sendto(socketID, packetBuffer, strlen(packetBuffer),
			0, (struct sockaddr *) &serverIPAddr,
				sizeof(serverIPAddr)) < 0) {
		printf("Could not send UDP packet\n");
		perror("due to : ");
	}
}

/**
 * close the socket
 */
void closeUDPSocket() {
	close(socketID);
}

/**
 * close the GPS file pointer
 */
void closeGPSStream() {
	fclose(fpGPSDevice);
}

/**
 * main program
 */
int main(int argc, char *argv[]) {
	openGPSStream(argc, argv);
	openUDPSocket(argc, argv);

	while(readGPSStream() != NULL) {
		if(extractGPSValues() == TRUE) {
			buildUDPPacket();
			sendUDPPacket();
		}
	}
	closeUDPSocket();
	closeGPSStream();
}