#! /usr/bin/env python

# Copyright (C) 2011 Andre Gouws
#
#    This program is free software: you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation, either version 3 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program.  If not, see <http://www.gnu.org/licenses/>

import sys
import time
from ConfigParser import SafeConfigParser
from collections import deque
import numpy as np
from OpenGL.GL import *
from OpenGL.GLUT import *
from OpenGL.GLU import *
from OSCeleton import *
import threading
import vtk
from ynicKinectHandActor import AddHandActor
from ynicKinectAlgorithms import *
from ynicKinectVTKObjects import *
from ynicKinectHandPositions import *
from ynicKinectAddVTKButtons import addButtons
from ynicKinectClippingFunctions import *


#----------------------------------
#some variables
global renderer, prev_zoom, renderWindow, txtLeftHand, txtRightHand, combinedHands, right_hand_pos, left_hand_pos, l_h_actor2d

SIZE_X = 160
SIZE_Y = 120
TARGET_SIZE = 0
REMOVE_AFTER = 200
usersTargets = []
server = OSCeleton(7110)
server.realWorld = True
frameCount = 0
users = {}
lastDisplayed = 0.0

vtkScreenX = 640
vtkScreenY = 480

left_hand_pos = (0,0,0)
right_hand_pos = (0,0,0)
prev_left_hand_pos = (0,0,0)
prev_right_hand_pos = (0,0,0)
prev_zoom = 1
#----------------------------------



## start of skeleton tracking / window code -----------------

class Player(Skeleton):
    """Holds a players data.  Inherits from Skeleton.
        
    Player.last is the time at which this player's joints were updated. 
    """
    
    def __init__(self, user_id):
        Skeleton.__init__(self, user_id)
        self.hits = 0
        self.last = 0.0
        
    def still_moving(self):
        """Tests whether the Player's joints have been updated recently"""
        return time.time() - self.last < REMOVE_AFTER
        
class Target(Point):
    """Stores target information"""
    def __init__(self, x, y, z):
        Point.__init__(self, x, y, z)
        self.baseJoint = ""
        self.middleJoint = ""
        self.hitJoint = ""
        self.calcLen = False

    
def getRGB(joint, colorRange = 800):
    """Returns a tuple with r, g and b color values based on joint's Z.
    
    Ugly but does the job, needs to be completely rewritten"""
    z = joint.z
    z = z % colorRange #smaller range gives more noticeable transitions
    subInterval = colorRange / 6.0
    rgb = [0, 0, 0]
    if z < subInterval:
        rgb[0] = 1.0
        rgb[1] = z / subInterval
    elif z < 2 * subInterval:
        rgb[1] = 1.0
        rgb[0] = (2 * subInterval - z) / subInterval
    elif z < 3 * subInterval:
        rgb[1] = 1.0
        rgb[2] = (z - 2 * subInterval) / subInterval
    elif z < 4 * subInterval:
        rgb[2] = 1.0
        rgb[1] = (4 * subInterval - z) / subInterval
    elif z < 5 * subInterval:
        rgb[2] = 1.0
        rgb[0] = (z - 4 * subInterval) / subInterval
    else:
        rgb[0] = 1
        rgb[2] = (colorRange - z) / subInterval
    return tuple(rgb)


def drawLine(player, jointLabel1, jointLabel2):
    global left_hand_pos, right_hand_pos, prev_left_hand_pos, prev_right_hand_pos
    """Accepts a skeleton and two joint labels.
    Draws a colored line between the skeleton's two joints that have the desired labels"""
    
    if (jointLabel1, jointLabel2) in player:
        joint1 = player[jointLabel1]
        joint2 = player[jointLabel2]
        if jointLabel2 == 'l_hand':
            prev_left_hand_pos = left_hand_pos #before assigning the new value keep track of previous
            left_hand_pos = joint2
        if jointLabel2 == 'r_hand':
            prev_right_hand_pos = right_hand_pos #before assigning the new value keep track of previous
            right_hand_pos = joint2	
        
        r, g, b = getRGB(joint1)
        glColor3f(r, g, b)
        glVertex3f(joint1.x, joint1.y, joint1.z)
        r, g, b = getRGB(joint2)
        glColor3f(r, g, b)
        glVertex3f(joint2.x, joint2.y, joint2.z)
            
def glutIdle():
    """Registered as GlutIdleFunc.
    
    Catches server events, adds and removes users and loads newest Skeletons"""
    global frameCount, users
    server.run()
    if server.frames > frameCount or server.lostUsers:
        if server.lostUsers:
            try:
                for each in server.lostUsers:
                    del users[each]
                    glutPostRedisplay()
            except KeyError:
                pass
            del server.lostUsers[:]
        for player in server.get_new_skeletons():
            if player.id not in users:
                users[player.id] = Player(player.id)
            users[player.id].joints = player.copy_joints()
            users[player.id].last = time.time()
            frameCount = server.frames
            glutPostRedisplay()
    elif time.time() - lastDisplayed > REMOVE_AFTER:
        glutPostRedisplay()


def drawPlayers():
    """Draws lines connecting available joints for every player in users"""
    glBegin(GL_LINES)
    for player in users.values():
        if player.still_moving():
            drawLine(player, HEAD, NECK)
            drawLine(player, NECK, TORSO)
            drawLine(player, LEFT_SHOULDER, LEFT_HIP)
            drawLine(player, RIGHT_SHOULDER, RIGHT_HIP)
            drawLine(player, LEFT_HIP, RIGHT_HIP)
            drawLine(player, LEFT_HIP, LEFT_KNEE)
            drawLine(player, LEFT_KNEE, LEFT_FOOT)
            drawLine(player, RIGHT_HIP, RIGHT_KNEE)
            drawLine(player, RIGHT_KNEE, RIGHT_FOOT)
            drawLine(player, LEFT_SHOULDER, RIGHT_SHOULDER)
            drawLine(player, LEFT_SHOULDER, LEFT_ELBOW)
            drawLine(player, LEFT_ELBOW, LEFT_HAND)   
            drawLine(player, RIGHT_SHOULDER, RIGHT_ELBOW)
            drawLine(player, RIGHT_ELBOW, RIGHT_HAND)
    glEnd()
 
    
def getPlayersOrientation(player):
    """Determines a users orientation.
    
    Accepts a Skeleton and returns a Point.
    
    Calculates orientation by using 3 points to create two vectors
    and then cross multiplies those vectors to find a vector perpindicular to both"""
    if (TORSO, LEFT_SHOULDER, RIGHT_SHOULDER) in player:
        torso = player[TORSO]
        tl = player[LEFT_SHOULDER] - player[TORSO]
        tr = player[RIGHT_SHOULDER] - player[TORSO]
        tl.normalize()
        tr.normalize()
        orientationArray = np.cross(tr.vals(), tl.vals())
        orientation = Point(orientationArray[0], orientationArray[1], orientationArray[2]) 
        orientation.normalize()
    else:
        orientation = Point(1, 1, 1)
        orientation.normalize()
    return orientation


def glutDisplay():
    """Registered as GlutDisplayFunc.  Calls all drawing functions"""
    global last_displayed
    lastDisplayed = time.time()
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glMatrixMode(GL_PROJECTION)
    glShadeModel(GL_SMOOTH)
    glLineWidth(5)
    glLoadIdentity()
    drawPlayers()
    glFlush()
    glutSwapBuffers()


def runSkeletonWindow():
    if len(sys.argv) > 1:
        iniFile = sys.argv[1]
    else:
        iniFile = "generic.ini"
    #getTargets(iniFile)
    glutInit(sys.argv)
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH)
    glutInitWindowSize(SIZE_X, SIZE_Y)
    glutCreateWindow("mySkeleton")
    glOrtho(-1280, 1280, -960, 960, 0, 10000)
    glEnable(GL_DEPTH_TEST)
    glutDisplayFunc(glutDisplay)
    glutIdleFunc(glutIdle)
    glLineWidth(5)
    glutMainLoop()

## end of skeleton tracking / window code -----------------




def doTheVTKStuff():
    global renderer, prev_zoom, renderWindow, left_hand_pos, right_hand_pos, prev_left_hand_pos, prev_right_hand_pos, txtLeftHand, txtRightHand, combinedHands, l_h_actor2d

    #Create a mapper and actor for the brain

    my_mnorm_brain, mn_Mapper = CreateVTKPolyDataSurface('../mni_surface_5500_3mm.vtk')
    #actorBrain.SetMapper(mn_Mapper)
    mn_Mapper.Update()
    
    #Create a mapper and actor for the left hand
    txtLeftHand = AddHandActor((30,30), './icons/left_hand.jpg')
    txtRightHand = AddHandActor((vtkScreenX-30,30), './icons/right_hand.jpg')
    combinedHands = AddHandActor((vtkScreenX/2,30), './icons/both_hand.jpg')

    # Setup a renderer, render window, and interactor
    renderer = vtk.vtkRenderer()
    renderWindow = vtk.vtkRenderWindow()
    #renderWindow.SetWindowName("Test")

    renderWindow.AddRenderer(renderer);
    renderWindowInteractor = vtk.vtkRenderWindowInteractor()
    renderWindowInteractor.SetRenderWindow(renderWindow)
    renderWindowInteractor.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera()) 

    #add some buttons first so they are in the background
    addButtons(renderer, vtkScreenX, vtkScreenY)

    #Add the actors to the scene
    #renderer.AddActor(actorBrain)
    renderer.AddActor(txtLeftHand)
    renderer.AddActor(txtRightHand)
    renderer.AddActor(combinedHands) 
    renderer.SetBackground(1,1,1) # Background color white
    
    clipData(mn_Mapper, renderer, renderWindowInteractor)
    
    #Render and interact
    renderWindow.Render()

    # Initialize must be called prior to creating timer events.
    renderWindowInteractor.Initialize()

    # Sign up to receive TimerEvent
    ##timerId = renderWindowInteractor.CreateRepeatingTimer(1);
    ##renderWindowInteractor.AddObserver('TimerEvent', checkTheZoomer)
    ##renderWindowInteractor.AddObserver('TimerEvent', getKinectInfo)

    ##rw = renderWindowInteractor.GetRenderWindow()
    renderWindow.SetSize(vtkScreenX, vtkScreenY)
    
    cam = renderer.GetActiveCamera()
    
    #here we run our algorithms that control the movement gesture input
    while True:
        time.sleep(0.05) #loops just don't seem to work without this .. time to tal superMark
        clipPlanePositions()
        if left_hand_pos != (0,0,0): #not tuple basically
            #first lets see where the hands are
            hand_diff, lhpx_vtk, lhpy_vtk, rhpx_vtk, rhpy_vtk = checkHandPos(vtkScreenX, vtkScreenY, left_hand_pos, right_hand_pos)
            #print hand_diff
            if hand_diff > 100: #the hands are separate in the window
                #update the hand actor positions
                updateIndividualHandPos(lhpx_vtk, lhpy_vtk, rhpx_vtk, rhpy_vtk, renderer, prev_zoom, renderWindow, right_hand_pos, left_hand_pos, txtLeftHand, txtRightHand, combinedHands)
                #check if the user made a horz sweep with the right hand
                checkSetAzimuth(cam, renderer, prev_zoom, renderWindow, right_hand_pos, prev_right_hand_pos)
                #check if the user made a vert sweep with the left right hand                
                checkSetElevation(cam, renderer, prev_zoom, renderWindow, left_hand_pos, prev_left_hand_pos)
                #check if the users hands are over buttons
                checkButtonPress(renderer, left_hand_pos, right_hand_pos, lhpx_vtk, lhpy_vtk, rhpx_vtk, rhpy_vtk, vtkScreenX, vtkScreenY, prev_zoom, txtLeftHand, txtRightHand, renderWindow, combinedHands)
            else:
                #the hands are together
                updateCombinedHandPos(lhpx_vtk, lhpy_vtk, rhpx_vtk, rhpy_vtk, renderer, prev_zoom, renderWindow, right_hand_pos, left_hand_pos, txtLeftHand, txtRightHand, combinedHands, )
                checkSetZoom(cam, renderer, renderWindow, right_hand_pos, prev_right_hand_pos)
        else:
            pass
    #start the interaction and timer
    renderWindowInteractor.Start()


if __name__ == "__main__":
    #doTheVTKStuff()
    t = threading.Thread(target=doTheVTKStuff, kwargs={})
    t.start()
    runSkeletonWindow()
    #t2 = threading.Thread(target=runSkeletonWindow, kwargs={})
    #t2.start()
    
    
    
    





