#! /usr/bin/env python

from __future__ import print_function

import gps_common.gps_message_converter as converter
import os
from rosbag import Bag
import shutil
import sys


def process_bag(bag_path):
    # Append .orig.bag to the requested bag so we do not overwrite it
    orig = os.path.splitext(bag_path)[0] + '.orig.bag'
    shutil.move(bag_path, orig)

    # Get information about the topic names and types so we can automatically
    # convert the correct topics
    with Bag(orig, 'r') as in_bag:
        topic_info = in_bag.get_type_and_topic_info()

    # Create a new bag with the original path
    with Bag(bag_path, 'w') as out_bag:
        # Put every message in the original bag into the new bag
        for topic, msg, ts in Bag(orig).read_messages():
            # If the message is on a NavSatFix topic, convert it to a GPSFix
            # message
            if topic_info[1][topic][0] == 'sensor_msgs/NavSatFix':
                msg = converter.navsatfix_to_gpsfix(msg)
            # If the message is on a GPSFix topic, convert it to a NavSatFix
            # message
            elif topic_info[1][topic][0] == 'gps_common/GPSFix':
                msg = converter.gpsfix_to_navsatfix(msg)
            # Let all other messages fall through without changing them

            out_bag.write(topic, msg, ts)


if __name__ == '__main__':
    try:
        b = sys.argv[1]
    except IndexError:
        print('Usage: bag_converter <full_path_to_bag>')
        sys.exit(1)

    process_bag(b)

