#include <stdio.h>
#include <bluetooth/bluetooth.h>
#include "cwiid.h"

cwiid_mesg_callback_t cwiid_callback;

void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
                    union cwiid_mesg mesg[], struct timespec *timestamp)
{
  for (int i=0; i < mesg_count; i++)
  {
    cwiid_mesg& msg = mesg[i];
    switch (msg.type) {
      case CWIID_MESG_ACC:
	printf("Acc Report: x=%d, y=%d, z=%d\n",
	       mesg[i].acc_mesg.acc[CWIID_X],
	       mesg[i].acc_mesg.acc[CWIID_Y],
	       mesg[i].acc_mesg.acc[CWIID_Z]);
	break;
      default:
	break;
    }
  }
}

class Wiimote {
private:
  cwiid_wiimote_t *wiimote;
  bdaddr_t bdaddr;
public:
  Wiimote() : wiimote( NULL ), bdaddr( *BDADDR_ANY ) {}
  ~Wiimote();
  void init();
  void dumpOrientation();
};

Wiimote::~Wiimote() {
  if ( wiimote != NULL ) {
    if (cwiid_close(wiimote)) {
      fprintf(stderr, "Error on wiimote disconnect\n");
      // throw exception
    }
    wiimote = NULL;
  }
}

void Wiimote::init() {
  /* Connect to the wiimote */
  printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
  if (!(wiimote = cwiid_open(&bdaddr, 0))) {
    fprintf(stderr, "Unable to connect to wiimote\n");
    // throw exception
    return;
  }
  if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
    fprintf(stderr, "Unable to set message callback\n");
    // throw exception
    return;
  }
  unsigned rpt_mode = CWIID_RPT_ACC;
  if (cwiid_set_rpt_mode(wiimote, rpt_mode)) {
    fprintf(stderr, "Error setting report mode\n");
    // throw exception
  }
}  
  
void Wiimote::dumpOrientation() {
}  

int main( int argc, char* argv[] ) {
  Wiimote moteA;
  moteA.init();
  while (1) {
    moteA.dumpOrientation();
  }
}