def main():
	global loc_list
	global active_location
	loc_list = []
	
	mainSceneWindow = None
	cameraWindow = None
	cameraWindowView = None
	cam = None
	pause_screen = None
	nunchuck_disconnect_screen = None
	message_screen = None
	wiimote = None
	nunchuck_wiimote = None
	should_it_run = None
	
	mainSceneWindow, cameraWindow, cameraWindowView, cam, pause_screen, nunchuck_disconnect_screen, message_screen, wiimote, nunchuck_wiimote, should_it_run = init_settings(mainSceneWindow, cameraWindow, cameraWindowView, cam, pause_screen, nunchuck_disconnect_screen, message_screen, wiimote, nunchuck_wiimote, should_it_run)
	
	vizact.ontimer(0, moveCamera, nunchuck_wiimote, should_it_run)
	viz.callback(wii.EXT_CONNECT_EVENT,onConnect)
	viz.callback(wii.EXT_DISCONNECT_EVENT,onDisconnect)
	
	initializer(loc_list)
	arMarkerLoader(cam)
	wiimoteInitializer(wiimote, nunchuck_wiimote, should_it_run, message_screen)
	
	viz.go() # Inicia VIZ
	
	for item in loc_list:
		if item.space_name == "piazza.osgb":
			active_location = item
			item.addLocation()

    def setup_world(self):
        viz.setOption('viz.fullscreen.monitor', 1)
        #viz.setOption('viz.window.width', 2560)
        #viz.setOption('viz.window.height', 1040)
        #viz.setMultiSample(4)
        #viz.MainWindow.clip(0.01, 500)
        #viz.vsync(1)

        vizconfig.register(nvis.nvisorSX111())

        viz.go(viz.FULLSCREEN)

def main():
    global TRACKER

    viz.go(0)

    # configure the phasespace.
    mocap = vrlab.Phasespace('192.168.1.230', freq=100., postprocess=True)
    mocap.start_thread()
    TRACKER = mocap.track_points(M)

    viztask.schedule(workflow)

	def go(self):
		"""Use this command to start the application."""
		
			
		if self.__cave_mode:
			viz.go(viz.STEREO | viz.FULLSCREEN)
		else:
			viz.go(viz.FULLSCREEN)
				
		viz.mouse.setOverride(viz.ON)
		
		self.callback(viz.UPDATE_EVENT,self.__onUpdate)

def main():

    viz.vsync(viz.ON)
    viz.window.setFullscreenMonitor([2])
    viz.setMultiSample(4)
    viz.MainWindow.clip(0.01, 200)

    viz.go(viz.FULLSCREEN)

    environment = viz.addChild("piazza.osgb")
    environment.setPosition(2.75, 0, -0.75)

    mocap = phasespace.Phasespace()

    head = mocap.get_rigidTracker("hmd")

    # DOES NOT ACCOUNT FOR RIGID BODY OFFSET DURING RESET

    # mocap.track_rigid('Resources/hmd-nvisMount.rb', center_markers=(1,2))
    head.link_pose(viz.MainView)

    # glove = mocap.track_points([8, 9, 10])

    paddle = mocap.track_rigid({17: (0, 0, 1), 19: (0, 0, 0), 20: (1, 0, 0), 22: (0, 1, 0)})

    mocap.start_thread()
    # mocap.start_timer()

    def log_mocap(timer_id):
        # print 'glove#9 pos {0[1]:.2f} {0[1]:.2f} {0[2]:.2f}'.format(*glove.get_marker(9))
        print "head    pos {0[0]:.2f} {0[1]:.2f} {0[2]:.2f}, quat {1[0]:.3f} {1[1]:.3f} {1[2]:.3f} {1[3]:.3f}".format(
            *head.get_pose()
        )
        print "main    " + str(viz.MainView.getPosition())
        # print 'paddle  pos {0[0]:.2f} {0[1]:.2f} {0[2]:.2f}, quat {1[0]:.3f} {1[1]:.3f} {1[2]:.3f} {1[3]:.3f}'.format(*paddle.get_pose())

    viz.callback(viz.TIMER_EVENT, log_mocap)
    viz.starttimer(0, 1, viz.FOREVER)

    def keydown(key):

        if key == "h":
            head.reset()
        if key == "H":
            head.save()
        elif key == "1":

            print "Marker Pos: " + str(mocap.get_MarkerPos(1))

    viz.callback(viz.KEYDOWN_EVENT, keydown)

	def __init__(self):

		viz.go(viz.FULLSCREEN |viz.QUAD_BUFFER)
		viz.window.setFullscreen(1)

		#####################
		#setup CAVE
		polyMode = viz.POLY_WIRE
		#viz.setFarPlane(1)
		#viz.setMultiSample(4) #didn't observe a difference in landscapes with anti-aliasing.
		viz.window.setPolyMode(viz.POLY_WIRE)
		viz.vsync(viz.ON)#this may increase frame rate but may get tears
		viz.ipd(0.2) #20 cm apart eyes seemed right for our scaling.

		self.cave = vizcave.Cave()

	def __setupVizParams__(self):
		
		#Just playing here#
		#viz.setMultiSample(8)
		#viz.fov(120)
		
		#Check monitor refresh rate and set stereo mode accordingly#
		monitor_RefreshRate = viz.getOption('viz.monitor.refresh_rate', type=int)
		#if(119 <= monitor_RefreshRate):
			#Turn on the physics engine
		viz.phys.enable() 
		viz.window.setFullscreenMonitor(2)
		viz.setMultiSample(8)
		viz.go(viz.FULLSCREEN | viz.QUAD_BUFFER)
		print "here"

def InitHMD():
	global collisionLimbBoxes
	if(USE_HMD):
		pptExtension.setup();
		viz.go(viz.FULLSCREEN)
		viz.clip(0.1)
		vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);
		if(len(collisionLimbBoxes) > 0):
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes);
		else:
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None);
	else:
		pptSimulation.setup(avatar);
		viz.clip(0.1)
		viz.go(viz.FULLSCREEN);
		vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);

def InitHMD():
#BL:start		
	global collisionLimbBoxes
#	if USE_HMD:
	pptextension.setup()
	if some_global_variables.headTrackingActive:
		viz.go(viz.HMD | viz.TRACKER)
	else:
		viz.go()
	viz.window.setSize(1260, 950)		
	#viz.go(viz.FULLSCREEN)
#BL:end
	viz.clip(0.1)
	vizact.onupdate(0, pptextension.pptUpdate, some_global_variables.avatar, some_global_variables.ghostAvatar, collisionLimbBoxes);
	if(len(collisionLimbBoxes) > 0):
		vizact.ontimer(1, pptextension.refreshAvatar, some_global_variables.ghostAvatar, some_global_variables.avatar, collisionLimbBoxes);
	else:
		vizact.ontimer(1, pptextension.refreshAvatar, some_global_variables.ghostAvatar, some_global_variables.avatar, None);

def main():
    viz.setOption('viz.fullscreen.monitor', 1)
    viz.setOption('viz.window.width', 2 * 640)
    viz.setOption('viz.window.height', 480)
    viz.setMultiSample(4)
    viz.MainWindow.clip(0.01, 500)

    vizconfig.register(nvis.nvisorSX111())

    viz.go(viz.FULLSCREEN)

    piazza = viz.addChild('piazza.osgb')

    mocap = phasespace.Mocap('192.168.1.230')

    head = mocap.track_rigid('Resources/hmd-nvis.rb', center_markers=(0, 5))
    head.link_pose(viz.MainView)

    glove = mocap.track_points([8, 9, 10])

    paddle = mocap.track_rigid({
      17:(0, 0, 1),
      19:(0, 0, 0),
      20:(1, 0, 0),
      22:(0, 1, 0),
    })

    mocap.start_thread()
    #mocap.start_timer()

    def log_mocap(timer_id):
        print 'glove#9 pos {0[1]:.2f} {0[1]:.2f} {0[2]:.2f}'.format(*glove.get_marker(9))
        print 'head    pos {0[0]:.2f} {0[1]:.2f} {0[2]:.2f}, quat {1[0]:.3f} {1[1]:.3f} {1[2]:.3f} {1[3]:.3f}'.format(*head.get_pose())
        print 'paddle  pos {0[0]:.2f} {0[1]:.2f} {0[2]:.2f}, quat {1[0]:.3f} {1[1]:.3f} {1[2]:.3f} {1[3]:.3f}'.format(*paddle.get_pose())

    viz.callback(viz.TIMER_EVENT, log_mocap)
    viz.starttimer(0, 1, viz.FOREVER)

    def keydown(*args):
        head.reset()
        paddle.reset()

    viz.callback(viz.KEYDOWN_EVENT, keydown)

def InitHMD():
	global collisionLimbBoxes
	if(USE_HMD):
#BL:start		
		viz.go(viz.HMD | viz.TRACKER)
		viz.window.setSize(1260, 950)		
		pptExtension.setup();
		#viz.go(viz.FULLSCREEN)
#BL:end
		viz.clip(0.1)
		vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);
		if(len(collisionLimbBoxes) > 0):
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes);
		else:
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None);
	else:
		pptSimulation.setup(avatar);
		viz.clip(0.1)
		viz.go(viz.FULLSCREEN);
		vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);

def vhilGo(rift=False, ppt1=False, OH=False, otherTrackerVirtualIDs=[], debug=False, homePos=[0,1.82,0], homeOri=[0,0,0]): #, **kwargs
	global RIFT
	global PPT1
	global DEBUG
	global HOME_POS
	global HOME_ORI
	
	# Run-context constants #
	RIFT = rift # Indicates whether we are going to run the simulation on an Oculus Rift.
	PPT1 = ppt1 # Indicates whether we are going to run the simulation in PPT1.
	DEBUG = debug # Indicates whether we are going to run the simulation in debug mode, enabling extra controls and features
	
	# HOME_POS, HOME_ORI: Starting position and orientation of viz.MainView.
	# reset() resets to the home position and home orientation by default.
	HOME_POS = homePos
	HOME_ORI = homeOri

	viz.go()
	reset()
	return setupHardware(OH, otherTrackerVirtualIDs)

def InitHMD():
    # BL:start
    global collisionLimbBoxes, isLeftHanded
    if USE_HMD:
        viz.go(viz.HMD | viz.TRACKER)
        viz.window.setSize(1260, 950)
        pptExtension.setup(isLeftHanded)
        # viz.go(viz.FULLSCREEN)
        # BL:end
        viz.clip(0.1)
        vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes)
        if len(collisionLimbBoxes) > 0:
            vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes)
        else:
            vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None)
    else:
        pptSimulation.setup(avatar)
        viz.clip(0.1)
        viz.go(viz.FULLSCREEN)
        vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes)

def setUp():
	global currentCondition
	
	viz.setMultiSample(8)
	initGlobals()
	
	currentCondition = viz.choose('Select condition:', CONDITION_CHOICES)
	print "CurCondition:", currentCondition
	
	setFlags()

#	setUpRecording()
#
#	if YOKE_F:
#		setUpYoke()
#
#	if not FLYING_F:
#		setUpHelicopter()
#	
#	if(not confirmStates(currentCondition)):
#		viz.quit()
#		finish()  #TODO make it so that on failure no files are created.
#		return
#
#	#configData()
	
	if USE_HMD_F:
		setUpHMD()
		if not FLYING_F:
			setHeliInitPos()
	else:
		viz.go()
		viz.MainView.setPosition(START_POSITION_HELI)

	setUpWorld()

	setUpChild()
	
	setUpSound()

    def __init__(self, filename=''):
        '''Opens and interprets configuration files for running experiments.

        This constructor opens both the system config (as defined by the
        <platform>.cfg file) and the experiment config (as defined by the given
        filename).

        Both configurations must conform the specs given in SYSTEM.ini and
        experiment.ini respectively. It also initializes the system as specified
        in the system config.
        '''
        self.exp = self.create_exp_config(filename)
        self.sys = self.create_sys_config()
        for path in self.sys['set_path']:
            viz.res.addPath(path)

        viz.window.setFullscreenMonitor(self.sys['displays'])
        viz.setMultiSample(self.sys['antiAliasPasses'])
        viz.MainWindow.clip(0.01, 200)

        self.writables = []
        self.writer = None
        self.mocap = None
        self.body_camera = None

        self._setup_hmd()
        self._setup_recorder()
        self._setup_eyetracking()
        self._setup_phasespace()
        self._setup_hiball()

        self.writables.append(self.mocap)
        self.writables.append(self.body_camera)

        if self.sys['use_fullscreen']:
            viz.go(viz.FULLSCREEN)
        else:
            viz.go()

def vhilGo(
    vive=False,
    ppt1=False,
    OH=False,
    otherTrackerVirtualIDs=[],
    debug=False,
    homePos=[0, 1.82, 0],
    homeOri=[0, 0, 0],
    keyboardcontrol=True,
    mode=0,
    **kwargs
):
    global VIVE
    global PPT1
    global DEBUG
    global HOME_POS
    global HOME_ORI

    # Run-context constants #
    VIVE = vive  # Indicates whether we are going to run the simulation on an HTC Vive.
    PPT1 = ppt1  # Indicates whether we are going to run the simulation in PPT1.
    DEBUG = (
        debug
    )  # Indicates whether we are going to run the simulation in debug mode, enabling extra controls and features

    # HOME_POS, HOME_ORI: Starting position and orientation of viz.MainView.
    # reset() resets to the home position and home orientation by default.
    HOME_POS = homePos
    HOME_ORI = homeOri

    viz.go(mode)

    if keyboardcontrol:
        keyboardControl(viz.MainView)

    return setupHardware(OH, otherTrackerVirtualIDs, **kwargs)

def setUpHMD():
	global lhTracker, rhTracker, flyingLink, headTracker
	viz.window.setSize(1520,1205)
	viz.go(viz.TRACKER)

	
	view = viz.MainView
#	isense = viz.add('intersense.dle')
#	headTracker = isense.addTracker(port=1)
#	vizact.onkeydown('r',headTracker.resetHeading)
	#vrpn = viz.add('vrpn7.dle')
	#Tracker_HOSTNAME = '171.64.33.43'
	
	from vizconnect.util.virtual_trackers import OpticalHeading
	
	import steamvr
	# vive view will take the Main View (viz.MainView)
	hmd = steamvr.HMD()


		
### Trackers ###

	ORI_TRACKER = hmd.getSensor()
	offsetMarkerNode = viz.addGroup()
	headTracker = viz.link(ORI_TRACKER, offsetMarkerNode)

	lhTracker = steamvr.getControllerList()[0]
	rhTracker = steamvr.getControllerList()[1]
	
	viveViewLink = viz.link(headTracker, viz.MainView)

	
	if FLYING_F:
		flyingLink = viveViewLink
		flyingLink.postTrans(START_POSITION_BASE)

	def display(self):
		"""
		Initialize the display
		
		Mode selection:
			0 - Regular computer
			1 - 3D TV
			2 - Oculus rift
		"""
		if self.displayMode == 0:
			viz.setMultiSample(4)
			viz.fov(60)
			viz.window.setSize([1280,720])
			viz.go()
			viz.window.setFullscreenMonitor(1)
#			viz.go(viz.FULLSCREEN) #viz.FULLSCREEN

		elif self.displayMode == 1:
			viz.setMultiSample(4)
			viz.go(viz.STEREO_HORZ | viz.FULLSCREEN)
			
		elif self.displayMode == 2:
			viz.go(viz.STEREO_HORZ)
			viz.setMultiSample(16)
			viz.window.setSize([1280,720])
			
			KEYS = {
				'reset'		: 'r',
				'camera'	: 'c'}
			
			# Helps reduce latency
			#do not use ? makes things worse.
			#viz.setOption('viz.glFinish',1)
		
		elif self.displayMode == 3:
			viz.setMultiSample(4)
			viz.fov(60)
			viz.go(viz.FULLSCREEN) #viz.FULLSCREEN
			viz.window.setFullscreenMonitor(2)

		# Initial direction of main view
		viz.MainView.setEuler([0,0,0])
		viz.MainView.setPosition([0,0,-3], viz.ABS_GLOBAL)

import viz
import viztask
import vizjoy
import math
import random
import time
import vizshape
import vizact
import datetime
import time
import Queue


viz.go(viz.PROMPT)
viz.collision(viz.ON)


''' *************************** KINECT CODE ***************************** '''
#myHead = vrpn.addTracker( '[email protected]', HEAD)
HEAD = 0
NECK = 1
TORSO = 2
WAIST = 3
LEFTCOLLAR = 4
LEFTSHOULDER = 5
LEFTELBOW = 6
LEFTWRIST = 7
LEFTHAND = 8
LEFTFINGERTIP = 9
RIGHTCOLLAR = 10
RIGHTSHOULDER = 11

Subject is intended to stand still on the treadmill wearing typical plugin gait with hip marker set.
Targets are displayed showing how far a subject should step forward.

In version 3 rev3, feedback, cursor, and target are on.

Treadmill moves the feet back to neutral after a step

rev 3 incorps a counter of successful steps
"""
import viz
import vizshape
import time
import vizinfo

viz.go(

viz.FULLSCREEN #run world in full screen
)

#viz.addChild('dojo.osgb')
# code to add the grid to the environment
#grid = vizshape.addGrid()

#initExtraWindows()

#viz.MainView.setPosition(0, 0, -1)
#viz.MainView.setEuler(0,0,0)

#set target tolerance for stride length

global targetL
targetL = 0.3