#!/usr/bin/env python

import argparse
import logging

import matplotlib.pyplot as plt
import rosbag_pandas


def build_parser():
    """
    Builds the parser for reading the command line arguments
    :return: Argument parser
    """
    parser = argparse.ArgumentParser(description='Bagfile key to graph')
    parser.add_argument('-b', '--bag', help='Bag file to read',
                        required=True, type=str)
    parser.add_argument('-k', '--key',
                        help='Key you would like to plot',
                        required=True, nargs='*')
    parser.add_argument('-y ', '--ylim',
                        help='Set min and max y lim',
                        required=False, nargs=2)
    parser.add_argument('-c', '--combined',
                        help="Graph them all on one",
                        required=False, action="store_true", dest="sharey")
    parser.add_argument('-v', '--verbose',
                        help="Log verbose",
                        default=False, action="store_true")
    return parser


def graph(df, keys, sharey):
    if sharey or len(keys) == 1:
        fig, axes = plt.subplots()
        for key in keys:
            s = df[key].dropna()
            axes.plot(s.index, s.values, label=key)

        axes.legend(loc=0)
        plt.show()
    else:
        fig, axes = plt.subplots(len(keys), sharex=True)
        idx = 0
        for key in keys:
            s = df[key].dropna()
            axes[idx].plot(s.index, s.values)
            axes[idx].set_title(key)
            idx = idx + 1
        plt.show()


if __name__ == '__main__':
    parser = build_parser()
    args = parser.parse_args()

    if args.verbose:
        logging.getLogger().setLevel(logging.DEBUG)

    topics = rosbag_pandas.topics_from_keys(args.key)
    df = rosbag_pandas.bag_to_dataframe(args.bag, include=topics)
    graph(df, args.key, args.sharey)
