lang/LangPrimSource/WiiMote_OSX/wiiremote.c

   1 // wiiremote.c
   2 // Copyright by Masayuki Akamatsu
   3 // Based on "DarwiinRemote" by Hiroaki Kimura
   4
   5 #include <unistd.h>
   6 #include "wiiremote.h"
   7
   8
   9 // this type is used a lot (data array):
  10 typedef unsigned char darr[];
  11
  12 #define kTrial  10
  13 #define kWait   10000
  14 //  the unit of kWait is microseconds, thus 10000 means 10ms
  15
  16 #define kWiiIRPixelsWidth 1024.0
  17 #define kWiiIRPixelsHeight 768.0
  18
  19
  20 Boolean requestUpdates(WiiRemoteRef wiiremote);
  21 void myEventListener(IOBluetoothL2CAPChannelRef channel, void *refCon, IOBluetoothL2CAPChannelEvent *event);
  22
  23
  24 //--------------------------------------------------------------------------------------------
  25 //--------------------------------------------------------------------------------------------
  26
  27 void wiiremote_init(WiiRemoteRef wiiremote)
  28 {
  29         wiiremote->inquiry = nil;
  30         wiiremote->device = nil;
  31         wiiremote->ichan = nil;
  32         wiiremote->cchan = nil;
  33
  34         wiiremote->address = nil;
  35
  36         wiiremote->accX = 0x10;
  37         wiiremote->accY = 0x10;
  38         wiiremote->accZ = 0x10;
  39         wiiremote->buttonData = 0;
  40
  41         wiiremote->lowZ = 0;
  42         wiiremote->lowX = 0;
  43         wiiremote->leftPoint = -1;
  44         wiiremote->tracking = false;
  45
  46         wiiremote->batteryLevel = 0;
  47
  48         wiiremote->readingRegister = false;
  49         wiiremote->isMotionSensorEnabled = false;
  50         wiiremote->isVibrationEnabled = false;
  51         wiiremote->isIRSensorEnabled = false;
  52         wiiremote->wiiIRMode = kWiiIRModeExtended;
  53         wiiremote->isExpansionPortEnabled = false;
  54         wiiremote->isExpansionPortAttached = false;
  55         wiiremote->expType = WiiExpNotAttached;
  56
  57         wiiremote->isLED1Illuminated = false;
  58         wiiremote->isLED2Illuminated = false;
  59         wiiremote->isLED3Illuminated = false;
  60         wiiremote->isLED4Illuminated = false;
  61
  62         wiiremote->nAccX = 0x10;
  63         wiiremote->nAccY = 0x10;
  64         wiiremote->nAccZ = 0x10;
  65         wiiremote->nButtonData = 0;
  66
  67         wiiremote->nLowZ = 0;
  68         wiiremote->nLowX = 0;
  69
  70 }
  71
  72 //--------------------------------------------------------------------------------------------
  73 //--------------------------------------------------------------------------------------------
  74
  75 Boolean openCChan(WiiRemoteRef wiiremote)
  76 {
  77         short           i;
  78         IOReturn        ret;
  79
  80         // open L2CAPChannel : BluetoothL2CAPPSM = 17
  81         for (i=0; i<kTrial; i++)
  82         {
  83                 ret = IOBluetoothDeviceOpenL2CAPChannelSync(wiiremote->device, &(wiiremote->cchan), 17, myEventListener, (void *)wiiremote);
  84                 if ( ret == kIOReturnSuccess)
  85                         break;
  86                 usleep(kWait); //  wait 10ms
  87         }
  88         if (i==kTrial)
  89         {
  90                 wiiremote->cchan = nil;
  91                 IOBluetoothDeviceCloseConnection(wiiremote->device);
  92                 return false;
  93         }
  94         IOBluetoothObjectRetain(wiiremote->cchan);
  95
  96         return (ret==kIOReturnSuccess);
  97 }
  98
  99 Boolean openIChan(WiiRemoteRef wiiremote)
 100 {
 101         short           i;
 102         IOReturn        ret;
 103
 104         // open L2CAPChannel : BluetoothL2CAPPSM = 19
 105         for (i=0; i<kTrial; i++)
 106         {
 107                 ret = IOBluetoothDeviceOpenL2CAPChannelSync(wiiremote->device, &(wiiremote->ichan), 19, myEventListener, (void *)wiiremote);
 108                 if ( ret == kIOReturnSuccess)
 109                         break;
 110                 usleep(kWait); //  wait 10ms
 111         }
 112         if (i==kTrial)
 113         {
 114                 wiiremote->ichan = nil;
 115                 IOBluetoothL2CAPChannelCloseChannel(wiiremote->cchan);
 116                 IOBluetoothObjectRelease(wiiremote->cchan);
 117                 IOBluetoothDeviceCloseConnection(wiiremote->device);
 118                 return false;
 119         }
 120         IOBluetoothObjectRetain(wiiremote->ichan);
 121
 122         return (ret==kIOReturnSuccess);
 123 }
 124
 125 //--------------------------------------------------------------------------------------------
 126
 127 Boolean sendCommand(WiiRemoteRef wiiremote, unsigned char *data, unsigned short length)
 128 {
 129         unsigned char buf[40];
 130         IOReturn        ret;
 131         int                     i;
 132
 133         memset(buf,0,40);
 134         buf[0] = 0x52;
 135         memcpy(buf+1, data, length);
 136         if (buf[1] == 0x16)
 137                 length=23;
 138         else
 139                 length++;
 140
 141         usleep(kWait); //  wait 10ms    // Done to make sure commands don't happen too fast.
 142
 143         for (i = 0; i<kTrial; i++)
 144         {
 145                 ret = IOBluetoothL2CAPChannelWriteSync(wiiremote->cchan, buf, length);
 146                 if (ret == kIOReturnSuccess)
 147                         break;
 148                 usleep(kWait);
 149         }
 150
 151         if (ret != kIOReturnSuccess)
 152                 wiiremote_disconnect(wiiremote);
 153
 154         return (ret==kIOReturnSuccess);
 155 }
 156
 157 Boolean writeData(WiiRemoteRef wiiremote, const unsigned char *data, unsigned long address, unsigned short length)
 158 {
 159         unsigned char cmd[22];
 160         int i;
 161         unsigned long addr = address;
 162
 163
 164         for(i=0 ; i<length ; i++)
 165                 cmd[i+6] = data[i];
 166
 167         for(;i<16 ; i++)
 168                 cmd[i+6]= 0;
 169
 170         cmd[0] = 0x16;
 171         cmd[1] = (addr>>24) & 0xFF;
 172         cmd[2] = (addr>>16) & 0xFF;
 173         cmd[3] = (addr>> 8) & 0xFF;
 174         cmd[4] = (addr>> 0) & 0xFF;
 175         cmd[5] = length;
 176
 177         // and of course the vibration flag, as usual
 178         if (wiiremote->isVibrationEnabled)      cmd[1] |= 0x01;
 179
 180         data = cmd;
 181
 182         return sendCommand(wiiremote, cmd, 22);
 183 }
 184
 185 Boolean readData(WiiRemoteRef wiiremote, unsigned long address, unsigned short length)
 186 {
 187
 188         unsigned char cmd[7];
 189         unsigned long addr = address;
 190         unsigned short len = length;
 191
 192         cmd[0] = 0x17;
 193         cmd[1] = (addr>>24)&0xFF;
 194         cmd[2] = (addr>>16)&0xFF;
 195         cmd[3] = (addr>> 8)&0xFF;
 196         cmd[4] = (addr>> 0)&0xFF;
 197
 198         cmd[5] = (len >> 8)&0xFF;
 199         cmd[6] = (len >> 0)&0xFF;
 200
 201         if (wiiremote->isVibrationEnabled)      cmd[1] |= 0x01;
 202         if (cmd[1] & 0x02) wiiremote->readingRegister = true;
 203
 204         return sendCommand(wiiremote, cmd, 7);
 205 }
 206
 207 //--------------------------------------------------------------------------------------------
 208 //--------------------------------------------------------------------------------------------
 209
 210 void checkDevice(WiiRemoteRef wiiremote, IOBluetoothDeviceRef device)
 211 {
 212         CFStringRef     name;
 213         CFStringRef     address;
 214
 215         if (wiiremote_isconnected(wiiremote))
 216                 return;
 217
 218         name = IOBluetoothDeviceGetName(device);
 219         address = IOBluetoothDeviceGetAddressString(device);
 220         if (name != nil && address != nil)
 221         {
 222                 if (CFStringCompare(name, CFSTR("Nintendo RVL-CNT-01"), 0) == kCFCompareEqualTo)
 223                 {
 224                         if ( CFStringGetLength(wiiremote->address) == 0
 225                         || CFStringCompare(address, wiiremote->address, kCFCompareCaseInsensitive) == kCFCompareEqualTo)
 226                         {
 227                                 wiiremote->device = IOBluetoothObjectRetain(device);
 228                                 if ( wiiremote_connect(wiiremote) == false )
 229                                         wiiremote_disconnect(wiiremote);
 230                         }
 231                 }
 232         }
 233 }
 234
 235 void myFoundFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOBluetoothDeviceRef device)
 236 {
 237         checkDevice((WiiRemoteRef)refCon, device);
 238 }
 239
 240 void myUpdatedFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOBluetoothDeviceRef device, uint32_t devicesRemaining)
 241 {
 242         checkDevice((WiiRemoteRef)refCon, device);
 243 }
 244
 245 void myCompleteFunc(void *refCon, IOBluetoothDeviceInquiryRef inquiry, IOReturn error, Boolean aborted)
 246 {
 247         IOReturn        ret;
 248
 249         if (aborted) return; // called by stop ;)
 250
 251         if (error != kIOReturnSuccess)
 252         {
 253                 wiiremote_stopsearch((WiiRemoteRef)refCon);
 254                 return;
 255         }
 256
 257         /*
 258          ret = IOBluetoothDeviceInquiryStart(((WiiRemoteRef)refCon)->inquiry);
 259         if (ret != kIOReturnSuccess)
 260         {
 261                 wiiremote_stopsearch((WiiRemoteRef)refCon);
 262         }
 263         */
 264 }
 265
 266 //--------------------------------------------------------------------------------------------
 267
 268 Boolean wiiremote_isconnected(WiiRemoteRef wiiremote)
 269 {
 270         Boolean result;
 271
 272         result = wiiremote->device != nil && IOBluetoothDeviceIsConnected(wiiremote->device);
 273         return result;
 274 }
 275
 276 Boolean wiiremote_search(WiiRemoteRef wiiremote, char *address)
 277 {
 278         IOReturn        ret;
 279
 280         if (wiiremote->inquiry != nil)
 281                 return true;
 282
 283         wiiremote->inquiry = IOBluetoothDeviceInquiryCreateWithCallbackRefCon((void *)wiiremote);
 284         IOBluetoothDeviceInquirySetDeviceFoundCallback(wiiremote->inquiry, myFoundFunc);
 285         IOBluetoothDeviceInquirySetDeviceNameUpdatedCallback(wiiremote->inquiry, myUpdatedFunc);
 286         IOBluetoothDeviceInquirySetCompleteCallback(wiiremote->inquiry, myCompleteFunc);
 287
 288         if (wiiremote->address != nil)
 289                 CFRelease(wiiremote->address);
 290         wiiremote->address = CFStringCreateWithCString(nil, address, kCFStringEncodingMacRoman);
 291
 292         ret = IOBluetoothDeviceInquiryStart(wiiremote->inquiry);
 293         if (ret != kIOReturnSuccess)
 294         {
 295                 IOBluetoothDeviceInquiryDelete(wiiremote->inquiry);
 296                 wiiremote->inquiry = nil;
 297                 return false;
 298         }
 299         return true;
 300 }
 301
 302 Boolean wiiremote_stopsearch(WiiRemoteRef wiiremote)
 303 {
 304         IOReturn        ret;
 305
 306         if (wiiremote->inquiry == nil)
 307         {
 308                 return true;    // already stopped
 309         }
 310
 311         ret = IOBluetoothDeviceInquiryStop(wiiremote->inquiry);
 312
 313         if (ret != kIOReturnSuccess && ret != kIOReturnNotPermitted)
 314         {
 315                 // kIOReturnNotPermitted is if it's already stopped
 316         }
 317
 318         IOBluetoothDeviceInquiryDelete(wiiremote->inquiry);
 319         wiiremote->inquiry = nil;
 320
 321         return (ret==kIOReturnSuccess);
 322 }
 323
 324 //--------------------------------------------------------------------------------------------
 325 //--------------------------------------------------------------------------------------------
 326
 327 unsigned char decrypt(unsigned char data)
 328 {
 329         return (data ^ 0x17) + 0x17;
 330 }
 331
 332 //--------------------------------------------------------------------------------------------
 333
 334 /**
 335 * Handle report 0x21 (Read Data) from wiimote.
 336  * dp[0] = Bluetooth header
 337  * dp[1] = (0x21) Report/Channel ID
 338  * dp[2] = Wiimote Buttons
 339  * dp[3] = Wiimote Buttons
 340  * dp[4] = High 4 bits = payload size; Low 4 bits = Error flag (0 = all good)
 341  * dp[5] = Offset of memory read
 342  * dp[6] = Offset of memory read
 343  * dp[7+] = the Data.
 344  **/
 345
 346 void handleRAMData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
 347 {
 348         // specify attached expasion device
 349         if ((dp[5] == 0x00) && (dp[6] == 0xF0))
 350         {
 351                 if (decrypt(dp[21]) == 0x00)
 352                 {
 353                         wiiremote->expType = WiiNunchuk;
 354                 }
 355                 else
 356                 if (decrypt(dp[21]) == 0x01)
 357                 {
 358                         wiiremote->expType = WiiClassicController;
 359                 }
 360                 else
 361                 {
 362                         wiiremote->expType = WiiExpNotAttached;
 363                 }
 364                 //              initExpPort = NO;
 365                 return;
 366         }
 367
 368         // wiimote calibration data
 369         if (!wiiremote->readingRegister && dp[5] == 0x00 && dp[6] == 0x20)
 370         {
 371                 wiiremote->wiiCalibData.accX_zero = dp[7];
 372                 wiiremote->wiiCalibData.accY_zero = dp[8];
 373                 wiiremote->wiiCalibData.accZ_zero = dp[9];
 374
 375                 //dp[10] - unknown/unused
 376
 377                 wiiremote->wiiCalibData.accX_1g = dp[11];
 378                 wiiremote->wiiCalibData.accY_1g = dp[12];
 379                 wiiremote->wiiCalibData.accZ_1g = dp[13];
 380                 return;
 381         }
 382
 383         // expansion device calibration data.
 384         if (wiiremote->readingRegister && dp[5] == 0x00 && dp[6] == 0x20)
 385         {
 386                 if (wiiremote->expType == WiiNunchuk)
 387                 {
 388                         //nunchuk calibration data
 389                         wiiremote->nunchukCalibData.accX_zero =  decrypt(dp[7]);
 390                         wiiremote->nunchukCalibData.accY_zero =  decrypt(dp[8]);
 391                         wiiremote->nunchukCalibData.accZ_zero =  decrypt(dp[9]);
 392
 393                         wiiremote->nunchukCalibData.accX_1g =  decrypt(dp[11]);
 394                         wiiremote->nunchukCalibData.accY_1g =  decrypt(dp[12]);
 395                         wiiremote->nunchukCalibData.accZ_1g =  decrypt(dp[13]);
 396
 397                         wiiremote->nunchukJoyStickCalibData.x_max =  decrypt(dp[15]);
 398                         wiiremote->nunchukJoyStickCalibData.x_min =  decrypt(dp[16]);
 399                         wiiremote->nunchukJoyStickCalibData.x_center =  decrypt(dp[17]);
 400
 401                         wiiremote->nunchukJoyStickCalibData.y_max =  decrypt(dp[18]);
 402                         wiiremote->nunchukJoyStickCalibData.y_min =  decrypt(dp[19]);
 403                         wiiremote->nunchukJoyStickCalibData.y_center =  decrypt(dp[20]);
 404
 405                         return;
 406                 }
 407                 else
 408                 if (wiiremote->expType == WiiClassicController)
 409                 {
 410                         //classic controller calibration data (probably)
 411                 }
 412         }
 413
 414         // wii remote buttons
 415         wiiremote->buttonData = ((short)dp[2] << 8) + dp[3];
 416 }
 417
 418 void handleStatusReport(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
 419 {
 420         wiiremote->batteryLevel = (double)dp[7];
 421         wiiremote->batteryLevel /= (double)0xC0; // C0 = fully charged.
 422
 423         if ((dp[4] & 0x02)) //some device attached to Wiimote
 424         {
 425                 wiiremote->isExpansionPortAttached = true;
 426                 // initExpPort = YES;
 427
 428                 Boolean ret = writeData(wiiremote, (darr){0x00}, 0x04A40040, 1); // Initialize the device
 429
 430                 if (ret == false)
 431                 {
 432                         wiiremote->isExpansionPortAttached = false;
 433                         return;
 434                 }
 435
 436                 usleep(kWait); // Give the write a chance to be processed.
 437
 438                 ret = readData(wiiremote, 0x04A400F0, 16); // read expansion device type
 439                 if (ret == false)
 440                 {
 441                         wiiremote->isExpansionPortAttached = false;
 442                 }
 443         }
 444         else
 445         { // unplugged
 446                 wiiremote->isExpansionPortAttached = false;
 447                 wiiremote->expType = WiiExpNotAttached;
 448         }
 449
 450         if (dp[4] & 0x10)
 451                 wiiremote->isLED1Illuminated = true;
 452         else
 453                 wiiremote->isLED1Illuminated = false;
 454
 455         if (dp[4] & 0x20)
 456                 wiiremote->isLED2Illuminated = true;
 457         else
 458                 wiiremote->isLED2Illuminated = false;
 459
 460         if (dp[4] & 0x40)
 461                 wiiremote->isLED3Illuminated = true;
 462         else
 463                 wiiremote->isLED3Illuminated = false;
 464
 465         if (dp[4] & 0x80)
 466                 wiiremote->isLED4Illuminated = true;
 467         else
 468                 wiiremote->isLED4Illuminated = false;
 469 }
 470
 471 void handleExtensionData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
 472 {
 473         unsigned char startByte;
 474
 475         switch (dp[1]) {
 476                 case 0x34 :
 477                         startByte = 4;
 478                         break;
 479                 case 0x35 :
 480                         startByte = 7;
 481                         break;
 482                 case 0x36 :
 483                         startByte = 14;
 484                         break;
 485                 case 0x37 :
 486                         startByte = 17;
 487                         break;
 488                 default:
 489                         return; // This shouldn't ever happen.
 490                         break;
 491         }
 492
 493         if (wiiremote->expType == WiiNunchuk)
 494         {
 495                 wiiremote->nStickX = decrypt(dp[startByte]);
 496                 wiiremote->nStickY = decrypt(dp[startByte +1]);
 497                 wiiremote->nAccX   = decrypt(dp[startByte +2]);
 498                 wiiremote->nAccY   = decrypt(dp[startByte +3]);
 499                 wiiremote->nAccZ   = decrypt(dp[startByte +4]);
 500                 wiiremote->nButtonData = decrypt(dp[startByte +5]);
 501
 502                 wiiremote->nLowZ = wiiremote->nLowZ * .9 + wiiremote->nAccZ * .1;
 503                 wiiremote->nLowX = wiiremote->nLowX * .9 + wiiremote->nAccX * .1;
 504
 505                 float absx = abs(wiiremote->nLowX - 128);
 506                 float absz = abs(wiiremote->nLowZ - 128);
 507
 508                 if (wiiremote->nOrientation == 0 || wiiremote->nOrientation == 2) absx -= 5;
 509                 if (wiiremote->nOrientation == 1 || wiiremote->nOrientation == 3) absz -= 5;
 510
 511                 if (absz >= absx)
 512                 {
 513                         if (absz > 5)
 514                                 wiiremote->nOrientation = (wiiremote->nLowZ > 128) ? 0 : 2;
 515                 }
 516                 else
 517                 {
 518                         if (absx > 5)
 519                                 wiiremote->nOrientation = (wiiremote->nLowX > 128) ? 3 : 1;
 520                 }
 521         }
 522         else
 523         if (wiiremote->expType == WiiClassicController)
 524         {
 525                 wiiremote->cButtonData = (unsigned short)(decrypt(dp[startByte + 4]) << 8) + decrypt(dp[startByte + 5]);
 526                 wiiremote->cButtonData = ~wiiremote->cButtonData;       // bit reverse
 527
 528                 wiiremote->cStickX1 = decrypt(dp[startByte]) & 0x3F;
 529                 wiiremote->cStickY1 = decrypt(dp[startByte + 1]) & 0x3F;
 530
 531                 wiiremote->cStickX2 = (((decrypt(dp[startByte +0]) & 0xC0) >> 3) |
 532                                                            ((decrypt(dp[startByte +1]) & 0xC0) >> 5) |
 533                                                            ((decrypt(dp[startByte +2]) & 0x80) >> 7)) & 0x1F;
 534                 wiiremote->cStickY2 = decrypt(dp[startByte + 2]) & 0x1F;
 535
 536                 wiiremote->cAnalogL = (((decrypt(dp[startByte +2]) & 0x60) >> 2) |
 537                                                            ((decrypt(dp[startByte +3]) & 0xE0) >> 5)) & 0x1F;
 538                 wiiremote->cAnalogR =  decrypt(dp[startByte + 3]) & 0x1F;
 539         }
 540 }
 541
 542 void handleIRData(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
 543 {
 544         int i;
 545
 546         if (dp[1] == 0x33)
 547         { // 12 IR bytes
 548                 int startByte = 0;
 549                 for(i=0 ; i < 4 ; i++)
 550                 {
 551                         startByte = 7 + 3 * i;
 552                         wiiremote->irData[i].x = (dp[startByte +0] | ((dp[startByte +2] & 0x30) << 4)) & 0x3FF;
 553                         wiiremote->irData[i].y = (dp[startByte +1] | ((dp[startByte +2] & 0xC0) << 2)) & 0x3FF;
 554                         wiiremote->irData[i].s =  dp[startByte +2] & 0x0F;
 555                 }
 556         }
 557         else
 558         { // 10 IR bytes
 559                 int shift = (dp[1] == 0x36) ? 4 : 7;
 560                 int startByte = 0;
 561                 for (i=0; i < 2; i++) {
 562                         startByte = shift + 5 * i;
 563                         wiiremote->irData[2*i].x = (dp[startByte +0] | ((dp[startByte +2] & 0x30) << 4)) & 0x3FF;
 564                         wiiremote->irData[2*i].y = (dp[startByte +1] | ((dp[startByte +2] & 0xC0) << 2)) & 0x3FF;
 565                         wiiremote->irData[2*i].s = ((wiiremote->irData[2*i].x == wiiremote->irData[2*i].y) && (wiiremote->irData[2*i].x == 0x3FF)) ? 0x0F : 0x05;  // No size is given in 10 byte report.
 566
 567                         wiiremote->irData[(2*i)+1].x = (dp[startByte +3] | ((dp[startByte +2] & 0x03) << 8)) & 0x3FF;
 568                         wiiremote->irData[(2*i)+1].y = (dp[startByte +4] | ((dp[startByte +2] & 0x0C) << 6)) & 0x3FF;
 569                         wiiremote->irData[(2*i)+1].s = ((wiiremote->irData[(2*i)+1].x == wiiremote->irData[(2*i)+1].y) && (wiiremote->irData[(2*i)+1].x == 0x3FF)) ? 0x0F : 0x05;  // No size is given in 10 byte report.
 570                 }
 571         }
 572
 573         int p1 = -1;
 574         int p2 = -1;
 575         // we should modify this loop to take the points with the lowest s (the brightest ones)
 576         for (i=0 ; i<4 ; i++) {
 577                 if (p1 == -1) {
 578                         if (wiiremote->irData [i].s < 0x0F)
 579                                 p1 = i;
 580                 } else {
 581                         if (wiiremote->irData [i].s < 0x0F) {
 582                                 p2 = i;
 583                                 break;
 584                         }
 585                 }
 586         }
 587
 588         double ox, oy;
 589         if ((p1 > -1) && (p2 > -1))
 590         {
 591                 int l = wiiremote->leftPoint;
 592                 if (wiiremote->leftPoint == -1)
 593                 {
 594                         switch (wiiremote->orientation)
 595                         {
 596                                 case 0: l = (wiiremote->irData[p1].x < wiiremote->irData[p2].x) ? 0 : 1; break;
 597                                 case 1: l = (wiiremote->irData[p1].y > wiiremote->irData[p2].y) ? 0 : 1; break;
 598                                 case 2: l = (wiiremote->irData[p1].x > wiiremote->irData[p2].x) ? 0 : 1; break;
 599                                 case 3: l = (wiiremote->irData[p1].y < wiiremote->irData[p2].y) ? 0 : 1; break;
 600                         }
 601
 602                         wiiremote->leftPoint = l;
 603                 }
 604
 605                 int r = 1-l;
 606
 607                 double dx = wiiremote->irData[r].x - wiiremote->irData[l].x;
 608                 double dy = wiiremote->irData[r].y - wiiremote->irData[l].y;
 609                 double d = hypot (dx, dy);
 610
 611                 dx /= d;
 612                 dy /= d;
 613
 614                 double cx = (wiiremote->irData[l].x + wiiremote->irData[r].x)/kWiiIRPixelsWidth - 1;
 615                 double cy = (wiiremote->irData[l].y + wiiremote->irData[r].y)/kWiiIRPixelsHeight - 1;
 616
 617                 ox = -dy*cy-dx*cx;
 618                 oy = -dx*cy+dy*cx;
 619
 620                 // cam:
 621                 // Compensate for distance. There must be fewer than 0.75*768 pixels between the spots for this to work.
 622                 // In other words, you have to be far enough away from the sensor bar for the two spots to have enough
 623                 // space on the image sensor to travel without one of the points going off the image.
 624                 // note: it is working very well ...
 625                 double gain = 4;
 626                 if (d < (0.75 * kWiiIRPixelsHeight))
 627                         gain = 1 / (1 - d/kWiiIRPixelsHeight);
 628
 629                 ox *= gain;
 630                 oy *= gain;
 631
 632                 wiiremote->angle = atan2(dy, dx);
 633                 wiiremote->tracking = true;
 634         }
 635         else
 636         {
 637                 ox = oy = -100;
 638                 wiiremote->leftPoint = -1;      // not tracking
 639                 wiiremote->angle = -100;
 640                 wiiremote->tracking = false;
 641         }
 642
 643         wiiremote->posX = ox;
 644         wiiremote->posY = oy;
 645 }
 646
 647 void handleButtonReport(WiiRemoteRef wiiremote, unsigned char *dp, size_t dataLength)
 648 {
 649         // wiimote buttons
 650         wiiremote->buttonData = ((short)dp[2] << 8) + dp[3];
 651
 652         // report contains extension data
 653         switch (dp[1])
 654         {
 655                 case 0x34 :
 656                 case 0x35 :
 657                 case 0x36 :
 658                 case 0x37 :
 659                         handleExtensionData(wiiremote, dp, dataLength);
 660                         break;
 661         }
 662
 663         // report contains IR data
 664         if (dp[1] & 0x02)
 665         {
 666                 handleIRData(wiiremote, dp, dataLength);
 667         }
 668
 669         // report contains motion sensor data
 670         if (dp[1] & 0x01)
 671         {
 672                 wiiremote->accX = dp[4];
 673                 wiiremote->accY = dp[5];
 674                 wiiremote->accZ = dp[6];
 675
 676                 wiiremote->lowZ = wiiremote->lowZ * 0.9 + wiiremote->accZ * 0.1;
 677                 wiiremote->lowX = wiiremote->lowX * 0.9 + wiiremote->accX * 0.1;
 678
 679                 float absx = abs(wiiremote->lowX-128);
 680                 float absz = abs(wiiremote->lowZ-128);
 681
 682                 if (wiiremote->orientation == 0 || wiiremote->orientation == 2) absx -= 5;
 683                 if (wiiremote->orientation == 1 || wiiremote->orientation == 3) absz -= 5;
 684
 685                 if (absz >= absx)
 686                 {
 687                         if (absz > 5)
 688                                 wiiremote->orientation = (wiiremote->lowZ > 128)?0:2;
 689                 }
 690                 else
 691                 {
 692                         if (absx > 5)
 693                                 wiiremote->orientation = (wiiremote->lowX > 128)?3:1;
 694                 }
 695         }
 696 }
 697
 698 //--------------------------------------------------------------------------------------------
 699
 700  void myDataListener(IOBluetoothL2CAPChannelRef channel, void *dataPointer, UInt16 dataLength, void *refCon)
 701 {
 702         WiiRemoteRef    wiiremote = (WiiRemoteRef)refCon;
 703         unsigned char* dp = (unsigned char*)dataPointer;
 704
 705         if (!wiiremote->device)
 706                 return;
 707
 708         //controller status (expansion port and battery level data) - received when report 0x15 sent to Wiimote (getCurrentStatus:) or status of expansion port changes.
 709         if (dp[1] == 0x20 && dataLength >= 8)
 710         {
 711                 handleStatusReport(wiiremote, dp, dataLength);
 712                 requestUpdates(wiiremote); // Make sure we keep getting state change reports.
 713                 return;
 714         }
 715
 716         if (dp[1] == 0x21)
 717         {
 718                 handleRAMData(wiiremote, dp, dataLength);
 719                 return;
 720         }
 721
 722         if (dp[1] == 0x22)
 723         { // Write data response
 724                 //NSLog(@"Write data response: %00x %00x %00x %00x", dp[2], dp[3], dp[4], dp[5]);
 725                 return;
 726         }
 727
 728         // report contains button info
 729         if ((dp[1] & 0xF0) == 0x30)
 730         {
 731                 handleButtonReport(wiiremote, dp, dataLength);
 732         }
 733 }
 734
 735 void myEventListener(IOBluetoothL2CAPChannelRef channel, void *refCon, IOBluetoothL2CAPChannelEvent *event)
 736 {
 737         if (event->eventType == kIOBluetoothL2CAPChannelEventTypeData)
 738         {
 739                 // In thise case:
 740                 // event->u.newData.dataPtr  is a pointer to the block of data received.
 741                 // event->u.newData.dataSize is the size of the block of data.
 742                 myDataListener(channel, event->u.data.dataPtr, event->u.data.dataSize, refCon);
 743         }
 744         else
 745         if (event->eventType == kIOBluetoothL2CAPChannelEventTypeClosed)
 746         {
 747                 // In this case:
 748                 // event->u.terminatedChannel is the channel that was terminated. It can be converted in an IOBluetoothL2CAPChannel
 749                 // object with [IOBluetoothL2CAPChannel withL2CAPChannelRef:]. (see below).
 750         }
 751 }
 752
 753 void myDisconnectedFunc(void * refCon, IOBluetoothUserNotificationRef inRef, IOBluetoothObjectRef objectRef)
 754 {
 755         CFStringRef     itsAddress, myAddress;
 756
 757         itsAddress = IOBluetoothDeviceGetAddressString(objectRef);
 758         if (itsAddress != nil)
 759         {
 760                 myAddress = IOBluetoothDeviceGetAddressString(((WiiRemoteRef)refCon)->device);
 761                 if (myAddress != nil)
 762                 {
 763                         if (CFStringCompare(itsAddress, myAddress, 0) == kCFCompareEqualTo)
 764                         {
 765                                 wiiremote_disconnect((WiiRemoteRef)refCon);
 766                         }
 767                         CFRelease(myAddress);
 768                 }
 769                 CFRelease(itsAddress);
 770         }
 771 }
 772
 773 //--------------------------------------------------------------------------------------------
 774
 775 void wiiremote_getaddress(WiiRemoteRef wiiremote, char *address)
 776 {
 777         CFStringRef     cfstring;
 778
 779         cfstring = IOBluetoothDeviceGetAddressString(wiiremote->device);
 780         CFStringGetCString(cfstring, address, 32, kCFStringEncodingMacRoman);
 781         CFRelease(cfstring);
 782
 783 }
 784
 785 //--------------------------------------------------------------------------------------------
 786
 787 Boolean wiiremote_connect(WiiRemoteRef wiiremote)
 788 {
 789         IOReturn        ret;
 790         Boolean         result;
 791         short           i;
 792
 793         if (wiiremote->device == nil)
 794                 return false;
 795
 796         // connect the device
 797         for (i=0; i<kTrial; i++)
 798         {
 799                 ret = IOBluetoothDeviceOpenConnection(wiiremote->device, nil, nil);
 800                 if ( ret == kIOReturnSuccess)
 801                         break;
 802                 usleep(kWait); //  wait 10ms
 803         }
 804         if (i==kTrial)
 805                 return false;
 806
 807         wiiremote->disconnectNotification = IOBluetoothDeviceRegisterForDisconnectNotification(wiiremote->device, myDisconnectedFunc, (void *)wiiremote);
 808
 809         // performs an SDP query
 810         for (i=0; i<kTrial; i++)
 811         {
 812                 ret = IOBluetoothDevicePerformSDPQuery(wiiremote->device, nil, nil);
 813                 if ( ret == kIOReturnSuccess)
 814                         break;
 815                 usleep(kWait); //  wait 10ms
 816         }
 817         if (i==kTrial)
 818                 return false;
 819
 820         result = openCChan(wiiremote);
 821         result = openIChan(wiiremote);
 822
 823         if (result)
 824         {
 825                 result = wiiremote_led(wiiremote, wiiremote->isLED1Illuminated, wiiremote->isLED2Illuminated, wiiremote->isLED3Illuminated, wiiremote->isLED4Illuminated);
 826         }
 827
 828         if (result == false)
 829         {
 830                 wiiremote_disconnect(wiiremote);
 831                 return result;
 832         }
 833
 834         wiiremote_getstatus(wiiremote);
 835         requestUpdates(wiiremote);
 836
 837         readData(wiiremote, 0x0020, 7); //      Get Accelerometer callibration data
 838
 839         return true;
 840 }
 841
 842
 843 Boolean wiiremote_disconnect(WiiRemoteRef wiiremote)
 844 {
 845         short   i;
 846
 847         if (wiiremote->cchan)
 848         {
 849                 if (IOBluetoothDeviceIsConnected(wiiremote->device))
 850                 {
 851                         for (i=0; i<kTrial; i++)
 852                         {
 853                                 if (IOBluetoothL2CAPChannelCloseChannel(wiiremote->cchan) == kIOReturnSuccess)
 854                                         break;
 855                                 usleep(kWait); //  wait 10ms
 856                         }
 857                 }
 858                 if (i==kTrial)  return false;
 859                 IOBluetoothObjectRelease(wiiremote->cchan);
 860                 wiiremote->cchan = nil;
 861         }
 862
 863         if (wiiremote->ichan)
 864         {
 865                 if (IOBluetoothDeviceIsConnected(wiiremote->device))
 866                 {
 867                         for (i=0; i<kTrial; i++)
 868                         {
 869                                 if (IOBluetoothL2CAPChannelCloseChannel(wiiremote->ichan) == kIOReturnSuccess)
 870                                         break;
 871                         }
 872                 }
 873                 if (i==kTrial)  return false;
 874                 IOBluetoothObjectRelease(wiiremote->ichan);
 875                 wiiremote->ichan = nil;
 876         }
 877
 878         if (wiiremote->device)
 879         {
 880                 if (IOBluetoothDeviceIsConnected(wiiremote->device))
 881                 {
 882                         for (i=0; i<kTrial; i++)
 883                         {
 884                                 if (IOBluetoothDeviceCloseConnection(wiiremote->device) == kIOReturnSuccess)
 885                                         break;
 886                         }
 887                 }
 888                 if (i==kTrial)  return false;
 889                 IOBluetoothObjectRelease(wiiremote->device);
 890                 wiiremote->device = nil;
 891         }
 892
 893         if (wiiremote->disconnectNotification != nil)
 894         {
 895                 IOBluetoothUserNotificationUnregister(wiiremote->disconnectNotification);
 896                 wiiremote->disconnectNotification = nil;
 897         }
 898
 899         return true;
 900 }
 901
 902 //--------------------------------------------------------------------------------------------
 903 //--------------------------------------------------------------------------------------------
 904
 905 Boolean requestUpdates(WiiRemoteRef wiiremote)
 906 {
 907         Boolean result;
 908
 909         // Set the report type the Wiimote should send.
 910         unsigned char cmd[] = {0x12, 0x02, 0x30}; // Just buttons.
 911
 912         if (wiiremote->isVibrationEnabled)      cmd[1] |= 0x01;
 913
 914         /*
 915          There are numerous status report types that can be requested.
 916          The IR reports must be matched with the data format set when initializing the IR camera:
 917          0x36, 0x37     - 10 IR bytes go with Basic mode
 918          0x33           - 12 IR bytes go with Extended mode
 919          0x3e/0x3f      - 36 IR bytes go with Full mode
 920
 921          The Nunchuk and Classic controller use 6 bytes to report their state, so the reports that
 922          give more extension bytes don't provide any more info.
 923
 924          Buttons                |       Accelerometer   |       IR              |       Extension
 925          --------------------+-------------------+-----------+-------------
 926          0x30: Core Buttons     |                                       |                       |
 927          0x31: Core Buttons     |       Accelerometer   |                       |
 928          0x32: Core Buttons     |                                       |                       |        8 bytes
 929          0x33: Core Buttons     |       Accelerometer   | 12 bytes      |
 930          0x34: Core Buttons     |                                       |                       |       19 bytes
 931          0x35: Core Buttons     |       Accelerometer   |                       |       16 bytes
 932          0x36: Core Buttons     |                                       | 10 bytes      |        9 bytes
 933          0x37: Core Buttons     |       Accelerometer   | 10 bytes      |        6 bytes
 934          ?? 0x38: Core Buttons and Accelerometer with 16 IR bytes ??
 935          0x3d:                          |                                       |                       |       21 bytes
 936
 937          0x3e / 0x3f: Interleaved Core Buttons and Accelerometer with 16/36 IR bytes
 938
 939          */
 940
 941         if (wiiremote->isIRSensorEnabled)
 942         {
 943                 if (wiiremote->isExpansionPortEnabled)
 944                 {
 945                         cmd[2] = 0x36;  // Buttons, 10 IR Bytes, 9 Extension Bytes
 946                         wiiremote->wiiIRMode = kWiiIRModeBasic;
 947                 }
 948                 else
 949                 {
 950                         cmd[2] = 0x33; // Buttons, Accelerometer, and 12 IR Bytes.
 951                         wiiremote->wiiIRMode = kWiiIRModeExtended;
 952                 }
 953
 954                 // Set IR Mode
 955                 writeData(wiiremote, (darr){ wiiremote->wiiIRMode }, 0x04B00033, 1);
 956                 usleep(kWait); //  wait 10ms
 957         }
 958         else
 959         {
 960                 if (wiiremote->isExpansionPortEnabled)
 961                 {
 962                         cmd[2] = 0x34;  // Buttons, 19 Extension Bytes
 963                 }
 964                 else
 965                 {
 966                         cmd[2] = 0x30; // Buttons
 967                 }
 968         }
 969
 970         if (wiiremote->isMotionSensorEnabled)   cmd[2] |= 0x01; // Add Accelerometer
 971
 972         usleep(kWait); //  wait 10ms
 973         result = sendCommand(wiiremote, cmd, 3);
 974
 975         return(result);
 976 }
 977
 978 //--------------------------------------------------------------------------------------------
 979
 980 Boolean wiiremote_motionsensor(WiiRemoteRef wiiremote, Boolean enabled)
 981 {
 982         wiiremote->isMotionSensorEnabled = enabled;
 983         return requestUpdates(wiiremote);
 984 }
 985
 986 Boolean wiiremote_vibration(WiiRemoteRef wiiremote, Boolean enabled)
 987 {
 988
 989         wiiremote->isVibrationEnabled = enabled;
 990         return requestUpdates(wiiremote);
 991 }
 992
 993 Boolean wiiremote_led(WiiRemoteRef wiiremote, Boolean enabled1, Boolean enabled2, Boolean enabled3, Boolean enabled4)
 994 {
 995         unsigned char cmd[] = {0x11, 0x00};
 996         if (wiiremote->isVibrationEnabled)      cmd[1] |= 0x01;
 997         if (enabled1)   cmd[1] |= 0x10;
 998         if (enabled2)   cmd[1] |= 0x20;
 999         if (enabled3)   cmd[1] |= 0x40;
1000         if (enabled4)   cmd[1] |= 0x80;
1001
1002         wiiremote->isLED1Illuminated = enabled1;
1003         wiiremote->isLED2Illuminated = enabled2;
1004         wiiremote->isLED3Illuminated = enabled3;
1005         wiiremote->isLED4Illuminated = enabled4;
1006
1007         return sendCommand(wiiremote, cmd, 2);
1008 }
1009
1010 Boolean wiiremote_expansion(WiiRemoteRef wiiremote, Boolean enabled)
1011 {
1012         wiiremote->isExpansionPortEnabled = enabled;
1013         if (wiiremote->isExpansionPortAttached == false)
1014         {
1015                 wiiremote->isExpansionPortEnabled = false;
1016         }
1017         else
1018         {
1019                 readData(wiiremote, 0x04A40020, 16);    //get calbdata
1020         }
1021
1022         return requestUpdates(wiiremote);
1023 }
1024
1025 Boolean wiiremote_irsensor(WiiRemoteRef wiiremote, Boolean enabled)
1026 {
1027         Boolean ret;
1028
1029         wiiremote->isIRSensorEnabled = enabled;
1030
1031         // ir enable 1
1032         unsigned char cmd[] = {0x13, 0x00};
1033         if (wiiremote->isVibrationEnabled)      cmd[1] |= 0x01;
1034         if (wiiremote->isIRSensorEnabled)       cmd[1] |= 0x04;
1035         if ((ret = sendCommand(wiiremote, cmd, 2)) == false)
1036                 return ret;
1037         usleep(kWait);
1038
1039         // set register 0x1a (ir enable 2)
1040         unsigned char cmd2[] = {0x1a, 0x00};
1041         if (enabled)    cmd2[1] |= 0x04;
1042         if ((ret = sendCommand(wiiremote, cmd2, 2)) == false)
1043                 return ret;
1044         usleep(kWait);
1045
1046         if(enabled)
1047         {
1048                 // based on marcan's method, found on wiili wiki:
1049                 // tweaked to include some aspects of cliff's setup procedure in the hopes
1050                 // of it actually turning on 100% of the time (was seeing 30-40% failure rate before)
1051                 // the sleeps help it it seems
1052                 usleep(kWait);
1053                 if ((ret = writeData(wiiremote, (darr){0x01}, 0x04B00030, 1)) == false) return ret;
1054                 usleep(kWait);
1055                 if ((ret = writeData(wiiremote, (darr){0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0xC0}, 0x04B00000, 9)) == false) return ret;
1056                 usleep(kWait);
1057                 if ((ret = writeData(wiiremote, (darr){0x40, 0x00}, 0x04B0001A, 2)) == false)   return ret;
1058                 usleep(kWait);
1059                 if ((ret = writeData(wiiremote, (darr){0x08}, 0x04B00030, 1)) == false) return ret;
1060                 usleep(kWait);
1061
1062                 requestUpdates(wiiremote);
1063         }
1064         else
1065         {
1066                 // probably should do some writes to power down the camera, save battery
1067                 // but don't know how yet.
1068
1069                 ret = wiiremote_motionsensor(wiiremote, wiiremote->isMotionSensorEnabled);
1070                 ret = wiiremote_vibration(wiiremote, wiiremote->isVibrationEnabled);
1071                 ret = wiiremote_expansion(wiiremote, wiiremote->isExpansionPortEnabled);
1072         }
1073
1074         return ret;
1075 }
1076
1077 Boolean wiiremote_getstatus(WiiRemoteRef wiiremote)
1078 {
1079         unsigned char cmd[] = {0x15, 0x00};
1080         return sendCommand(wiiremote, cmd, 2);
1081 }
1082
1083