#!/usr/bin/python

from __future__ import print_function

import math
import re
import sys

def rad2deg(deg):
	return deg * (math.pi / 180);

def get_distance(lat0, lon0, lat1, lon1):
	R = 6371;
	dLat = rad2deg(lat1 - lat0)
	dLon = rad2deg(lon1 - lon0)
	a = math.sin(dLat / 2) * math.sin(dLat / 2) + \
	  math.cos(rad2deg(lat0)) * math.cos(rad2deg(lat1)) * \
	  math.sin(dLon / 2) * math.sin(dLon / 2)
	c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
	d = R * c * 1000
	return d;

def read_mission(mfile):
	f = open(mfile, 'r')
	latitude = 1000.0
	longitude = 1000.0
	points = []

	for line in f.readlines():
		try:
			if line.strip().startswith("#"):
				continue
			if 'latitude' in line.lower():
				latitude = float(re.split("[=;]", line)[1].strip())
				#print(latitude)
			if 'longitude' in line.lower():
				longitude = float(re.split("[=;]", line)[1].strip())
				#print(longitude)
			if '}' in line:
				if latitude != 1000.0 and longitude != 1000.0:
					points.append((latitude, longitude))
					#print("adding", (latitude, longitude))
				latitude = 1000.0
				longitude = 1000.0
		except:
			continue

	return points;


if __name__ == "__main__":
	if (len(sys.argv) != 2):
		print("Usage: " + sys.argv[0] + " <mission_file>")
		sys.exit(2)

	try:
		points = read_mission(sys.argv[1])
	except Exception as e:
		print("Error: " + str(e))
		sys.exit(1)

	if not points:
		sys.exit(0)

	last_point = None
	d = 0
	print("0,Route:1,0,0.000,0.000,1,2,65280,0,0.200,0,0,1.000")
	for p in reversed(points):
		if last_point is not None:
			d += get_distance(p[0], p[1], last_point[0], last_point[1])
		s = "1,{:.10f},{:.10f},0.00m,0.00m,0.00,0.00,{:.3f}".format(p[0], p[1], d)
		print(s)
		last_point = p