## VRHandler.py
## author: Ben Buchwald
## last updated 10/20/2004
##
## VRPN base for using tracking in Panda.
## primarily built to work with Spacepads in the BVW class.

import math
from pandac.PandaModules import *            # panda functionality
from pandac import CoordinateSystem
from direct.task import Task

class VRHandler:
    def __init__(self,
                 server=None, # VRPN server
                 parent=None,           # scenegraph parent of root tracker object
                 trackerDevice=None,    # default tracker device name
                 buttonDevice=None,     # default button device name
                 analogDevice=None):    # default analog device name

        self.VRPN = None                # VRPN client object
        self.trackerDevice = trackerDevice
        self.buttonDevice = buttonDevice
        self.analogDevice = analogDevice

        if not parent:
                parent = render
        self.rootHelper = parent.attachNewNode("TrackerRoot")

        self.trackerHelpers = {}
        self.trackerNodePaths = {}

        self.buttonNodePaths = {}
        self.analogNodePaths = {}

        if (server==None):
            server = base.config.GetString("vrpn-server","imve18")
        self.VRPN = VrpnClient( server )

        # check if vrpn client inits successfully
        if not self.VRPN.isConnected():
            print 'VRHandler::__init__. Warning. VRPN Client connection unsuccessful'
    # end __init__

    def close(self):
        for name in self.trackerNodePaths:
            self.removeTracker(name)
        self.rootHelper.remove()
    # end close()

    def createTracker(self,tracker,name=None, toSG=True):
        if type(tracker)==int:
            tracker = self.trackerDevice+":"+str(tracker)
        else:
            if self.trackerDevice==None:
                self.trackerDevice = tracker[:tracker.index(":")]
            
        if not name:
            name = tracker

        trackerNode = TrackerNode(self.VRPN,tracker)
        trackerNodePath = base.dataRoot.attachNewNode(trackerNode)
        self.trackerNodePaths[name]=trackerNodePath
        trackerHelper = self.rootHelper.attachNewNode(name+"Helper")
        if toSG:
            tracker2SG = Transform2SG("Tracker2SG-"+name)
            tracker2SG.setNode(trackerHelper.node())
            trackerNodePath.attachNewNode(tracker2SG)
        
        self.trackerHelpers[name]=trackerHelper

        return trackerHelper
    # end createTracker()

    def removeTracker(self,name):
        if name in self.trackerNodePaths:
            self.trackerNodePaths[name].remove()
            del self.trackerNodePaths[name]
            self.trackerHelpers[name].remove()
            del self.trackerHelpers[name]
    # end removeTracker

    def getRootHelper(self):
        return self.rootHelper
    # end getRootHelper

    def getHelper(self,name):
        return self.trackerHelpers[name]
    # end getTracker

    def __getButtonNode(self,button):
        if type(button)==int:
            buttonDevice = self.buttonDevice
            index = button
        else:
            colon = button.index(":")
            buttonDevice = button[:colon]
            index = int(button[colon+1:])
            if self.buttonDevice==None:
                self.buttonDevice = buttonDevice

        if self.buttonNodePaths.has_key(buttonDevice):
            buttonNode = self.buttonNodePaths[buttonDevice].node()
        else:
            buttonNode = ButtonNode(self.VRPN,buttonDevice)
            buttonNodePath = base.dataRoot.attachNewNode(buttonNode)
            self.buttonNodePaths[buttonDevice] = buttonNodePath
            buttonThrower = ButtonThrower("VRHandler-ButtonThrower")
            buttonNodePath.attachNewNode(buttonThrower)

        return (buttonNode,index)
    # end __getButtonNode

    def setButtonEvent(self,button,event):
        buttonNode,index = self.__getButtonNode(button)
        
        if event!=None:
            buttonHandle = ButtonRegistry.ptr().getButton(event)
        else:
            buttonHandle = ButtonHandle.none()

        buttonNode.setButtonMap(index,buttonHandle)
    # end setButtonEvent

    def getButtonState(self,button):
        buttonNode,index = self.__getButtonNode(button)
        return buttonNode.getButtonState(index)
    # end getButtonState

    def getNumButtons(self,buttonDevice=0):
        if type(buttonDevice)==str:
            try:
                buttonDevice.index(":")
            except:
                buttonDevice+=":0"
        buttonNode,index = self.__getButtonNode(buttonDevice)
        return buttonNode.getNumButtons()
    # end getNumButtons

    def __getAnalogNode(self,analog):
        if type(analog)==int:
            analogDevice = self.analogDevice
            index = analog
        else:
            colon = analog.index(":")
            analogDevice = analog[:colon]
            index = int(analog[colon+1:])
            if self.analogDevice==None:
                self.analogDevice = analogDevice

        if self.analogNodePaths.has_key(analogDevice):
            analogNode = self.analogNodePaths[analogDevice].node()
        else:
            analogNode = AnalogNode(self.VRPN,analogDevice)
            analogNodePath = base.dataRoot.attachNewNode(analogNode)
            self.analogNodePaths[analogDevice] = analogNodePath

        return (analogNode,index)
    # end __getAnalogNode

    def getAnalogState(self,analog):
        analogNode,index = self.__getAnalogNode(analog)
        return analogNode.getControlState(index)
    # end getAnalogState

    def getNumAnalogs(self,analogDevice=0):
        if type(analogDevice)==str:
            try:
                analogDevice.index(":")
            except:
                analogDevice+=":0"
        analogNode,index = self.__getAnalogNode(analogDevice)
        return analogNode.getNumControls()
    # end getNumAnalogs
            
# end class VRHandler
    
class Spacepad(VRHandler):
    #hmdMat = Mat4( -0.0933044, 0.00534243, -0.995623, 0,
    #           -0.699765, 0.710995, 0.0693933, 0,
    #            0.708254, 0.703177, -0.0626005, 0,
    #           -0.65, 0.3, -0.39, 1 )
    hmdMat = Mat4( 0, 0, -1, 0,
                   -math.sqrt(2)/2, math.sqrt(2)/2, 0, 0,
                   math.sqrt(2)/2, math.sqrt(2)/2, 0, 0,
                   -0.176777, -0.0589256, -0.458333, 1 )

    def __init__(self,
                 server=None, # VRPN server
                 height=7.5,            # height of the spacepad
                 parent=None,           # scenegraph parent of root tracker object
                 #trackerDevice="SpacePad", # default tracker device name
                 trackerDevice="Tracker0", # default tracker device name
                 buttonDevice="GameportJoystick", # default button device name
                 analogDevice="GameportJoystick"): # default analog device name
        VRHandler.__init__(self,server,parent,trackerDevice,buttonDevice,analogDevice)

        self.rootHelper.setName("Spacepad")
        self.rootHelper.setZ(7.5)

        self.spacepadHelper = self.rootHelper.getParent().attachNewNode("SpacepadBase")
        self.rootHelper.reparentTo(self.spacepadHelper)

        self.hmdHelper = None
    # end __init__

    def getSpacepadHelper(self):
        return self.spacepadHelper
    # end getSpacepadHelper

    def getHMDHelper(self):
        red = self.getRedHelper()
        if (self.hmdHelper==None):
            self.hmdHelper = red.attachNewNode("hmd")
            self.hmdHelper.setMat(Spacepad.hmdMat)
        return self.hmdHelper
    # end getHMDHelper

    def getRedHelper(self):
        if not self.trackerHelpers.has_key("red"):
            self.createTracker(0,"red")
        return self.getHelper("red")
    # end getRedTracker

    def getYellowHelper(self):
        if not self.trackerHelpers.has_key("yellow"):
            self.createTracker(1,"yellow")
        return self.getHelper("yellow")
    # end getYellowTracker

    def getGreenHelper(self):
        if not self.trackerHelpers.has_key("green"):
            self.createTracker(2,"green")
        return self.getHelper("green")
    # end getGreenTracker

    def getBlueHelper(self):
        if not self.trackerHelpers.has_key("blue"):
            self.createTracker(3,"blue")
        return self.getHelper("blue")
    # end getRedTracker

    def setHeight(self,height):
        self.rootHelper.setZ(height)
    # end setHeight
# end class Spacepad
