#!/usr/bin/env python

import roslib; roslib.load_manifest('dynamic_graph_bridge')
import rospy

import dynamic_graph_bridge.srv

import sys
import code
from code import InteractiveConsole
import readline

class RosShell(InteractiveConsole):
    def __init__(self):
        self.cache = ""
        InteractiveConsole.__init__(self)

        rospy.loginfo('waiting for service...')
        rospy.wait_for_service('run_command')
        self.client = rospy.ServiceProxy(
            'run_command', dynamic_graph_bridge.srv.RunCommand, True)


    def runcode(self, code, retry = True):
        source = self.cache[:-1]
        self.cache = ""
        if source != "":
            try:
                if not self.client:
                    print("Connection to remote server lost. Reconnecting...")
                    self.client = rospy.ServiceProxy(
                        'run_command', dynamic_graph_bridge.srv.RunCommand, True)
                    if retry:
                        print("Retrying previous command...")
                        self.cache = source
                        return self.runcode(code, False)
                response = self.client(str(source))
                if response.stdout != "":
                    print response.stdout[:-1]
                if response.stderr != "":
                    print response.stderr[:-1]
                elif response.result != "None":
                    print response.result
            except rospy.ServiceException, e:
                print("Connection to remote server lost. Reconnecting...")
                self.client = rospy.ServiceProxy(
                    'run_command', dynamic_graph_bridge.srv.RunCommand, True)
                if retry:
                    print("Retrying previous command...")
                    self.cache = source
                    self.runcode(code, False)

    def runsource(self, source, filename = '<input>', symbol = 'single'):
        try:
            c = code.compile_command(source, filename, symbol)
            if c:
                return InteractiveConsole.runcode(self, c)
            else:
                return True
        except SyntaxError, OverflowError:
            self.showsyntaxerror()
            self.cache = ""
            return False

    def push(self,line):
        self.cache += line + "\n"
        return InteractiveConsole.push(self,line)

if __name__ == '__main__':
    import optparse
    manifest = roslib.manifest.load_manifest('dynamic_graph_bridge')
    parser = optparse.OptionParser(
        usage='\n\t%prog [options]',
        version='%%prog %s' % manifest.version)
    (options, args) = parser.parse_args(sys.argv[1:])

    sh = RosShell()

    if args[:]:
        infile = args[0]
        source = open(infile).read()
        if not sh.client:
            print("Connection to remote server has been lost.")
            sys.exit(1)
        response = sh.client(str(source))
        if response.stdout != "":
            print response.stdout[:-1]
            if response.stderr != "":
                print response.stderr[:-1]
            elif response.result != "None":
                print response.result
    else:
        sh.interact("Interacting with remote server.")