################################################################################
# TangibleTec, LLC                                                             #
# Ruben Kackstaetter                                                           #
#                                                                              #
# License:                                                                     #
# Apache License, Version 2.0                                                  #
# http://www.apache.org/licenses/LICENSE-2.0                                   #
#                                                                              #
# As noted in the terms of the Leap Motion SDK Agreement                       #
# https://developer.leapmotion.com/sdk_agreement                               #
#                                                                              #
# Purpose:                                                                     #
# Control an RGB LED connected to an Arduino                                   #
#                                                                              #
# Wiring:                                                                      #
# USB to Arduino running PWM_serial.ino                                        #
################################################################################

import sys
sys.path.append('C:\\LeapSDK\\lib')
sys.path.append('C:\\LeapSDK\\lib\\x86')

import Leap, thread, time, serial, colorsys
import serial.tools.list_ports
from Leap import Finger

# Auto detect Arduino Serial Port and Configure
for p in list(serial.tools.list_ports.comports()):
    if "Arduino" in p[1]:
        PORT = p[0]
        print "Arduino on port: %s" %PORT
BAUD = 115200

class SampleListener(Leap.Listener):
    finger_names = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky']
    ser = serial.Serial(PORT, BAUD)
    frame_cnt = 0

    def on_init(self, controller):
        print "Initialized"
        self.send_rgb([210,45,0]) #Yellow

    def on_connect(self, controller):
        print "Connected"
        self.send_rgb([180,255,22]) #Green

    def on_disconnect(self, controller):
        # Note: not dispatched when running in a debugger.
        print "Disconnected"

    def on_exit(self, controller):
        print "Exited"

    def on_frame(self, controller):
        # Get the most recent frame and report some basic information
        frame = controller.frame()
        self.frame_cnt += 1

        if self.frame_cnt % 2 == 0:
            # Get hands
            for hand in frame.hands:

                handType = "Left hand" if hand.is_left else "Right hand"

                is_pointing = True if hand.grab_strength == 0.0 else False
                
                print "  %s, id %d, pointing: %s" % (
                    handType, hand.id, is_pointing)

                # Get fingers
                index_fingers = hand.fingers.finger_type(Finger.TYPE_INDEX)
                for finger in index_fingers:
                
                    tpos = finger.tip_position
                
                    print "   %s finger, tip: %s" % (
                        self.finger_names[finger.type],
                        tpos)
                
                    if is_pointing:
                        rgb = self.pos_to_rgb(tpos.x, tpos.y, tpos.z)
                        self.send_rgb(rgb)

                
    def pos_to_rgb(self, px, py, pz):
    
        hue = (0.6375 * px + 127.5) % 256.0
        
        sat = 255.0 #pz
        
        if py < 25.0:
            lum = 0.0
        elif py > 380.0:
            lum = 255.0
        else:
            lum = 0.5595 * py -13.546
        
        return self.hsl_to_rgb(hue, sat, lum)
        
    def hsl_to_rgb(self, hue, sat, lum):
        rgb_dec = colorsys.hls_to_rgb(hue/255.0, lum/255.0, sat/255.0)
        rgb = [255.0 * i for i in rgb_dec]
        return rgb
        
    def send_rgb(self, rgb):
        cmd_str = str(map(int, rgb))[1:-1] + '\n'
        print "       cmd_str: %s" % cmd_str
        self.ser.write(cmd_str.encode())
        
    

    
def main():
    # Create a sample listener and controller
    listener = SampleListener()
    controller = Leap.Controller()

    # Have the sample listener receive events from the controller
    controller.add_listener(listener)

    # Keep this process running until Enter is pressed
    print "Press Enter to quit..."
    
    try:
        sys.stdin.readline()
    except KeyboardInterrupt:
        pass
    finally:
        # Remove the sample listener when done
        controller.remove_listener(listener)


if __name__ == "__main__":
    main()