//debug = "wm1 - Pitch:"+var.outPITCH1+" Roll:"+var.outROLL1+" Yaw:"+var.outYAW1+" Z:"+var.outZ1
//debug = "wm2 - Pitch:"+var.outPITCH2+" Roll:"+var.outROLL2+" Yaw:"+var.outYAW2+" Z:"+var.outZ2
//debug = "wm1pad: x:"+var.padx1+" y:"+var.pady1+" z:"+var.padz1
//debug = "wm2pad: x:"+var.padx2+" y:"+var.pady2+" z:"+var.padz2
//debug = "w1btns:"+var.plusminus1+":"+var.btnone1+":"+var.btntwo1
//debug = "w2btns:"+var.plusminus2+":"+var.btnone2+":"+var.btntwo2
//debug = "b1:"+wiimote1.Battery+" b2:"+wiimote2.Battery
debug = "b1:"+wiimote1.Battery+" b2:"+wiimote2.Battery+" w1P:R:"+var.outPITCH1+":"+var.outROLL1+" wmP:R:"+var.outPITCH2+":"+var.outROLL2
if (!var.init) then
   var.init = true
   //MY VARS
   var.PORT1 = 9001
   var.ROTthresh = .05
   var.Zminthresh = .05
   var.Zmaxthresh = 1
   //IR VARS
   var.SensorBarSeparation = 10.5 inches   // distance between middles of two sensor bar dots
   var.min = 30                         //Minimum and maximum IR distance values.
   var.max = 85
endif
   //WiiMOTE-ID
   wiimote1.Led1 = true
   wiimote2.Led2 = true
/////---------------WiiMOTE 1(ONE)1 CALIBRATION---------------

var.Poff1 = var.a1
var.Roff1 = var.b1
if Space = true and B = true then
   wait 50 ms
   var.a1 = -var.xRot1
   var.b1 = -var.yRot1
   wiimote1.led4 = true
   wait 20 ms
   wiimote1.led4 = 0
   wait 20 ms
   wiimote1.led4 = true
   wait 200 ms
   wiimote1.led4 = 0
end if
/////---------------WiiMOTE 2(TWO)2 CALIBRATION---------------

var.Poff2 = var.a2
var.Roff2 = var.b2
if Space = true and N = true then
   wait 50 ms
   var.a2 = -var.xRot2
   var.b2 = -var.yRot2
   wiimote2.led4 = true
   wait 20 ms
   wiimote2.led4 = 0
   wait 20 ms
   wiimote2.led4 = true
   wait 200 ms
   wiimote2.led4 = 0
end if

/////CALIBRATED ROTATION, pitch, roll
var.xRot1 = Smooth(Wiimote1.SmoothPitch + var.Poff1)   //1
var.yRot1 = Smooth(Wiimote1.SmoothRoll + var.Roff1)
var.xRot2 = Smooth(Wiimote2.SmoothPitch + var.Poff2)   //2
var.yRot2 = Smooth(Wiimote2.SmoothRoll + var.Roff2)

/////---------------WiiMOTE IR MOTION---------------
var.d1x1 = Smooth(wiimote1.dot1x)//, 5, 1)   //1
var.d1y1 = Smooth(wiimote1.dot1y)
var.d2x1 = Smooth(wiimote1.dot2x)
var.d2y1 = Smooth(wiimote1.dot2y)
var.d1x2 = Smooth(wiimote2.dot1x)            //2
var.d1y2 = Smooth(wiimote2.dot1y)
var.d2x2 = Smooth(wiimote2.dot2x)
var.d2y2 = Smooth(wiimote2.dot2y)


/////---------------WiiMOTE PADS and BUTTONS---------------------
if Wiimote1.Up                                      //1
var.pady1 = 1
elseif Wiimote1.Down
var.pady1 = -1
else var.pady1 = 0
if Wiimote1.Left
var.padx1 = -1
elseif Wiimote1.Right
var.padx1 = 1
else var.padx1 = 0
if Wiimote1.A
var.padz1 = 1
elseif Wiimote1.B
var.padz1 = -1
else var.padz1 = 0
endif

if Wiimote1.Plus
var.plusminus1 = 1
elseif Wiimote1.Minus
var.plusminus1 = -1
else var.plusminus1 = 0
if Wiimote1.One
var.btnone1 = 1
else var.btnone1 = 0
if Wiimote1.Two
var.btntwo1 = 1
else var.btntwo1 = 0
endif

if Wiimote2.Up                                       //2
var.pady2 = 1
elseif Wiimote2.Down
var.pady2 = -1
else var.pady2 = 0
if Wiimote2.Left
var.padx2 = -1
elseif Wiimote2.Right
var.padx2 = 1
else var.padx2 = 0
if Wiimote2.A
var.padz2 = 1
elseif Wiimote2.B
var.padz2 = -1
else var.padz2 = 0
endif

if Wiimote2.Plus
var.plusminus2 = 1
elseif Wiimote2.Minus
var.plusminus2 = -1
else var.plusminus2 = 0
if Wiimote2.One
var.btnone2 = 1
else var.btnone2 = 0
if Wiimote2.Two
var.btntwo2 = 1
else var.btntwo2 = 0
endif

if abs(var.padx1) + abs(var.pady1)+ abs(var.padz1) > 0
   SendOSC("127.0.0.1", var.PORT1, '/w1PAD', var.padx1, var.pady1, var.padz1)
endif
if abs(var.plusminus1) + var.btnone1 + var.btntwo1 > 0
   SendOSC("127.0.0.1", var.PORT1, '/w1BTN', var.plusminus1, var.btnone1, var.btntwo1)
endif

if abs(var.padx2) + abs(var.pady2)+ abs(var.padz2) > 0
   SendOSC("127.0.0.1", var.PORT1, '/w2PAD', var.padx2, var.pady2, var.padz2)
endif

if abs(var.plusminus2) + var.btnone2 + var.btntwo2 > 0
   SendOSC("127.0.0.1", var.PORT1, '/w2BTN', var.plusminus2, var.btnone2, var.btntwo2)
endif

/////---------------WiiMOTE PRIMARY MOTION---------------
/////---------4DoF: Pitch, Roll:ACC | Yaw, Z:IR----------
var.outPITCH1 = RemoveUnits(var.xRot1)                   //1
var.outROLL1 = RemoveUnits(var.yRot1)
var.outYAW1 = RemoveUnits(asin((var.d1x1-508)/718))
var.outPITCH2 = RemoveUnits(var.xRot2)                   //2
var.outROLL2 = RemoveUnits(var.yRot2)
var.outYAW2 = RemoveUnits(asin((var.d1x2-508)/718))
//var.outYAW = RemoveUnits(asin((wiimote1.dot1x1-508)/718))
//var.outYAW = RemoveUnits(asin((var.dot1x1-508)/718))


if abs(Delta(var.outPITCH1)) + abs(Delta(var.outROLL1)) + abs(Delta(var.outYAW1)) > var.ROTthresh
   SendOSC("127.0.0.1", var.PORT1, '/w1ROT', var.outPITCH1, var.outROLL1, var.outYAW1)
endif
if abs(Delta(var.outPITCH2)) + abs(Delta(var.outROLL2)) + abs(Delta(var.outYAW2)) > var.ROTthresh
   SendOSC("127.0.0.1", var.PORT1, '/w2ROT', var.outPITCH2, var.outROLL2, var.outYAW2)
endif






//////////////////////////////////////////////////////////
/////---------------WiiMOTE MOTION TESTING---------------
//////////////////////////////////////////////////////////
//-------------------COMPENSATED DISTANCE----------------


// --- WiiMOTE 1 (changing to calibrated roll --- if it works do on 2
//var.c1 = cos(Smooth(wiimote1.roll, 10))
//var.s1 = sin(Smooth(wiimote1.roll, 10))
var.c1 = cos(Smooth(var.yRot1, 10))
var.s1 = sin(Smooth(var.yRot1, 10))
if wiimote1.dot1vis then
  var.dot1x1 = var.c1*(511.5-wiimote1.dot1x)/511.5 - var.s1*(wiimote1.dot1y-383.5)/511.5
  var.dot1y1 = var.s1*(511.5-wiimote1.dot1x)/511.5 + var.c1*(wiimote1.dot1y-383.5)/511.5
end if
if wiimote1.dot2vis then
  var.dot2x1 = var.c1*(511.5-wiimote1.dot2x)/511.5 - var.s1*(wiimote1.dot2y-383.5)/511.5
  var.dot2y1 = var.s1*(511.5-wiimote1.dot2x)/511.5 + var.c1*(wiimote1.dot2y-383.5)/511.5
end if

// if both dots are visible check which is which and how far apart
if wiimote1.dot1vis and wiimote1.dot2vis then
  if var.dot1x1 <= var.dot2x1 then
    var.leftdot1 = 1
    var.dotdeltay1 = var.dot2y1 - var.dot1y1
  else
    var.leftdot1 = 2
    var.dotdeltay1 = var.dot1y1 - var.dot2y1
  end if
  var.dotdeltax1 = abs(var.dot1x1-var.dot2x1)
  var.DotSep1 = hypot(var.dotdeltax1, var.dotdeltay1) * 511.5
  var.IRDistance1 = var.SensorBarSeparation * 1320 / var.DotSep1
end if

var.outZ1 = -smooth(delta(var.IRDistance1)) //-var.Zorigin

if abs(var.outZ1) > var.Zminthresh and abs(var.outZ1) < var.Zmaxthresh
   SendOSC("127.0.0.1", var.PORT1, '/w1LOC', var.outZ1)
endif

// --- WiiMOTE 2
var.c2 = cos(Smooth(wiimote2.roll, 10))
var.s2 = sin(Smooth(wiimote2.roll, 10))
if wiimote2.dot1vis then
  var.dot1x2 = var.c2*(511.5-wiimote2.dot1x)/511.5 - var.s2*(wiimote2.dot1y-383.5)/511.5
  var.dot1y2 = var.s2*(511.5-wiimote2.dot1x)/511.5 + var.c2*(wiimote2.dot1y-383.5)/511.5
end if
if wiimote2.dot2vis then
  var.dot2x2 = var.c2*(511.5-wiimote2.dot2x)/511.5 - var.s2*(wiimote2.dot2y-383.5)/511.5
  var.dot2y2 = var.s2*(511.5-wiimote2.dot2x)/511.5 + var.c2*(wiimote2.dot2y-383.5)/511.5
end if

// if both dots are visible check which is which and how far apart
if wiimote2.dot1vis and wiimote2.dot2vis then
  if var.dot1x2 <= var.dot2x2 then
    var.leftdot2 = 1
    var.dotdeltay2 = var.dot2y2 - var.dot1y2
  else
    var.leftdot2 = 2
    var.dotdeltay2 = var.dot1y2 - var.dot2y2
  end if
  var.dotdeltax2 = abs(var.dot1x2-var.dot2x2)
  var.DotSep2 = hypot(var.dotdeltax2, var.dotdeltay2) * 511.5
  var.IRDistance2 = var.SensorBarSeparation * 1320 / var.DotSep2
end if

var.outZ2 = -smooth(delta(var.IRDistance2)) //-var.Zorigin

if abs(var.outZ2) > var.Zminthresh and abs(var.outZ2) < var.Zmaxthresh
   SendOSC("127.0.0.1", var.PORT1, '/w2LOC', var.outZ2)
endif





////////////////////////////////////////////////
//////Keys

if keyboard.NUMPAD0
   var.keyboardnumpad = 0
   if keyboard.NUMPAD1 then var.keyboardnumpad = 1
   if keyboard.NUMPAD2 then var.keyboardnumpad = 2
   if keyboard.NUMPAD3 then var.keyboardnumpad = 3
   if keyboard.NUMPAD4 then var.keyboardnumpad = 4
   if keyboard.NUMPAD5 then var.keyboardnumpad = 5
   if keyboard.NUMPAD6 then var.keyboardnumpad = 6
   if keyboard.NUMPAD7 then var.keyboardnumpad = 7
   if keyboard.NUMPAD8 then var.keyboardnumpad = 8
   if keyboard.NUMPAD9 then var.keyboardnumpad = 9
endif
if var.keyboardnumpad > 0
     SendOSC("127.0.0.1", var.PORT1, '/gpKEYBOARD', var.keyboardnumpad)
     wait 100 ms
endif





////////////////////////////////////////
/////////RESET CALIBRATIO///////////////
////////////////////////////////////////


if Space = true and Z = true then
   wait 50 ms
   var.a1 = 0
   var.b1 = 0
   var.a2 = 0
   var.b2 = 0
   //var.Poff1 = var.a1
   //var.Roff1 = var.b1

   var.Poff1 = 0
   var.Roff1 = 0
   var.Poff2 = 0
   var.Roff2 = 0

   wiimote1.led3 = true
   wiimote2.led3 = true
   wiimote1.led4 = 0
   wiimote2.led4 = 0
   wait 50 ms
   wiimote1.led3 = 0
   wiimote2.led3 = 0
   wiimote1.led4 = true
   wiimote2.led4 = true
   wait 50 ms
   wiimote1.led3 = true
   wiimote2.led3 = true
   wiimote1.led4 = 0
   wiimote2.led4 = 0
   wait 50 ms
   wiimote1.led3 = 0
   wiimote2.led3 = 0
   wiimote1.led4 = true
   wiimote2.led4 = true
   wait 50 ms
   wiimote1.led4 = 0
   wiimote2.led4 = 0
   wait 200 ms
end if