MYO2.py

﻿import os, sys, viz, vizact
import GLOBALS
sys.path.append(os.path.join(GLOBALS.MYO_LOCATION))
sys.path.append(os.path.join(GLOBALS.MYO_SDK_LOCATION))
import myo
from myo.lowlevel import pose_t, stream_emg
import math
from myo.six import print_
import random
import vizcam
SHOW_OUTPUT_CHANCE = .1
import oculus
import viz
import vizact
import oculus
import math
import linecache
import time
import os
import random
import sys
import viztask
import vizinfo


viz.setMultiSample(4)
viz.go()
viz.phys.enable()
viz.phys.setGravity(0,0,0)
dojo = viz.add('ground_grass.osgb')
#dojo.setScale([20,0,20])
#frame = viz.add("frame.dae")
sky = viz.add('sky_day.osgb')


#----------------a-
#-Main-View-Setup-
#----------------- 

testsCompleted = 0 # amount of rounds completed, condition changes if 15 are completed
view = viz.MainView
view.collisionBuffer(1.8)
c = [0.0]

viz.collision(viz.ON)
ori = oculus.Rift()
global navigationNode
navigationNode = viz.addGroup()
viewLink = viz.link(navigationNode, viz.MainView)
viewLink.preMultLinkable(ori.getSensor())
if ori.getSensor().getDisplayMode() == oculus.DISPLAY_DESKTOP:
    viz.window.setFullscreen(True)

'''def unitVector(x,y,z):
    vecMag = math.sqrt(x*x+y*y+z*z)
    return x/vecMag, y/vecMag, z/vecMag'''
    
def move(a,b,c):
    viz.move(a,b,c)


##viewLink.setEuler(viz.MainView.getEuler())
##viz.MainView.setEuler(0,0,0)
#Global variables
pi = 3.1452
gyro = [[0,0,0]]
gy = [0]
gy2 = [0]
gy3 = [0]
or1x = [0] #where you start
or1y = [0]
or1z = [0]
or1w = [0]
orx = [0]
ory = [0]
orz = [0]
orw = [0]
wingr = ""
wingrarm = ""
wingl = ""
winglarm = ""
hasfoundarm = [0,0]
center = [0,0,0]

pco = 0
prev = 0
# Setup Oculus Rift ori
"""ori = oculus.Rift()
if not ori.getSensor():
    sys.exit('Oculus Rift not detected')"""
#Myo update function that updates every SHOW_OUTPUT_CHANCE seconds
def updateMyo(l):
    global prev
    global pco
    wingl = l[3]

    xvel = l[0][0] 
    yvel = l[1][0]
    zvel = l[2][0]
    oQuatl = [-l[7][0],-l[5][0],l[6][0],-l[4][0]]
#    oQuatr = [-l[7][0],-l[5][0],-l[6][0],l[4][0]]
    offsetQuatl = [-l[11][0], -l[9][0], l[10][0], -l[8][0]]
#    offsetQuatr = [-l[11][0], -l[9][0], -l[10][0], l[8][0]]
    wingl.setQuat(offsetQuatl)
#    wingr.setQuat(offsetQuatr)
    wingl.setQuat(oQuatl)
#    wingr.setQuat(oQuatr)
    winglQ = wingl.getQuat()
#    wingrQ = wingr.getQuat()
    winglE = wingl.getEuler()
#    wingrE = wingr.getEuler()
    if pco == 10:
        #print(winglE)
        #print(wingrE)
        pco = 0
    else:
        pco += 1
    
    #-----------------------------------------
    #This allows the movement, however it only goes forward on the x axis. It needs a bit tuning to go the direction the oculus is facing
    #-----------------------------------------
    joel = viz.MainView.getEuler()
    joel[0] += 180
    #navigationNode.runAction(update)
    viz.MainView.velocity(0,0,0)
    #if prev-winglE[1] > 0:
    c[0] = ((prev-winglE[1])/100)**2*100
      
    navigationNode.setPosition(math.cos(ori.getSensor().getEuler()[0]*pi/180-90)*c[0],0,-math.sin(ori.getSensor().getEuler()[0]*pi/180-90)*c[0], viz.REL_PARENT)
    '''
        print(ori.getSensor().getEuler())
        #move(0,0,c[0])
    else:
        c[0] = -1*(prev-winglE[1])/100
      
        #move(0,0,c[0])
        navigationNode.setPosition(math.sin(ori.getSensor().getEuler()[0])*c[0],0,math.sin(ori.getSensor().getEuler()[0])*c[0], viz.REL_PARENT)'''
    #print(math.cos(joel[0]) , " " , math.sin(joel[0]))
    #print(joel[0])
    prev = winglE[1]
    wingl.setEuler(winglE[0]+230,-winglE[1],-winglE[2])
#    wingr.setEuler(wingrE[0]+320,-wingrE[1],-wingrE[2])
    
    #Right now, point your arm away from the screen and press space to calibrate
    #The myo logo should be up with the wings pointing up your arm
    #by 'up' I mean it should point up and your palm should be dow
    
    
def matrixFromQuat(qx,qy,qz,qw):
    r = [[[1 - 2*qy**2 - 2*qz**2]	[2*qx*qy - 2*qz*qw]	    [2*qx*qz + 2*qy*qw]]
        [[2*qx*qy + 2*qz*qw]	    [1 - 2*qx**2 - 2*qz**2]	[2*qy*qz - 2*qx*qw]]
        [[2*qx*qz - 2*qy*qw]	    [2*qy*qz + 2*qx*qw]	    [1 - 2*qx**2 - 2*qy**2]]]
    return r
    
def multMatrices(X,Y):
    return [[sum(a*b for a,b in zip(X_row,Y_col)) for Y_col in zip(*Y)] for X_row in X]
    
def addQuaternion(q,r):
    x = math.sin(math.asin(r[0])+math.asin(q[0]))
    y = math.sin(math.asin(r[1])+math.asin(q[1]))
    z = math.sin(math.asin(r[2])+math.asin(q[2]))
    w = math.sin(math.asin(r[3])+math.asin(q[3]))
    return [x,y,z,w] 
    
def multiplyQuaternion(q,r):
    x = r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3]
    y = r[0]*q[1] + r[1]*q[0] - r[2]*q[3] + r[3]*q[2]
    z = r[0]*q[2] + r[1]*q[3] + r[2]*q[0] - r[3]*q[1]
    w = r[0]*q[3] - r[1]*q[2] + r[2]*q[1] + r[3]*q[0]
    return [x,y,z,w]
    
def quatToEuler(q): #doesn't work, setQuat and then getEuler on something instead ;)
    x = (math.atan2((q[0]*q[1]+q[2]*q[3]),1-2*(q[1]*q[1]+q[2]*q[2])) + pi)/(2*pi)*360
    y = (math.asin(2*(q[0]*q[2] - q[3]*q[1])) + pi)/(2*pi)*360
    z = (math.atan2(2*(q[0]*q[3] +q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3])) + pi)/(2*pi)*360
    return [-x,y,-z]

def invertQuaternion(q):
    return [-q[0],-q[1],-q[2],q[3]]
r"""w
There can be a lot of output from certain da1a like acceleration and orientation.
This parameter controls the percent of times that data is shown.
"""
class Listener(myo.DeviceListener):
    # return False from any method to stop the Hub
    def on_connect(self, myo, timestamp):
        print_("Connected to Myo")
        myo.vibrate('short')
        myo.request_rssi()
        #print_(myo.mac_address())

    def on_rssi(self, myo, timestamp, rssi):
        print_("RSSI:", rssi)

    def on_event(self, event):
        r""" Called before any of the event callbacks. """

    def on_event_finished(self, event):
        r""" Called after the respective event callbacks have been
        invoked. This method is *always* triggered, even if one of
        the callbacks requested the stop of the Hub. """

    def on_pair(self, myo, timestamp):
        print_('Paired')
        print_("If you don't see any responses to your movements, try re-running the program or making sure the Myo works with Myo Connect (from Thalmic Labs).")
        print_("Double tap enables EMG.")
        print_("Spreading fingers disables EMG.\n")

    def on_disconnect(self, myo, timestamp):
        print_('on_disconnect')

    def on_pose(self, myo, timestamp, pose):
        print_('on_pose', pose)
        if pose == pose_t.double_tap:
            print_("=" * 80)
            print_("Reset Orientation")
            or1x[0] = orx[0]
            or1y[0] = ory[0]
            or1z[0] = orz[0]
            or1w[0] = orw[0]
            #myo.set_stream_emg(stream_emg.enabled)
        elif pose == pose_t.fingers_spread:
            egg = 0
            #print_("=" * 80)
            #myo.set_stream_emg(stream_emg.disabled)
        

    def on_orientation_data(self, myo, timestamp, orientation):
        #show_output('orientation', orientation)
        #print_(orientation) #- The actual orientation quaternion that seems to be from -1 to 1
        #print_(orientation[0])
        orx[0] = orientation[0]
        ory[0] = orientation[1]
        orz[0] = orientation[2]
        orw[0] = orientation[3]
        if (or1x[0] == [0] and or1y[0] == [0] and or1z[0] == 0 and or1w[0] == [0]):
            or1x[0] = orientation[0]
            or1y[0] = orientation[1]
            or1z[0] = orientation[2]
            or1w[0] = orientation[3]

    def on_accelerometor_data(self, myo, timestamp, acceleration):
        show_output('acceleration', acceleration)

    def on_gyroscope_data(self, myo, timestamp, gyroscope):
        #show_output('gyroscope', gyroscope)
        gy[0] = gyroscope[0]
        gy2[0] = gyroscope[1]
        gy3[0] = gyroscope[2]
            

    def on_unlock(self, myo, timestamp):
        print_('unlocked')

    def on_lock(self, myo, timestamp):
        print_('locked')

    def on_sync(self, myo, timestamp, arm, x_direction):
        print_('synced', arm, x_direction)
        

    def on_unsync(self, myo, timestamp):
        print_('unsynced')
        
    def on_emg(self, myo, timestamp, emg):
        show_output('emg', emg)
        
def show_output(message, data):
    if random.random() < SHOW_OUTPUT_CHANCE: 
        return

test = False
validID = False
subjectID = 1
while not (validID):
    try:
        #Prompt the user for an ID number
        subjectID = int(viz.input('Input Subject ID Number'))
        RESULTS_DIRECTORY = 'C:\Users\Bryton\Downloads\MyoResults2015'##CHANGE THIS
        #Validate the subject ID number
        outFilePath = '%s\Subject_%s.txt' %(RESULTS_DIRECTORY, str(subjectID))
        if os.path.exists(outFilePath) or subjectID is '':
            yes = viz.ask('The Subject ID Number ' + str(subjectID) + ' already exists. Pick a new one?')
            if not yes:
                raise 'Exiting...'
        else:
            validID = True
    except ValueError:
        print('Subject ID number must be an integer')
if test:
    subjectInitial = 't'
else:
    subjectInitial = (viz.input('Input subject initial'))
#Determine whether or not trial recording is enabled 
recordingEnabled = True
#print recordingEnabled
if recordingEnabled:
    dirPath = '.\Results\Subject_%s' % (str(subjectID))
    if(not os.path.exists(dirPath)):
        os.makedirs(dirPath)
    recordFile = open('.\Results\Subject_%s\myo_%s.txt' % (str(subjectID), subjectInitial), 'w')

#~~~~~
#~PHI~
#~~~~~

def phi(e):
    #print "ob:", e
    gaze_euler = viz.MainView.getEuler()
    gaze_euler = [math.radians(gaze_euler[0]),math.radians(gaze_euler[1]),math.radians(gaze_euler[2])]
    gaze_vector = [math.cos(gaze_euler[0])*math.cos(gaze_euler[1]),math.sin(gaze_euler[1]),math.sin(gaze_euler[0])*math.cos(gaze_euler[1])]
    cos_gaze_phi = math.sqrt(gaze_vector[0]*gaze_vector[0]+gaze_vector[2]*gaze_vector[2])/math.sqrt(gaze_vector[0]*gaze_vector[0]+gaze_vector[1]*gaze_vector[1]+gaze_vector[2]*gaze_vector[2])
    gaze_phi = math.acos(cos_gaze_phi)
    gaze_position = viz.MainView.getPosition()
    object_vector = [0,0,0]
    object_vector[0] = e[0] - gaze_position[0]
    object_vector[1] = e[1] - gaze_position[1]
    object_vector[2] = e[2] - gaze_position[2]
    cos_object_phi = math.sqrt(object_vector[0]*object_vector[0]+object_vector[2]*object_vector[2])/math.sqrt(object_vector[0]*object_vector[0]+object_vector[1]*object_vector[1]+object_vector[2]*object_vector[2])
    object_phi = math.acos(cos_object_phi)

    phi = gaze_phi - object_phi
    phi = math.degrees(phi)
    return phi

#--------------
#-Object-Setup-
#--------------

#The object sets
addresses0 = ["THEREALDOG.WRL", "thechair.WRL", "THEBARREL.WRL", "thebirds.WRL", "plant.WRL", "shield.WRL"]
addresses1 = ["bookshelf.WRL", "HARP.WRL", "THECHALICE.WRL", "THECLOCK.WRL", "THEBOX.WRL", "THEPHONEBOOTH.WRL"]
addresses2 = ["THEPIGGYBANK.WRL", "THETREASURECHEST.WRL", "THEURN.WRL", "THEWASHINGMACHINE.WRL", "watch.WRL", "wheelbarrow.ive"]

objectAddresses = [addresses0, addresses1, addresses2] #array of object sets
objects = [] #will contain actual objects
objectFiles = [] #will contain filenames and viz.add(obj) items
set = 0 #object set

#~~~~~~~~~~~~~~~~~~~~~~~~
#~File~Reading~Functions~
#~~~~~~~~~~~~~~~~~~~~~~~~

fileStartIndex = 0 #where to pick up reading the file
#LocationOrder reads lines from files that are randomized based on subjectID
def LocationOrder(fileName, lines = 1):
    MODerator = subjectID % 12 * lines
    list = []
    index = 0
    MODerator +=1
    for number_index in range(MODerator, MODerator + lines):
        if lines != 1:
            list.append([])
        real_line = linecache.getline(fileName, number_index)
        if real_line == '\n':
            real_line = linecache.getline(fileName, number_index+1)
        for character in real_line:
            if(character == ' '):
                pass
            elif(character == '\n'):
                pass
            else:
                if lines != 1:
                    list[index].append(int(character))
                else:
                    list.append(int(character))
        index += 1
    return list

def FileReader():
    global targetOrder, locationOrder
    targetOrder = LocationOrder("indexOrder.txt")
    locationOrder = LocationOrder("random.txt")

#------------------------
#-Master Location Arrays-
#------------------------

scale_h = 0
obj_h = 2 #will be hard coded in at end
xdif = 0.5 #not sure what this is for
scale = 5
slope35 = 0.7718232131 #slopefromthreeto5, also not sure what this is for
#masterObjectLocations is the locations for the 6 objects in the active set
masterObjectLocations = [[ -1.04082/scale, (obj_h-0.9)* scale_h, 24/scale ],\
 [ -8.44218/scale, (obj_h-1)*scale_h, 4.5714/scale ],\
 [ -15.2653/scale, (obj_h-1.3)*scale_h , -9.90476/scale ],\
 [ 1.04082/scale, (obj_h-0.6)*scale_h, -11.8095/scale ],\
 [ 12.6054/scale, (obj_h-1)*scale_h , -21.7143/scale ],\
 [ (15.6122 + xdif)/scale, (obj_h-1.15)*scale_h, (-9.71429 + xdif * slope35)/scale ]]

test_h = 1.8*scale_h #also will be hard coded in
#masterTargetLocations is the locations for cylinders, height will be hard coded in
masterTargetLocations = [[ -17/scale, test_h, 1.333333/scale ],\
[ -7.17007/scale, test_h, -11.2381/scale ],\
[ 1.04082/scale, test_h, -20.8095/scale ],\
[ -18/scale, test_h, 9/scale ],\
[ 6.82313/scale, test_h, -3.80952/scale ],\
[ 15.2653/scale, test_h, 7.47619/scale ]]
targetLocations = [] #will contain the randomized targets from FileReader

offset = 0.5 #not sure about the math on this one
slope4 = -0.7224 #fournegrecslope, also not sure where this comes from
arrow_h = 1.8*scale_h #may have to hard code for height but also could just have the arrows in midair at the same level for all
#masterArrowLocations is the locations for the arrows
masterArrowLocations = [[(masterTargetLocations[0][0])/scale, arrow_h, (masterTargetLocations[0][2]+offset)/scale],\
[(masterTargetLocations[1][0] + math.cos(math.pi/4.25) * offset)/scale, arrow_h, (masterTargetLocations[1][2] + math.sin(math.pi/4.25) * offset)/scale],\
[(masterTargetLocations[2][0] + offset)/scale, arrow_h, (masterTargetLocations[2][2])/scale],\
[(masterTargetLocations[3][0])/scale, arrow_h, (masterTargetLocations[3][2] - offset)/scale],\
[(masterTargetLocations[4][0] - offset)/scale, arrow_h, (masterTargetLocations[4][2] - slope4 * offset)/scale],\
[(masterTargetLocations[5][0] - offset)/scale, arrow_h, (masterTargetLocations[5][2])/scale]]

#---------
#-Targets-
#---------

targetRotations = [45,180,30,310,100,350] #targetRotations is an array for how much to rotate each target each rotation
#masterTargetObjects: each index is a testing location, and the objects are the targets for that location
masterTargetObjects = [[0,1,2],[0,2,3],[2,3,5],[3,4,5],[0,3,5],[0,4,5]]
targetPosition = [0,0,0]
targetLocations = []
targetOrder = [1,2,0, 2,0,1, 2,0,1, 1,2,0, 2,0,1, 2,0,1] #order in which !!!!NOTSURE WILL COME BACK!!! probs from filereader
locationOrder = [1,4,2,3,5,0] #will be a 1d array because of separate code for each condition. each item is the number of the target location to go to
#the locationOrder will be randomized
targetCounter = 0 #which index of targetObjects
locationCounter = 0 #which index of locationOrder
currentTarget = 0 #not sure

def getCurrentTestingLocationOrder():
    global targetLocations
    global arrowLocations
    global targetObjects
    global locationOrder
    
    targetLocations = []
    arrowLocations = []
    targetObjects = []
    
    for i in range(len(masterTargetObjects)):
        arrowLocations.append([])
        targetLocations.append([])
        targetObjects.append([])
        
        currentTargetObjectIndex = locationOrder[i]
        currentTargetObject = masterTargetObjects[currentTargetObjectIndex]
        for j in range(len(masterTargetObjects[i])):
            indexTargetOrder = currentTargetObjectIndex * len(currentTargetObject) + j
            nextObjectIndex = targetOrder[indexTargetOrder]
            targetLocations[i].append(masterTargetLocations[locationOrder[i]][j])
            arrowLocations[i].append(masterArrowLocations[locationOrder[i]][j])
            targetObjects[i].append(currentTargetObject[nextObjectIndex])

#~~~~~~~~~~~~~~~~~~
#~Object~Functions~
#~~~~~~~~~~~~~~~~~~

def initializeObjectFiles():
    global objectFiles
    commonAddress='C:\Users\Bryton\Desktop\Terrain Vizard\TerrainStuff\\'
    for i in range(len(objectAddresses)):
        objectFiles.append([])
        for j in range(len(objectAddresses[i])):
            objectFiles[i].append(viz.add(commonAddress + objectAddresses[i][j]))
            objectFiles[i][j].visible(viz.OFF)
            
def initializeLocations():
    global objects, set
    for x in range(len(masterObjectLocations)):
        objects.append(objectFiles[set][x])
        objects[x].setScale([0.5,0.5,0.5])
        objects[x].setPosition(masterObjectLocations[x])
        objects[x].alpha = 0.0
        objects[x].visible(viz.ON)

def changeObjects():
    global fileStartIndex, objects, objectFiles, set
    set = (set + 1) % len(objectFiles)
    for x in range(len(objects)):
        temp = objectFiles[objectSet][x]
        temp.setPosition(objects[x].getPosition())
        objects[x].visible(viz.OFF)
        temp.visible(viz.ON)
        objects[x] = temp

#----------
#-Rotation-
#----------

rotation = 0 #will come back after def rotation():
startAngle = 0 #starting angle
turnAngle = 0 #angle after turning
angleIndex = 0 #not sure yet

off = 11		#precise amount off
ERROR = (43- off) * -1	#correct AMOUNT OF DEGREES to correct movement
currentYaw = 3.3	#angle of hmd accounting for error

#rotates objects by given angle about the origin
def rotate(angle, positions):
    #find the rotation matrix for the given angle
    radians = math.radians(angle)
    rotation = [[math.cos(radians), -math.sin(radians)], [math.sin(radians), math.cos(radians)]]
    newLocation = []
    #multiply all the locations by the matrix
    for x in range(len(positions)):
        newX = positions[x][0] * rotation[0][0] + positions[x][2] * rotation[0][1]
        newY = positions[x][0] * rotation[1][0] + positions[x][2] * rotation[1][1]
        newLocation.append([newX, positions[x][1], newY])
    #print newLocation
    return newLocation

#--------
#-States-
#--------

sight = False #will the subject be able to see when turning to face target object
currentState = 0 # 0=Observe 1=Walk 2=Turn 3=Reorient
locationFinished = False #I think this is whether or not all 3 have been tested at current spot

def changeState(state):
    global objects, objectLocations, targetLocations, targetRotations, currentTarget, rotation, startPosition, set
    global cylinder, arrow, targetToFace, objectFiles, startFileIndex, targetObjects, targetPosition, startAngle
    global recordFile, angleIndex, sun, monument
    #--------------------------------------------------------------
    #A state of 0 corresponds to showing all of the objects and no target
    if(state == 0):
        showScenery()
        for x in range(len(objects)):
                objects[x].visible(viz.ON)
        hideArrowCylinder()
    
    #--------------------------------------------------------------
    #A state of 1 corresponds to showing the target and waiting for the participant to
    #walk toward the target and face the right direction
    elif(state == 1):
        targetPosition = rotate(rotation, targetLocations)[currentTarget]
        targetRotation = targetRotations[currentTarget] + rotation
        cylinder.setPosition(targetPosition)
        angleArray = [45,90,45]
        arrowPosition = rotate(rotation, arrowLocations)[currentTarget]
        arrow.setPosition(arrowPosition)
        cylinder.setEuler(targetRotation)
        
        #show all of the objects and the cylinder and the arrow
        
        for x in range(len(objects)):
                objects[x].visible(viz.ON)
        showArrowCylinder()
    #--------------------------------------------------------------
    #A state of 2 corresponds to hiding the scene and waiting for the
    #participant to finish turning
    elif(state == 2):
        global startTime
        if not sight:
            hideScenery()
            for x in range(len(objects)):
                objects[x].visible(viz.OFF)
        hideArrowCylinder()
        startPosition = view.getPosition()
        viz.MainView.velocity(0,0,0)
        startAngle = view.getEuler()[0]#ERROR
        startTime = time.clock()
        viztask.schedule(ShowMessage('Please turn to face %s' % objectAddresses[set][(fileStartIndex + targetObjects[currentTarget][targetToFace]) % len(objectFiles[set])]))
        
    #--------------------------------------------------------------
    #A state of 3 corresponds to showing only the cylinder and arrow
    #so that the participant can be reoriented to the starting position
    elif(state == 3):
        if not sight:
            hideScenery()
            for x in range(len(objects)):
                objects[x].visible(viz.OFF)
        showArrowCylinder()

def statusUpdate():
    global  currentState, targetToFace, targetObjects, turnAngle, locationFinished, testsCompleted
    global currentTarget, set, recordFile
    #Show the next target to walk toward
    #print "currentState:",currentState
    if(currentState == 0):
        currentState = 1
        if(recordingEnabled):
            recordFile.write('Target: %d\n' % currentTarget)
        changeState(currentState)
    #Hide the scene and let the participant turn
    elif(currentState == 1):
        targetToFace = -1
        currentState = 3
        #print 'Object Index: %d, Total Objects: %d' % (targetToFace, len(targetObjects[currentTarget]))
        changeState(currentState)
    #Show the arrow to reorient the participant
    elif(currentState == 2):
        global startTime
        timeDifference = time.clock() - startTime
        currentPosition = view.getPosition()
        movedTurned = False
        if startPosition != currentPosition:
            print "WARNING! STARTPOSITION IS NOT CURRENTPOSITION"
            movedTurned = True
        #calculate the angle turned and the angle desired
        objectPosition = rotate(rotation, masterObjectLocations)
        objectNumber = targetObjects[currentTarget][targetToFace]
        phi_stuff = masterObjectLocations[objectNumber]
        #print phi_stuff
        phi_error = phi(phi_stuff)
        print "phi_error:",phi_error
        destX = objectPosition[targetObjects[currentTarget][targetToFace]][0] - startPosition[0]
        destY = objectPosition[targetObjects[currentTarget][targetToFace]][2] - startPosition[2]
        mag = math.sqrt(destX*destX + destY*destY)
        normX = destX / mag
        normY = destY / mag
        startY = math.cos(math.radians(startAngle))
        startX = math.sin(math.radians(startAngle))
        endY = math.cos(math.radians(view.getEuler()[0]))#ERROR
        endX = math.sin(math.radians(view.getEuler()[0]))#ERROR
        dotNeeded = normX * startX + normY * startY
        dotTurned = startX * endX + startY * endY
        angleNeeded = math.degrees(math.acos(dotNeeded))
        angleTurned = math.degrees(math.acos(dotTurned))
        
        print 'Angle Needed: %f, Angle Turned: %f' % (angleNeeded, angleTurned)
        yaw_error = angleNeeded - angleTurned
        print "yaw_error:",yaw_error
        delim1 = '$'
        delim2 = '^'
        if(recordingEnabled):
            recordFile.write('%d,%s,%f,%f,%f,%f,%f,%f,%f, %f, %f, %s\n' % (objectNumber,
            objectAddresses[set][(fileStartIndex + targetObjects[currentTarget][targetToFace]) % len(objectFiles[set])],
            startPosition[0],startPosition[1],startPosition[2],startAngle,angleTurned,angleNeeded,timeDifference,yaw_error,phi_error,movedTurned))
        currentState = 3
        if(targetToFace + 1 >= len(targetObjects[currentTarget])):
            currentTarget = (currentTarget + 1) % len(targetObjects)
            locationFinished = True
        changeState(currentState)
        
    #Hide everything again and let the participant turn toward the new target
    elif(currentState == 3):
        if locationFinished:
            currentState = 0
            locationFinished = False
            changeState(currentState)
        targetToFace = targetToFace + 1
        testsCompleted += 1
        if(targetToFace < len(targetObjects[currentTarget])):
            currentState = 2
        #move on to next rotation
        else:
            currentState = 0
        changeState(currentState)

def redoTrial(trial):
    global currentState, currentTarget, targetToFace, locationFinished
    currentState = 0
    currentTarget = int(trial)
    locationFinished = False
    changeState(currentState)
    

#------
#-Time-
#------

startTime = time.clock() #initial time

#-----------------------------------
#-Cylinder-and-Arrow-Initialization-
#-----------------------------------

cylScale = (6,1.5,6)
cylinder = viz.add('cylinder.wrl')
cylinder.setPosition(masterTargetLocations[0]) #may have to modify this per environment
cylinder.setScale(cylScale)
cylinder.alpha(0.5) #dont know what alpha is
cylinder.visible(viz.OFF)
cylinder.disable(viz.INTERSECTION)
arrow = viz.add('arrow.wrl')
arrow.setScale(0.5,0.35,0.5)
arrow.setPosition(masterArrowLocations[0]) #why is this not masterArrowLocations[0]?
arrow.alpha(0.8)
arrow.visible(viz.OFF)
arrow.disable(viz.INTERSECTION)

def update():
    global currentState, arrow, cylinder, view, targetRotations, rotation, objects
    global targetObjects, currentTarget, targetToFace, targetPosition
    radius = 2
    if(currentState == 1 or currentState == 3):
        x = view.getPosition()[0] - targetPosition[0]
        y = view.getPosition()[2] - targetPosition[2]
        distSqrd = x*x + y*y
        if(distSqrd <= radius):
            cylinder.color(0,1,0)
            pointA = view.getPosition()[0],0,view.getPosition()[2] #+ math.sin(math.radians(currentYaw)) + math.cos(math.radians(currentYaw))   
            angle = get2Angle(view.getPosition(), pointA, arrow.getPosition())
            if(angle <= 15.0):
                arrow.color(0,1,0)
            else:
                arrow.color(1,0,0)
        else:
            cylinder.color(1,0,0)
            arrow.color(1,0,0)
    
#~~~~~~~~~~~~~~~~~~~~~~~~~
#~Miscellaneous~Functions~
#~~~~~~~~~~~~~~~~~~~~~~~~~

def hideScenery():
    global dojo, sky
    dojo.visible(viz.OFF)
    sky.visible(viz.OFF)
    
def showScenery():
    global dojo, sky
    dojo.visible(viz.ON)
    sky.visible(viz.ON)
    
def hideArrowCylinder():
    global cylinder, arrow
    cylinder.visible(viz.OFF)
    arrow.visible(viz.OFF)
    
def showArrowCylinder():
    global cylinder, arrow
    cylinder.visible(viz.ON)
    arrow.visible(viz.ON)
    
def ShowMessage(mystring):
    info = vizinfo.add(mystring)
    yield viztask.waitTime(2)
    info.visible(0)
    
def printOrderObjects():
    for set in targetObjects:
        for obj in set:
            print objectAddresses[currentCondition][obj]
    
def objPos():
    global objects
    for x in range (len(objects)):
        print objects[x].getPosition()
        
def get2Angle(A, B, C):
    if (A == B and B == C and C == (0,0,0)):
        return 0
    vectorAB = B[0] - A[0], B[1] - A[1], B[2] - A[2]
    vectorAC = C[0] - A[0], C[1] - A[1], C[2] - A[2]
    
    dot = TwoDotProduct(vectorAB, vectorAC)
    magAB = TwoMagnitude(vectorAB)
    magAC = TwoMagnitude(vectorAC)
    try:
        theta = math.acos(dot / magAB / magAC)
    except ZeroDivisionError:
        return 0
    return math.degrees(theta)
    
def TwoDotProduct(vectA, vectB):
    xA = vectA[0]
    zA = vectA[2]
    
    xB = vectB[0]
    zB = vectB[2]
    
    return xA*xB + zA*zB
    
def TwoMagnitude(vect):
    x = vect[0]
    z = vect[2]
    return math.sqrt(x*x + z*z)


def main():
    global wingl
#    global wingr
    viz.cam.setHandler(vizcam.KeyboardCamera())
    myo.init()
    gyrolist = [[0,0,0]]
    viz.setMultiSample(4)
#    
    wingl = viz.addChild('basketball.osgb')
    wingl.setScale([10,0.3,1])
    wingl.setCenter([100,100,100])
    wingl.setEuler([0,90,0])
    wingl.collideSphere(0.5)
    lStartPOS = [ 0, 2.5, 2 ]
    wingl.setPosition( lStartPOS )
#    wingr = viz.addChild('basketball.osgb')
#    wingr.setScale([10,0.3,1])
#    wingr.setCenter([100,100,100])
#    wingr.setEuler([0,90,0])
#    wingr.collideSphere(0.5)
#    rStartPOS = [ 0, 2.5, 2 ]
#    wingr.setPosition( rStartPOS )

    #viz.MainView.setPosition([0,2,-15])
    hub = myo.Hub()
    hub.set_locking_policy(myo.locking_policy.none)
    hub.run(1000, Listener())
    vizact.ontimer(0.01 ,updateMyo, [gy, gy2, gy3, wingl, orx, ory, orz, orw, or1x, or1y, or1z, or1w])
    vizact.onkeydown('p', hub.shutdown)
    FileReader()
    initializeObjectFiles()
    initializeLocations()
    vizact.ontimer(0.1, update)
    vizact.onkeydown(' ', statusUpdate)
    vizact.onkeydown('s', hub.shutdown)
    
#    viz.AVIRecorder.maxWidth = 1920;
#    viz.AVIRecorder.maxHight = 1080;
    vizact.onkeydown('b', viz.window.startRecording, 'myo_capture.avi' ) 
    vizact.onkeydown('e', viz.window.stopRecording )

main()