MOUSE_SPEED = 3;

-- Right Wiimote

-- mouse cursor/ aiming
if (wiimote[0]:getRight()) then
   mouse.DeltaX = MOUSE_SPEED;
elseif (wiimote[0]:Left()) then
   mouse.DeltaX = -MOUSE_SPEED;
end

if (wiimote[0]:getUp()) then
   mouse.DeltaY = -MOUSE_SPEED;
elseif (wiimote[0]:getDown()) then
   mouse.DeltaY = MOUSE_SPEED;
end

-- right attack
mouse.LeftButton = wiimote[0]:getB();

-- jump
keyboard:setKey(Key.Space, wiimote[0]:getA());

-- crouch/sneak toggle
keyboard:setKey(Key.LeftControl, wiimote[0]:getMinus());

-- run toggle
keyboard:setKey(Key.CapsLock, wiimote[0]:getPlus());

-- inventory
keyboard:setKey(Key.Tab, wiimote[0]:getHome());

-- Enter
keyboard:setKey(Key.Return, wiimote[0]:getOne());

-- Esc
keyboard:setKey(Key.Escape, wiimote[0]:getTwo());


-- Left Wiimote

-- character motion
keyboard:setKey(Key.W, wiimote[1]:getUp());
keyboard:setKey(Key.A, wiimote[1]:getLeft());
keyboard:setKey(Key.S, wiimote[1]:getDown());
keyboard:setKey(Key.D, wiimote[1]:getRight());
   

-- left attack
mouse.RightButton = wiimote[1]:getB();

-- use
keyboard:setKey(Key.E, wiimote[1]:getA());

-- put away weapon
keyboard:setKey(Key.R, wiimote[1]:getMinus());

-- shout
keyboard:setKey(Key.Z, wiimote[1]:getPlus());

def update():
   yaw = math.degrees(freeTrack.yaw)
   roll = math.degrees(freeTrack.roll)
   pitch = math.degrees(freeTrack.pitch)
   
   deltaYaw = filters.delta(yaw)
   deltaPitch = filters.delta(pitch)   
     
   if (enabled and hotkey):
      mouse.deltaX = deltaYaw*multiply
      mouse.deltaY = -deltaPitch*multiply

if starting:
   enabled = False
   multiply = 20
   freeTrack.update += update

hotkey = mouse.rightButton
toggle = keyboard.getPressed(Key.Z)

if toggle:
   enabled = not enabled

def update():
   x = math.degrees(freeTrack.yaw) * ratio
   y = math.degrees(freeTrack.pitch) * ratio
   
   ppJoy[0].setAxis(AxisTypes.X, x)
   ppJoy[0].setAxis(AxisTypes.Y, y)
   
if starting:
   maxAngle = 180
   axisRange = 1000
   ratio = axisRange / maxAngle
   freeTrack.update += update

def update():
   global yaw
   yaw = android.yaw
   global pitch
   pitch = -android.roll
   global roll
   roll = android.pitch

if starting:
   centerYaw = 0
   centerPitch = 0
   centerRoll = 0
   
   yaw = 0
   pitch = 0
   roll = 0
   android.update += update

freetrack.yaw  = math.degrees(yaw - centerYaw)
freetrack.pitch =  math.degrees(pitch - centerPitch)
freetrack.roll =  math.degrees(roll - centerRoll)

if keyboard.getKeyDown(Key.LeftControl) and keyboard.getPressed(Key.C):
   centerYaw = yaw
   centerPitch = pitch
   centerRoll = roll

import telnetlib
import re

def update():
   yaw = math.degrees(android.googleYaw)
   pitch = math.degrees(android.googlePitch)
   
   deltaYaw = filters.delta(yaw)
   deltaPitch = filters.delta(pitch)   
   
   deltax = int(deltaYaw*multiplyYaw)
   deltay = int(deltaPitch*multiplyPitch)
   
   #send screen position change to Infinite Screen, and consume response
   tn.write('POS:'+str(deltax)+','+str(deltay)+'\n')   
   pos,match,text = tn.expect([ptn])
     
if starting:
   multiplyYaw = 200 #separate yaw/pitch for future flexibility
   multiplyPitch = 200

   #open connection to Infinite Screen
   tn = telnetlib.Telnet()   
   tn.open('127.0.0.1', 5000)

   #Consume initial welcome message
   ptn = re.compile('^.*$')
   pos,match,text = tn.expect([ptn])
   
   #hook up 'update' handler
   android.update += update

if keyboard.getKeyDown(Key.LeftControl) and keyboard.getPressed(Key.C):
   tn.write('CLOSE\n')
   pos,match,text = tn.expect([ptn])

def update():
    Diagnostics.watch(ahrs.yaw)
    Diagnostics.watch(ahrs.pitch)
    Diagnostics.watch(ahrs.roll)

if Starting:
    ahrs.update += update

import telnetlib
import re

def update():
   global lastYaw
   global lastRoll
   global lastPitch
   
   yawDiff = ahrsImu.yaw - lastYaw
   rollDiff = ahrsImu.roll - lastRoll
   pitchDiff = ahrsImu.pitch - lastPitch
   
   lastRoll = ahrsImu.roll
   
   yawShift = 0
   if (yawDiff < -1):
      yawShift = yawDiff*40
      lastYaw = ahrsImu.yaw
   if (yawDiff > 1):
      yawShift = yawDiff*40
      lastYaw = ahrsImu.yaw
   
   pitchShift = 0
   if (pitchDiff <-0.6):
      pitchShift = pitchDiff*20
      lastPitch = ahrsImu.pitch
   if (pitchDiff > 0.6):
      pitchShift = pitchDiff*20
      lastPitch = ahrsImu.pitch

   #send screen position change to Infinite Screen, and consume response
   if UpdateScreenPosition == 1:
      cmd = 'POS:'+str(int(yawShift*-1))+','+str(int(pitchShift*-1))+'\n'
      diagnostics.watch(cmd)
      tn.write(cmd)
      pos,match,text = tn.expect([ptn])

if starting:
   lastYaw = ahrsImu.yaw
   lastRoll = ahrsImu.roll
   lastPitch = ahrsImu.pitch

   UpdateScreenPosition = 0

   global tn
   tn = telnetlib.Telnet()   
   tn.open('127.0.0.1', 5000)

   #Consume initial welcome message
   ptn = re.compile('^.*$')
   pos,match,text = tn.expect([ptn])

   ahrsImu.update += update

if stopping:
   tn.write('CLOSE\n')
   pos,match,text = tn.expect([ptn])
   diagnostics.debug("finished!")

if keyboard.getKeyDown(Key.LeftControl) and mouse.getPressed(3):
   UpdateScreenPosition = 1-UpdateScreenPosition
   diagnostics.debug("Head tracker toggled, is now ["+UpdateScreenPosition+"]")