#!/usr/bin/env python
# Copyright (c) 2008, 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 the 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.

# GUI to communicate with robotIQ driver
#
# Author: Derek King

"""
Usage:  riq_hand_gui

Description:
  Provides wxPython base GUI to RobotIQ hand EtherCAT driver.

Args:  
  NONE YET.
"""

PKG = 'riq_hand_gui'

import roslib
roslib.load_manifest(PKG)
import rospy
import sys
from riq_msgs.msg import RIQHandCommand, RIQHandState
import wx
import threading

def usage():
  print __doc__


############################################################
# Acutuator Panel
############################################################
class ActuatorPanel:
    def __init__(self, frame, name, id_base):
        POSITION_TEXT_BOX_ID = id_base + 0
        CURRENT_TEXT_BOX_ID  = id_base + 1
        OBJECT_TEXT_BOX_ID   = id_base + 2
        
        self.name_label        = wx.StaticText(frame, -1, name)
        self.position_text_box = wx.TextCtrl(frame, -1)
        self.position_slider   = wx.Slider(frame, -1, value=0, minValue=0, maxValue=1000, size=(200,10))
        self.current_text_box  = wx.TextCtrl(frame, -1)
        self.current_slider    = wx.Slider(frame, -1, value=0, minValue=0, maxValue=1000, size=(200,10))
        self.object_test_box   = wx.TextCtrl(frame, -1)

        self.position_sizer = wx.BoxSizer(wx.VERTICAL)
        self.position_sizer.Add(self.position_text_box, 1, wx.ALIGN_CENTER | wx.EXPAND)
        self.position_sizer.Add(self.position_slider  , 1, wx.ALIGN_CENTER | wx.EXPAND)
        self.current_sizer = wx.BoxSizer(wx.VERTICAL)
        self.current_sizer.Add(self.current_text_box, 1, wx.ALIGN_CENTER | wx.EXPAND)
        self.current_sizer.Add(self.current_slider  , 1, wx.ALIGN_CENTER | wx.EXPAND)


    def getRow(self):
        return [self.name_label, self.position_sizer, self.current_sizer, self.object_test_box]


    def Update(self, actuator):
        self.position_text_box.SetValue("%.4f"%actuator.position)
        self.position_slider.SetValue(1000. * actuator.position)
        self.current_text_box.SetValue("%.4f"%(actuator.current*1000.))
        # Max value for current (base on 8bit value) * 0.1mA = 25.5mA = 25.5e-3Amps
        # Max value (i've seen) for current is only 8mA 
        # Max value for slider is 1000.   
        # need to scale current value so 8e-3 amps == 1000.
        scale = 1000. / 8e-3
        self.current_slider.SetValue( scale * actuator.current)
        self.object_test_box.SetValue(str(actuator.object_detected))






def statusString(value):
    if (value == RIQHandState.FAULTED):
        result = "FAULTED"
    elif (value == RIQHandState.IN_PROGRESS):
        result = "IN_PROGRESS"
    elif (value == RIQHandState.ILLEGAL_OR_UNDEFINED):
        result = "ILLEGAL_OR_UNDEFINED"
    elif (value == RIQHandState.SUCCESSFUL):
        result =  "SUCCESSFUL"
    else:
        result = "???"
    return result + " (%d)"%value


def modeString(value):
    if (value == RIQHandState.CYLINDRICAL):
        result = "CYLINDRICAL"
    elif (value == RIQHandState.PINCH):
        result = "PINCH"
    elif (value == RIQHandState.SPHERIOD):
        result = "SPHERIOD"
    elif (value == RIQHandState.SCISSORS):
        result =  "SCISSORS"
    else:
        result = "???"
    return result + " (%d) "%value

def gripString(value):
    if (value == RIQHandState.STOPPED):
        result = "STOPPED"
    elif (value == RIQHandState.CLOSED):
        result = "CLOSED"
    elif (value == RIQHandState.OPENED):
        result = "OPENED"
    elif (value == RIQHandState.INITIALIZING_OR_CHANGING):
        result =  "INITIALIZING_OR_CHANGING"
    else:
        result = "???"
    return result + " (%d) "%value

def objectString(value):
    if (value == RIQHandState.NOT_DETECTED):
        result = "NOT_DETECTED"
    elif (value == RIQHandState.DETECTED_1_FINGER):
        result = "DETECTED_1_FINGER"
    elif (value == RIQHandState.DETECTED_2_FINGERS):
        result = "DETECTED_2_FINGER"
    elif (value == RIQHandState.DETECTED_ALL_FINGERS):
        result =  "DETECTED_ALL_FINGERS"
    else:
        result = "???"
    return result + " (%d) "%value


############################################################
# State panel
############################################################
class StatePanel(wx.Panel):
    def __init__(self, parent, id):
        wx.Panel.__init__(self, parent, id, style=wx.SUNKEN_BORDER)
        vsizer2 = wx.StaticBoxSizer(wx.StaticBox(self, -1, 'RobotIQ Hand State'), wx.VERTICAL)
        
        gsizer2 = wx.GridBagSizer()
        #gsizer2 = wx.GridSizer(rows=5,cols=2)
        op_label = wx.StaticText(self, -1, "Operational:")
        self.op_text_box = wx.TextCtrl(self, -1)
        
        status_label = wx.StaticText(self, -1, "Status:")
        self.status_text_box = wx.TextCtrl(self, -1)

        mode_label = wx.StaticText(self, -1, "Mode:")
        self.mode_text_box = wx.TextCtrl(self, -1)

        grip_label = wx.StaticText(self, -1, "Grip:")
        self.grip_text_box = wx.TextCtrl(self, -1)

        object_label = wx.StaticText(self, -1, "Object:")
        self.object_text_box = wx.TextCtrl(self, -1)        

        gsizer2.Add(op_label            , pos=(0,0), span=(1,1))
        gsizer2.Add(self.op_text_box    , pos=(0,1), span=(1,4))

        gsizer2.Add(status_label        , pos=(1,0), span=(1,1))
        gsizer2.Add(self.status_text_box, pos=(1,1), span=(1,4), flag=wx.EXPAND)

        gsizer2.Add(mode_label          , pos=(2,0), span=(1,1))
        gsizer2.Add(self.mode_text_box  , pos=(2,1), span=(1,4), flag=wx.EXPAND)

        gsizer2.Add(grip_label          , pos=(3,0), span=(1,1))
        gsizer2.Add(self.grip_text_box  , pos=(3,1), span=(1,4), flag=wx.EXPAND)

        gsizer2.Add(object_label        , pos=(4,0), span=(1,1))
        gsizer2.Add(self.object_text_box, pos=(4,1), span=(1,4), flag=wx.EXPAND)

        gsizer2.AddGrowableCol(1)


        self.thumb    = ActuatorPanel(self, "Thumb", 30)
        self.right    = ActuatorPanel(self, "Right Finger", 40)
        self.left     = ActuatorPanel(self, "Left Finger", 50)
        self.scissors = ActuatorPanel(self, "Scissors", 60)

        row1 = [wx.StaticText(self, -1, ''), \
                wx.StaticText(self, -1, "Position"), \
                wx.StaticText(self, -1, "Current (mA)"), \
                wx.StaticText(self, -1, "Object\nDetected") ]
            
        rows = [ row1, self.thumb.getRow(), self.right.getRow(), self.left.getRow(), self.scissors.getRow()]
        
        gsizer1 = wx.GridBagSizer()
        for row,items in enumerate(rows):
            gsizer1.Add(items[0], pos=(row,0), span=(1,1), flag=wx.EXPAND)
            gsizer1.Add(items[1],pos=(row,1), span=(1,2), flag=wx.EXPAND)
            gsizer1.Add(items[2],pos=(row,3), span=(1,2), flag=wx.EXPAND)
            gsizer1.Add(items[3],pos=(row,5), span=(1,1), flag=wx.EXPAND)
        gsizer1.AddGrowableCol(1)
        gsizer1.AddGrowableCol(3)

        vsizer2.Add(gsizer2, flag=wx.EXPAND)        
        vsizer2.Add(gsizer1, flag=wx.EXPAND)
        self.SetSizer(vsizer2)

    def updateGuiState(self, state):
        self.op_text_box.SetValue(str(state.operational))
        self.status_text_box.SetValue(statusString(state.status))
        self.mode_text_box.SetValue(modeString(state.mode))
        self.grip_text_box.SetValue(gripString(state.grip))
        self.object_text_box.SetValue(objectString(state.object))

        self.thumb.Update(state.thumb)
        self.right.Update(state.right_finger)
        self.left.Update(state.left_finger)
        self.scissors.Update(state.scissors)



class MainWindow(wx.Frame):
    def __init__(self, parent, id, title):
        wx.Frame.__init__(self, parent, id, title)   

        #
        # There is a separate thread to publish data at a consistant rate.
        # 
        #  * The event Callbacks, modify self.command.
        #  * The publish thread publishes value stored self.publish_command while holding publish_lock
        #  * The publish() function will copy self.command to self.publish_command while holding publish_lock

        self.publisher = rospy.Publisher('riq_hand_command', RIQHandCommand)
        rospy.sleep(0.5)
        self.command = RIQHandCommand(RIQHandCommand.CYLINDRICAL, RIQHandCommand.STOP, 0.0, 0.0)
        self.publish_lock = threading.Lock()
        self.publish()
        self.publish_thread = threading.Thread(None, self.publishThreadFunc)
        self.publish_thread.start()

        # Use a lock around state.  I beleive the callback is run from its own thread.
        # Use idle event to grab newest state and update GUI controls
        self.state_lock = threading.Lock()
        self.state = None
        self.subscriber = rospy.Subscriber("riq_hand_state", RIQHandState, self.handStateCallback)

        rospy.on_shutdown(self.OnRosShutdown)

        # User timer instead of update event to drive GUI updates
        # Idle callback only seems to be run after some other event occurs, (such as mouse movements)
        self.timer = wx.Timer(self, -1)
        self.Bind(wx.EVT_TIMER, self.OnTick, self.timer)
        self.timer.Start(milliseconds=100, oneShot=False)
        #self.Bind(wx.EVT_IDLE, self.OnIdle)
        self.Bind(wx.EVT_CLOSE, self.OnCloseFrame)

        #for s in subassem_list:
        ID_ABOUT=100
        ID_EXIT=101

        # Setting up the menu.
        filemenu= wx.Menu()
        #filemenu.Append(ID_ABOUT, "&About"," Information about this program")
        filemenu.AppendSeparator()
        filemenu.Append(ID_EXIT,"E&xit"," Terminate the program")
        # Creating the menubar.
        menuBar = wx.MenuBar()
        menuBar.Append(filemenu,"&File") 
        self.SetMenuBar(menuBar)  
                
        #EVT_MENU(self, ID_ABOUT, self.OnAbout)
        wx.EVT_MENU(self, ID_EXIT, self.OnQuit)

        STOP_BUTTON_ID    = 10
        OPEN_BUTTON_ID    = 11
        CLOSE_BUTTON_ID   = 12

        CYLINDRICAL_BUTTON_ID = 13
        PINCH_BUTTON_ID       = 14        
        SPHERIOD_BUTTON_ID    = 15
        SCISSORS_BUTTON_ID    = 16

        VELOCITY_TEXT_BOX_ID  = 17
        FORCE_TEXT_BOX_ID     = 18

        VELOCITY_SLIDER_ID    = 19
        FORCE_SLIDER_ID       = 20

        ############################################################
        # Control panel
        ############################################################
        control_panel = wx.Panel(self, -1, style=wx.SUNKEN_BORDER)
        vsizer1 = wx.StaticBoxSizer(wx.StaticBox(control_panel, -1, 'RobotIQ Hand Control'), wx.VERTICAL)

        # Action buttons
        # Buttons, stop, open, close
        hsizer1 = wx.BoxSizer(wx.HORIZONTAL)
        action_label = wx.StaticText(control_panel, -1, "Action:")
        hsizer1.Add(action_label, 0, wx.ALIGN_CENTER | wx.EXPAND)
        stop_button = wx.Button(control_panel, STOP_BUTTON_ID, "Stop")
        wx.EVT_BUTTON(control_panel, STOP_BUTTON_ID, self.OnStop)
        hsizer1.Add(stop_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        open_button = wx.Button(control_panel, OPEN_BUTTON_ID, "Open")
        wx.EVT_BUTTON(control_panel, OPEN_BUTTON_ID, self.OnOpen)
        hsizer1.Add(open_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        close_button = wx.Button(control_panel, CLOSE_BUTTON_ID, "Close")
        wx.EVT_BUTTON(control_panel, CLOSE_BUTTON_ID, self.OnClose)
        hsizer1.Add(close_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        hsizer1.SetMinSize((100,50))

        # Mode control buttons
        # Buttons, cyclindrical, pinch, spheriod, scissors
        hsizer2 = wx.BoxSizer(wx.HORIZONTAL)
        mode_label = wx.StaticText(control_panel, -1, "Mode:")
        hsizer2.Add(mode_label, 0, wx.ALIGN_CENTER | wx.EXPAND)
        cylindrical_button = wx.Button(control_panel, CYLINDRICAL_BUTTON_ID, "Cylindrical")
        wx.EVT_BUTTON(control_panel, CYLINDRICAL_BUTTON_ID, self.OnCylindrical)
        hsizer2.Add(cylindrical_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        pinch_button = wx.Button(control_panel, PINCH_BUTTON_ID, "Pinch")
        wx.EVT_BUTTON(control_panel, PINCH_BUTTON_ID, self.OnPinch)
        hsizer2.Add(pinch_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        spheriod_button = wx.Button(control_panel, SPHERIOD_BUTTON_ID, "Spheriod")
        wx.EVT_BUTTON(control_panel, SPHERIOD_BUTTON_ID, self.OnSpheriod)
        hsizer2.Add(spheriod_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        scissors_button = wx.Button(control_panel, SCISSORS_BUTTON_ID, "Scissors")
        wx.EVT_BUTTON(control_panel, SCISSORS_BUTTON_ID, self.OnScissors)
        hsizer2.Add(scissors_button, 0, wx.ALIGN_CENTER | wx.EXPAND)
        hsizer2.SetMinSize((100,50))

        # Velocity input
        hsizer3 = wx.BoxSizer(wx.HORIZONTAL)
        velocity_label = wx.StaticText(control_panel, -1, "Velocity (0.0-1.0):")
        hsizer3.Add(velocity_label, 0, wx.ALIGN_CENTER)
        self.velocityTextBox = wx.TextCtrl(control_panel, VELOCITY_TEXT_BOX_ID, style = wx.TE_PROCESS_ENTER)
        wx.EVT_TEXT_ENTER(control_panel, VELOCITY_TEXT_BOX_ID, self.OnVelocityChange)        
        hsizer3.Add(self.velocityTextBox, 0, wx.EXPAND)
        self.velocitySlider = wx.Slider(control_panel, VELOCITY_SLIDER_ID, value=0, minValue=0, maxValue=1000, name="Velocity", size=(300,50))
        wx.EVT_SLIDER(control_panel, VELOCITY_SLIDER_ID, self.velocitySliderUpdate)
        #hsizer3.Add(self.velocitySlider, 1, wx.EXPAND)
        
        # Force input
        hsizer4 = wx.BoxSizer(wx.HORIZONTAL)
        force_label = wx.StaticText(control_panel, -1, "Force (0.0-1.0):")
        self.forceTextBox = wx.TextCtrl(control_panel, FORCE_TEXT_BOX_ID, style = wx.TE_PROCESS_ENTER)        
        wx.EVT_TEXT_ENTER(control_panel, FORCE_TEXT_BOX_ID, self.OnForceChange)        
        hsizer4.Add(force_label, 0, wx.ALIGN_CENTER)
        hsizer4.Add(self.forceTextBox, 0, wx.EXPAND)
        self.forceSlider = wx.Slider(control_panel, FORCE_SLIDER_ID, value=0, minValue=0, maxValue=1000, name="Force", size=(300,50))
        wx.EVT_SLIDER(control_panel, FORCE_SLIDER_ID, self.forceSliderUpdate)
        #hsizer4.Add(self.forceSlider, 4, wx.EXPAND)

        vsizer1.Add(hsizer1, 0)
        vsizer1.Add(hsizer2, 0)
        vsizer1.Add(hsizer3, 0)
        vsizer1.Add(self.velocitySlider)
        vsizer1.Add(hsizer4, 0)
        vsizer1.Add(self.forceSlider)
        control_panel.SetSizer(vsizer1)


        ############################################################
        # State panel
        ############################################################
        self.state_panel = StatePanel(self, -1)
                

        ############################################################
        # Complete frame
        ############################################################
        gsizer0 = wx.BoxSizer(wx.HORIZONTAL)
        gsizer0.Add(control_panel, 0, wx.EXPAND)
        gsizer0.Add(self.state_panel, 2, wx.EXPAND)

        self.SetSizer(gsizer0)
        self.SetAutoLayout(1)
        #gsizer0.SetMinSize((300,300))
        gsizer0.Fit(self)

        self.updateInputControls()

        self.Show(True)


    def publishThreadFunc(self):
        rate = rospy.Rate(50.)
        while (not rospy.is_shutdown()):
            with self.publish_lock:
                command = self.publish_command
            self.publisher.publish(command)
            rate.sleep()
        print "Publish thread is exiting"

    def publish(self):
        with self.publish_lock:
            self.publish_command = self.command

    def handStateCallback(self, msg):
        with self.state_lock:
            self.state = msg

    def OnTick(self, event):
        #print "Tick"
        self.updateGuiState()
            
    def OnIdle(self, event):
        #print "Idle"
        self.updateGuiState()

    def updateGuiState(self):
        with self.state_lock:
            state = self.state
        if state is not None:
            self.state_panel.updateGuiState(state)

    def OnStop(self, event):
        self.command.action = RIQHandCommand.STOP
        self.publish()
        #print 'Stop'

    def OnOpen(self, event):
        self.command.action = RIQHandCommand.OPEN
        self.publish()
        #print 'Open'

    def OnClose(self, event):
        self.command.action = RIQHandCommand.CLOSE
        self.publish()
        #print 'Close'

    def OnCylindrical(self, event):
        self.command.mode = RIQHandCommand.CYLINDRICAL
        self.publish()
        #print 'Cylindrical'

    def OnPinch(self, event):
        self.command.mode = RIQHandCommand.PINCH
        self.publish()
        #print 'Pinch'

    def OnSpheriod(self, event):
        self.command.mode = RIQHandCommand.SPHERIOD
        self.publish()
        #print 'Spheriod'

    def OnScissors(self, event):
        self.command.mode = RIQHandCommand.SCISSORS
        #print 'Scissors'

    def OnQuit(self, event):
        rospy.signal_shutdown("Quit selected")
        print "Quit"

    def OnRosShutdown(self):
        print "Stopping timer..."
        self.timer.Stop()
        self.timer = None
        print "Releasing subscription"
        self.subscriber = None
        print "Waiting for publisher thread..."
        self.publish_thread.join(1.0)
        print "Shutdown complete"
        #self.Close(True)        
        self.Destroy()

    def OnCloseFrame(self, event):
        rospy.signal_shutdown("Close Frame")
        print "Close frame"

    def OnVelocityChange(self, event):
        try:
            self.command.velocity = float(self.velocityTextBox.GetValue())
            print 'Velocity', self.command.velocity
        except ValueError:
            pass
        self.updateInputControls()

    def velocitySliderUpdate(self, event):
        self.command.velocity = float(self.velocitySlider.GetValue()) / 1000.
        self.publisher.publish(self.command)
        self.updateInputControls()

    def OnForceChange(self, event):
        try:
            self.command.force = float(self.forceTextBox.GetValue())
            print 'Force', self.command.force
        except ValueError:
            self.updateInputControls()

    def forceSliderUpdate(self, event):
        self.command.force = float(self.forceSlider.GetValue()) / 1000.
        self.publisher.publish(self.command)
        self.updateInputControls()
            
    def updateInputControls(self):
        self.velocityTextBox.SetValue(str(self.command.velocity))
        self.velocitySlider.SetValue(self.command.velocity * 1000.0)
        self.forceTextBox.SetValue(str(self.command.force))
        self.forceSlider.SetValue(self.command.force * 1000.0)

            

def main():
    if len(sys.argv) > 1:
        usage()
        exit(2)
        
    app = wx.PySimpleApp()
    rospy.init_node('riq_hand_cli', anonymous=True, disable_signals=False)
    mw = MainWindow(None, -1, "RobotIQ hand GUI")
    app.MainLoop()
    print "Mainloop complete"

    return 0
    
        

if __name__ == '__main__':
    main()
