#!/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.

# This script will send commands to robotIQ driver
#
# Author: Derek King

"""
Usage:  riq_hand_cli

Description:
  Provides command-line-interface to RobotIQ hand EtherCAT driver.

Args:  
  NONE YET.
"""

PKG = 'riq_hand_cli'

import roslib
roslib.load_manifest(PKG)
import rospy
import sys
import readline
import re
from riq_msgs.msg import RIQHandCommand


publisher = None
command = None

def shutdown():
    print
    if (publisher is not None) and (command is not None):
        print "Sending STOP"
        command.action = RIQHandCommand.STOP
        publisher.publish(command)
        publisher.publish(command)
        rospy.sleep(0.5)
    print "Shutting down"

def help():
    print """
Valid options are:
 s[top], o[pen], c[lose]
 f[orce] <0.0-1.0>, v[elocity] <0.0-1.0>
 cyl[indrical], p[inch], sph[eriod], sc[issors]
"""
    
def usage():
  print __doc__

def main():
    if len(sys.argv) > 1:
        usage()
        exit(2)

    global publisher, command
    rospy.init_node('riq_hand_cli', anonymous=True)
    publisher = rospy.Publisher('riq_hand_command', RIQHandCommand)
    rospy.sleep(0.5)
    command = RIQHandCommand(RIQHandCommand.CYLINDRICAL, RIQHandCommand.STOP, 0.0, 0.0)
    publisher.publish(command)
    rospy.on_shutdown(shutdown)

    help()
    while not rospy.is_shutdown():
        try:
            line = raw_input('> ')            
            if re.match('s(top)? *$',line,re.IGNORECASE) or len(line) == 0:
                print 'stop'
                command.action = RIQHandCommand.STOP                
            elif re.match('o(pen)? *$',line,re.IGNORECASE):
                print 'open'
                command.action = RIQHandCommand.OPEN
            elif re.match('c(lose)? *$',line,re.IGNORECASE):
                print 'close'
                command.action = RIQHandCommand.CLOSE
            elif re.match('cyl(indrical)? *$',line,re.IGNORECASE):
                print 'cylindrical'
                command.mode = RIQHandCommand.CYLINDRICAL
            elif re.match('p(inch)? *$',line,re.IGNORECASE):
                print 'pinch'
                command.mode = RIQHandCommand.PINCH
            elif re.match('sph(eriod)? *$',line,re.IGNORECASE):
                print 'spheriod'
                command.mode = RIQHandCommand.SPHERIOD
            elif re.match('sc(issors)? *$',line,re.IGNORECASE):
                print 'scissors'
                command.mode = RIQHandCommand.SCISSORS
            elif re.match('v(elocity)? \d+\.?\d* *$',line,re.IGNORECASE):
                value = line.split()[1]
                command.velocity = float(value)
                print 'velocity %f' % command.velocity
            elif re.match('f(orce)? \d+\.?\d* *$',line,re.IGNORECASE):
                value = line.split()[1]
                command.force = float(value)
                print 'force %f' % command.force
            else:
                help()
            publisher.publish(command)
            #print command
        except ValueError,e:
            print "Invalid float : '%s'" % value
        except EOFError:
            break
        

if __name__ == '__main__':
    main()
