#!/usr/bin/env python

import math
import Tkinter as tk
import rospy
from articulating_hand.msg import ServoArray, ServoDrive

class Sliders(object):
    """
    Contains functions for creating joint control slider bars and converting their
    values into robot motion.
    """
    def __init__(self):
        rospy.init_node('slider_gui')           # initialize node

        self.servo_limits = rospy.get_param("servo_limits")     # load servo ranges

        self.joint_names = rospy.get_param("joint_names")       # load list of joints

        self.slider_inits = rospy.get_param("joint_inits")[1]   # initial slider bar positions

        self.root = tk.Tk()                     # create GUI root window
        self.root.geometry("800x425+30+30")     # define root window size

        self.sliders = []                       # empty list to hold slider bar objects

        # initialize publisher
        self.move_servo = rospy.Publisher('servo_cmd', ServoArray, queue_size=10)

        self.create_sliders()                   # create slider bars

    def create_sliders(self):
        """
        Creates the joint control GUI window by initializing and placing Tkinter
        slider bar objects.
        """
        for i in range(len(self.joint_names)): #fill in array once broken servo fixed
            srvrange = self.servo_limits[i][1] - self.servo_limits[i][0]
            if i < 8:                       # finger joints
                # create slider bar object
                j = tk.Scale(self.root, from_=self.servo_limits[i][1],
                             to_=self.servo_limits[i][0], tickinterval=srvrange,
                             length=100)
                c_a = math.floor(i/2)       # define positioning constants
                if (i-1) % 2 == 0:
                    c_b = 0
                else:
                    c_b = 1
                c_e = 0
                c_f = 0
            elif i == 8:                    # CMC joint
                j = tk.Scale(self.root, from_=self.servo_limits[i][0],
                             to_=self.servo_limits[i][1], tickinterval=srvrange,
                             length=100)
                c_a = math.floor(i/2)       # positioning constants vary based on joint
                c_b = 2
                c_e = 0
                c_f = 0
            else:                           # thumb joints
                j = tk.Scale(self.root, from_=self.servo_limits[i][0],
                             to_=self.servo_limits[i][1], tickinterval=srvrange,
                             length=100, orient='horizontal')
                c_a = i - 4                 # joint "column" multiplier
                c_b = 2                     # joint "row" multiplier
                c_e = 25*(i-9)              # horizontal offset
                c_f = 25                    # vertical offset

            # set slider initial position
            j.set(self.slider_inits[i])

            # place slider bar object in root window with x- and y- coordinates
            j.place(x=(100*c_a)+25+c_e, y=(125*c_b)+25+c_f)

            self.sliders.append(j)          # add slider bar object to list

    def servo_move(self):
        """
        Creates motion at each sequential joint by driving its servo to the position
        set by its slider bar.
        """
        servo_commands = ServoArray()

        for i in range(len(self.joint_names)):
            command = ServoDrive()                  # initialize servo command object

            # servo command details
            command.servo_num = i
            command.servo_pos = self.sliders[i].get()

            servo_commands.srv_comms.append(command)

        # publish servo command
        self.move_servo.publish(servo_commands)

def main():
    """
    Main function; initializes Sliders class and repeatedly calls its servo actuation
    function.
    """
    S = Sliders()               # call Sliders class
    rate = rospy.Rate(50)       # 50 Hz publishing rate
    while not rospy.is_shutdown():
        rate.sleep()            # wait 1/50 of a second (prevents comms delay between GUI and RViz)
        S.root.update()         # update GUI window
        S.servo_move()          # execute servo motion

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
