#! /usr/bin/env python
#
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

from __future__ import with_statement

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

WXVER = ['2.8', '2.9']
import wxversion
if wxversion.checkInstalled(WXVER):
    wxversion.select(WXVER)
else:
    import sys
    print >> sys.stderr, 'This application requires wxPython version %s' % (WXVER)
    sys.exit(1)

import math
import threading
import time
import unicodedata
import wx
import wx.lib.scrolledpanel

import dynamic_reconfigure.client
import rosservice
import sys

class MainWindow(wx.Frame):
    def __init__(self, node=None):
        if node is not None:
            title = 'Reconfigure %s' % node
        else:
            title = 'Reconfigure'

        wx.Frame.__init__(self, None, wx.ID_ANY, title, pos=(200, 200), size=(400, 400))

        # Create menu
        self.menubar = wx.MenuBar()
        self.filemenu = wx.Menu()
        self.filemenu.Append(wx.ID_EXIT, 'E&xit', 'Exit the program')
        wx.EVT_MENU(self, wx.ID_EXIT, self.on_exit)
        self.menubar.Append(self.filemenu, '&File')
        self.SetMenuBar(self.menubar)

        # Add reconfigure control
        if node:
            try:
                wx.SetCursor(wx.StockCursor(wx.CURSOR_WAIT))
                wx.Yield()

                reconf = dynamic_reconfigure.client.Client(node, timeout=5.0)
            except:
                print 'Can\'t connect to node %s' % node
                self.Close()
                return
            finally:
                wx.SetCursor(wx.StockCursor(wx.CURSOR_ARROW))
                wx.Yield()
                
            control = DynamicReconfigurePanel(self, reconf)

            control.SetMinSize((control.GetSize()[0] + 30, control.GetSize()[1]))
        else:
            control = DynamicReconfigureSelector(self)

        self.sizer = wx.BoxSizer(wx.VERTICAL)
        self.sizer.Add(control, 1, wx.EXPAND | wx.ALL, 0)
        self.SetSizer(self.sizer)

        if node:
            self.Fit()
            self.SetMinSize(self.GetSize())

        # Check every 1s if node has shutdown (and close GUI)
        self.timer = wx.Timer(self, wx.ID_ANY)
        self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
        self.timer.Start(1000, False)

    def on_timer(self, event):
        if rospy.is_shutdown():
            self.Close(True)
            self.Refresh()

    def on_exit(self, e):
        self.Close(True)
        self.Refresh()

    def on_error(self):
        self.Raise()

## Dropdown to choose which node to reconfigure
class DynamicReconfigureSelector(wx.Panel):
    def __init__(self, parent):
        wx.Panel.__init__(self, parent, wx.ID_ANY)

        self.parent       = parent
        self.reconf_panel = None
        self.has_master   = True

        self.combobox = wx.ComboBox(self, wx.ID_ANY, style=wx.CB_READONLY)
        self.update_combobox()
        self.Bind(wx.EVT_COMBOBOX, self.on_combobox)

        self.sizer = wx.BoxSizer(wx.VERTICAL)
        self.sizer.Add(self.combobox, 0, wx.EXPAND)
        self.SetSizer(self.sizer)

        # Refresh node list periodically
        self.timer = wx.Timer(self, wx.ID_ANY)
        self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
        self.timer.Start(5000, False)

    def on_timer(self, event):
        self.update_combobox()

    def update_combobox(self):
        try:
          nodes = dynamic_reconfigure.find_reconfigure_services()
        except rosservice.ROSServiceIOException:
          if self.has_master:
            print >> sys.stderr, "reconfigure_gui cannot connect to master."
          self.has_master = False
        else:
          if not self.has_master:
            print >> sys.stderr, "reconfigure_gui reconnected to master."
          self.has_master = True

          self.combobox.SetItems(nodes)
  
          selected_node = self.get_selected_node()
          if selected_node not in nodes:
              self.close_node()               # node has stopped
          elif self.reconf_panel is None:
              self.view_node(selected_node)   # node has restarted

    def get_selected_node(self):
        return str(self.combobox.GetValue())

    def on_combobox(self, event):
        self.view_node(self.get_selected_node())
        
    def view_node(self, node):
        try:
            wx.SetCursor(wx.StockCursor(wx.CURSOR_WAIT))
            wx.Yield()

            try:
                reconf = dynamic_reconfigure.client.Client(node, timeout=5.0)
            except rospy.exceptions.ROSException:
                return
            finally:
                self.close_node()
            
            self.reconf_panel = DynamicReconfigurePanel(self, reconf)

            self.sizer.Add(self.reconf_panel, 1, wx.EXPAND | wx.ALL, 0)
            self.sizer.Layout()

            self.parent.Fit()
            w = self.parent.GetSize()[0]
            h = self.reconf_panel.hbox.GetMinSize()[1] + self.reconf_panel.GetPosition()[1] + (self.parent.GetSize()[1] - self.parent.GetClientSize()[1])
            self.parent.SetMinSize((w, 100))
            self.parent.SetSize((w, h))
            
            if self.reconf_panel is None:
                self.combobox.SetValue('')
            else:
                self.combobox.SetValue(self.reconf_panel.reconf.name)
        finally:
            wx.SetCursor(wx.StockCursor(wx.CURSOR_ARROW))
            wx.Yield()

    def close_node(self):
        if self.reconf_panel is not None:
            self.reconf_panel.close()
            self.reconf_panel.Destroy()
            self.reconf_panel = None

class DynamicReconfigurePanel(wx.lib.scrolledpanel.ScrolledPanel):
    def __init__(self, parent, reconf):
        wx.lib.scrolledpanel.ScrolledPanel.__init__(self, parent, wx.ID_ANY)

        self.parent = parent
        self.reconf = reconf

        config = self.reconf.get_configuration()
        descr  = self.reconf.get_parameter_descriptions()
        
        # Create the editors
        self.editors = {}
        
        self._editor_types = {
            'bool':   DynamicReconfigurePanel.BooleanEditor,
            'int':    DynamicReconfigurePanel.IntegerEditor,
            'double': DynamicReconfigurePanel.DoubleEditor,
            'str':    DynamicReconfigurePanel.StringEditor
        }
        flags = {
            'bool':   wx.ALIGN_LEFT,
            'int':    wx.EXPAND,
            'str':    wx.EXPAND,
            'double': wx.EXPAND,
        }
        
        self.hbox = wx.BoxSizer(wx.HORIZONTAL)
        
        sizer = wx.FlexGridSizer(0, 2, vgap=1, hgap=4)
        sizer.SetFlexibleDirection(wx.BOTH)
        sizer.AddGrowableCol(1, 1)

        for param_descr in descr:
            name = param_descr['name']
            type, val, min, max, description = param_descr['type'], config[name], param_descr['min'], param_descr['max'], param_descr['description']

            # Label
            label = wx.StaticText(self, wx.ID_ANY, name + ':')
            sizer.Add(label, 1, wx.ALIGN_CENTER_VERTICAL | wx.ALIGN_RIGHT)

            # Editor
            try:
                enum = eval(param_descr['edit_method'])['enum']
                editor = self.EnumEditor(self, name, val, enum)
            except:
                editor = self._editor_types[type](self, name, val, min, max)
            self.editors[name] = editor
            sizer.Add(editor, 0, flags[type])

            tooltip_text = description
            min_max_text = editor.get_min_max_text(min, max)
            if min_max_text is not None:
                tooltip_text += ' ' + min_max_text
            tooltip = wx.ToolTip(tooltip_text)
            tooltip.SetDelay(2000)
            label.SetToolTip(tooltip)

        self.hbox.Add(sizer, 1, wx.ALL | wx.EXPAND, 4)

        self.SetSizerAndFit(self.hbox)
        self.SetMinSize((self.GetSize()[0] + 30, 10))
        self.SetupScrolling()

        self.reconf.config_callback = self._config_callback

        self.updater = self.BatchUpdater(self.reconf)
        self.updater.start()

    def _config_callback(self, config):
        for name, editor in self.editors.iteritems():
            wx.CallAfter(editor.update_value, config[name])

    def close(self):
        self.updater.stop()
        self.reconf.close()
        
    class BatchUpdater(threading.Thread):
        def __init__(self, reconf):
            threading.Thread.__init__(self)
            self.setDaemon(True)

            self._reconf         = reconf
            self._cv             = threading.Condition()
            self._pending_config = {}
            self._last_pending   = None
            self._stop_flag      = False

        def run(self):
            last_commit = None

            while not self._stop_flag:
                if last_commit > self._last_pending:
                    with self._cv:
                        self._cv.wait()                 
                        
                last_commit = time.time()
                
                update = self._pending_config.copy()
                self._pending_config = {}

                try:
                    updated = self._reconf.update_configuration(update)
                except:
                    pass

        def update(self, config):
            with self._cv:
                for name, value in config.items():
                    self._pending_config[name] = value

                self._last_pending = time.time()

                self._cv.notify()

        def stop(self):
            self._stop_flag = True
            with self._cv:
                self._cv.notify()

    class Editor:
        def __init__(self, name):
            self.name = name

        def update_value(self, value):
            pass

        def _update(self, event):
            new_value = self.get_value() 
            if new_value != self.old_value:
                self.update_configuration(new_value)

        def update_configuration(self, value):
            self.GetParent().updater.update({ self.name : value })

        def get_min_max_text(self, min, max):
            return None

    class EnumEditor(Editor, wx.ComboBox):
        def __init__(self, parent, name, val, enum):
            DynamicReconfigurePanel.Editor.__init__(self, name)
            wx.ComboBox.__init__(self, parent, wx.ID_ANY, style=wx.CB_READONLY)
            
            names  = [item['name'] + ' (' + repr(item['value']) + ')' for item in enum]
            values = [item['value'] for item in enum] 
            self.to_value = dict(zip(names, values))
            self.to_name  = dict(zip(values, names))
            
            self.SetItems(names)

            self.update_value(val)
            
            self.Bind(wx.EVT_COMBOBOX, self._update)

        def get_value(self):
            return self.to_value[self.GetValue()]

        def update_value(self, value):
            self.old_value = value
            self.SetValue(self.to_name.get(value, ''))

    class BooleanEditor(Editor, wx.CheckBox):
        def __init__(self, parent, name, value, min, max):
            DynamicReconfigurePanel.Editor.__init__(self, name)
            wx.CheckBox.__init__(self, parent, wx.ID_ANY)
    
            self.update_value(value)
            
            self.Bind(wx.EVT_CHECKBOX, self._update)
                                             
        def get_value(self):
            return self.GetValue()

        def update_value(self, value):
            self.old_value = value
            self.SetValue(value) 
    
    class StringEditor(Editor, wx.TextCtrl):
        def __init__(self, parent, name, value, min, max):
            DynamicReconfigurePanel.Editor.__init__(self, name)
            wx.TextCtrl.__init__(self, parent, wx.ID_ANY, style=wx.TE_PROCESS_ENTER)
    
            self.old_value = value
            
            self.SetValue(value)
            self.Bind(wx.EVT_TEXT_ENTER, self._update)
            self.Bind(wx.EVT_KILL_FOCUS, self._update)
        
        def get_value(self):
            return unicodedata.normalize('NFKD', self.GetValue()).encode('ascii', 'ignore')

        def update_value(self, value):
            self.old_value = value
            self.SetValue(value)
        
    class IntegerEditor(Editor, wx.Panel):
        def __init__(self, parent, name, value, min, max):
            DynamicReconfigurePanel.Editor.__init__(self, name)
            wx.Panel.__init__(self, parent, wx.ID_ANY)

            self.min = min
            self.max = max

            self.delay_slider_updates = False
            self.pending_update_value = None
            
            self.min_label = wx.StaticText(self, wx.ID_ANY, self._text_value(min))
            self.slider    = wx.Slider(self, wx.ID_ANY, minValue=self._slider_value(self.min), maxValue=self._slider_value(self.max), style=wx.SL_AUTOTICKS | wx.SL_HORIZONTAL)
            self.max_label = wx.StaticText(self, wx.ID_ANY, self._text_value(max))
            self.text      = wx.TextCtrl(self, wx.ID_ANY, style=wx.TE_PROCESS_ENTER)
            
            self.slider.SetMinSize((60, -1))

            sizer = wx.BoxSizer(wx.HORIZONTAL)
            sizer.Add((2, -1))
            sizer.Add(self.min_label)
            sizer.Add((2, -1))
            sizer.Add(self.slider, wx.EXPAND)
            sizer.Add((2, -1))
            sizer.Add(self.max_label)
            sizer.Add((4, -1))
            sizer.Add(self.text)
            self.SetSizer(sizer)
    
            self.slider.Bind(wx.EVT_LEFT_DOWN, self._slider_left_down)
            self.slider.Bind(wx.EVT_SCROLL, self._slider_update)
            self.slider.Bind(wx.EVT_LEFT_UP, self._slider_left_up)
            self.text.Bind(wx.EVT_TEXT_ENTER, self._text_update)
            self.text.Bind(wx.EVT_KILL_FOCUS, self._text_update)
            
            self.update_value(value)

        def update_value(self, value):
            self.textvalue = self._text_value(value)
            self.text.SetValue(self.textvalue)

            if self.delay_slider_updates:
                self.pending_update_value = value
                return

            self.value = value

            self.slidervalue = self._slider_value(self.value)
            self.slider.SetValue(self.slidervalue)

        def _slider_update(self, event):
            new_slidervalue = self.slider.GetValue()
            if self.slidervalue != new_slidervalue:
                self._change_value(self._inv_slider_value(new_slidervalue))

        def _slider_left_down(self, event):
            self.delay_slider_updates = True
            event.Skip()

        def _slider_left_up(self, event):
            self.delay_slider_updates = False
            if self.pending_update_value is not None:
                self.update_value(self.pending_update_value)
                self.pending_update_value = None
            event.Skip()

        def _text_update(self, event):
            new_textvalue = self.text.GetValue()
            if self.textvalue != new_textvalue:
                self._change_value(self._inv_bound_text_value(new_textvalue))
    
        def _change_value(self, new_value):
            if new_value != self.value:
                self.update_configuration(new_value)

        def _slider_value(self, val):     return val
        def _inv_slider_value(self, val): return val
        def _text_value(self, val):       return str(val)
        def _inv_text_value(self, val):   return int(val)

        def _inv_bound_text_value(self, val):
            try:
                return min(max(self._inv_text_value(val), self.min), self.max)
            except Exception, e:
                return self.value
            
        def get_min_max_text(self, min, max):
            return 'min: %s max: %s' % (self._text_value(min), self._text_value(max))
    
    class DoubleEditor(IntegerEditor):
        def __init__(self, parent, name, value, min, max):
            infinities = [ 1e1000, -1e1000, None ]
            if not min in infinities and not max in infinities:
                self.func  = lambda x: x
                self.ifunc = lambda x: x
            else:
                self.func  = lambda x: math.atan(x)
                self.ifunc = lambda x: math.tan(x)
            self.scale  = (self.func(max) - self.func(min)) / 100
            self.offset = self.func(min)

            DynamicReconfigurePanel.IntegerEditor.__init__(self, parent, name, value, min, max)
        
        def _slider_value(self, val):     return int(round((self.func(val) - self.offset) / self.scale))
        def _inv_slider_value(self, val): return self.ifunc(self.offset + val * self.scale)
        def _text_value(self, val):       return '%.5g' % val
        def _inv_text_value(self, val):   return float(val)

if __name__ == '__main__':
    argv = rospy.myargv()
    if len(argv) > 2:
        print 'Usage: reconfigure_gui <node_name>'
        exit(1)

    app = wx.PySimpleApp(clearSigInt=False)
    rospy.init_node('reconfigure_gui', anonymous=True)
    if len(argv) == 2:
        frame = MainWindow(argv[1])
    else:
        frame = MainWindow()
    frame.Show()

    print 'reconfigure_gui started'

    try:
        app.MainLoop()
    except KeyboardInterrupt, e:
        pass
    print 'exiting'
