#! /usr/bin/env python

'''
Simple Script to collect and process bag files
from RawSnapshot in Release 2.0.


Commandline args:
    -b <bag_dir>    directory to save bag file
    -o <out_dr>     output directory for report
    -v              get version number
    -h              get help

Please direct any question to multisense@carnegierobotics.com or
    http://support.carnegierobotics.com

'''

import sys
import getopt
import os
import time
import platform
import tempfile
import shutil
from Tkinter import Tk
from tkFileDialog import askopenfilename
import tkMessageBox
import getopt

from process_bags import _BagProcessor

try:
    import roslib.packages
except:
    raise Exception("Error importing ROS. Source the ROS environment" \
                   +" in the workspace where the multisense stack is located")
#end try

class CheckCal():

    def __init__(self, bag_dir=".", out_dir="."):

        self.bag_dir = bag_dir
        self.out_dir = out_dir

        #Create bag file directory if it does not already exist
        if not os.path.exists(self.bag_dir):
            os.makedirs(self.bag_dir)
        #end if

        #Create output file directory if it does not already exist
        if not os.path.exists(self.out_dir):
            os.makedirs(self.out_dir)
        #end if
    #end def


    #Run the routine to collect a bag file, process it, and run the
    #Calibration utility
    def run(self):
        [bag_out, fname] = self.collect_bag()
        print "Finished collecting bag file"

        if bag_out == 0:
            fname = self.process_bag(fname)
        else:
            print "Unable to capture a bagfile. Exiting."
        #end if
    #end def

    #Collects Bag files using the raw_snapshot executable
    def collect_bag(self):
        fname =  time.strftime("%Y-%m-%d_%H-%M_%Z",
                                time.localtime(time.time())) + ".bag"
        fname = os.path.join(self.bag_dir, fname)
        output = os.system("rosrun multisense_ros raw_snapshot %s" % fname)

        return output, fname
    #end def


    #Instantiate bag Processor and extract information saving to tmp dir
    #Returns bag file name
    def process_bag(self, bag):
        print "Beginning to compute error metric"
        processor = _BagProcessor(bag)
        return processor.process(self.out_dir)
    #end def




def usage():
    print "usage: ./cal_check <options>"
    print "Where <options> are"
    print "    -b <bag_dir>\t\tspecifies the directory to save bag files. " \
                 + "(default='.')"
    print "    -o <out_dir>\t\tspecifies the output directory to save the "\
                 + "calibration results file (default='.')"
    print "    -v \t\t\t\tversion number"
    print "    -h \t\t\t\tget help"
#end def


if __name__ == "__main__":


    #Default values
    bag_dir = "."
    out_dir = "."
    # Version 3.0 contains the new python based check function
    version = "Version 3.0"

    try:
        opts, args = getopt.getopt(sys.argv[1:],"b:o:vh")
    except getopt.GetoptError as err:
        print str(err)
        usage()
        sys.exit(2)
    for o, a in opts:
        if o == "-h":
            usage()
            sys.exit()
        elif o == "-v":
            print version
            sys.exit(1)
        elif o == "-b":
            bag_dir = str(a)
        elif o == "-o":
            out_dir = str(a)
        else:
            usage()
            sys.exit(2)
        #endif

    cal_check = CheckCal(bag_dir, out_dir)
    cal_check.run()

#end if
