#!/usr/bin/env python3
#
# Copyright (c) 2020 Tilman Sauerbeck (tilman at code-monkey de)
#
# Permission is hereby granted, free of charge, to any person obtaining
# a copy of this software and associated documentation files (the
# "Software"), to deal in the Software without restriction, including
# without limitation the rights to use, copy, modify, merge, publish,
# distribute, sublicense, and/or sell copies of the Software, and to
# permit persons to whom the Software is furnished to do so, subject to
# the following conditions:
#
# The above copyright notice and this permission notice shall be
# included in all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
# NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
# LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
# OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
# WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

from datetime import datetime
import struct
import sys

class Packet(object):
    def __init__(self, unix_time, latitude, longitude):
        self.unix_time = unix_time
        self.latitude = latitude
        self.longitude = longitude

class PacketExtractor(object):
    POINTS_PER_GROUP = 7
    PADDING_BYTE = 0xff

    def __init__(self, source):
        self._source = source
        self._unix_time = None
        self._latitude = 0
        self._longitude = 0
        self._flags = 0
        self._num_points = 0

    def run(self):
        header = self._source.read(5)

        format_version, self._unix_time = struct.unpack('<BI', header)

        if format_version != 1:
            s = 'Unexpected format version {}'.format(format_version)
            raise RuntimeError(s)

        while True:
            shift = self._num_points % PacketExtractor.POINTS_PER_GROUP
            self._num_points += 1

            if shift == 0:
                self._flags = PacketExtractor.PADDING_BYTE

                # Skip padding bytes.
                while self._flags == PacketExtractor.PADDING_BYTE:
                    self._flags, = struct.unpack('B', self._source.read(1))

            if (self._flags & (1 << shift)) == 0:
                d_time = 1
            else:
                d_time = self.read_uvarint()

            # End-of-stream marker hit?
            if d_time == 0xffffffff:
                break

            d_lat = self.read_svarint()
            d_lon = self.read_svarint()

            yield self.process_deltas(d_time, d_lat, d_lon)

    def process_deltas(self, d_time, d_lat, d_lon):
        self._unix_time += d_time
        self._latitude += d_lat
        self._longitude += d_lon

        q = 600000.0

        return Packet(self._unix_time, self._latitude / q, self._longitude / q)

    def read_uvarint(self):
        shift = 7
        mask = 1 << shift
        total_shift = 0
        v = 0

        while True:
            b, = struct.unpack('B', self._source.read(1))

            c = (b & (mask - 1))

            v |= c << total_shift
            total_shift += shift

            if (b & mask) == 0:
                break

        return v

    def read_svarint(self):
        u = self.read_uvarint()

        h = -(u & 1);
        n = (u >> 1) ^ h;

        return n

if __name__ == '__main__':
    filename = sys.argv[1]

    with open(filename, 'rb') as f:
        print('<?xml version="1.0"?>')
        print('<gpx version="1.1" creator="gpxify">')
        print('\t<trk>')
        print('\t\t<name>{}</name>'.format(filename))
        print('\t\t<number>{}</number>'.format(1))
        print('\t\t<trkseg>')

        for packet in PacketExtractor(f).run():
            print('\t\t\t<trkpt lat="{:.8f}" lon="{:.8f}">'.format(
                   packet.latitude, packet.longitude))

            dt = datetime.utcfromtimestamp(packet.unix_time)

            print(dt.strftime('\t\t\t\t<time>%Y-%m-%dT%H:%M:%SZ</time>'))
            print('\t\t\t</trkpt>')

        print('\t\t</trkseg>')
        print('\t</trk>')
        print('</gpx>')