#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>

#include "wiimote.h"
#include "wiimote_api.h"

int wiimode = 0;

int main(int argc, char *argv[]) {
	wiimote_t wiimote = WIIMOTE_INIT;

	if (argc < 2) {
		fprintf(stderr, "Usage: wiig BDADDR\n");
		exit(1);
	}

	fprintf(stderr, "Please press 1 and 2 on the wiimote to connect...\n");

	if (wiimote_connect(&wiimote, argv[1]) < 0) {
		fprintf(stderr, "Could not connect to wiimote: %s\n", wiimote_get_error());
		exit(1);
	}

	/* Turn on the accelerometers */
	wiimote.mode.acc = 1;

	float accmag;
	struct timeval t;
	while(wiimote_is_open(&wiimote)) {
		if (wiimote_update(&wiimote) < 0) {
			wiimote_disconnect(&wiimote);
			break;
		}

		gettimeofday(&t,NULL);

		accmag = sqrt(wiimote.force.x*wiimote.force.x+wiimote.force.y*wiimote.force.y+wiimote.force.z*wiimote.force.z);

		/* Switch LED monitoring modes */
		if (wiimote.keys.one)
			wiimode = 0;
		if (wiimote.keys.two)
			wiimode = 1;

		wiimote.led.bits = 0;
		switch(wiimode) {
		case 0:	/* L-R meter */
			if (wiimote.force.x < -1.0)
				wiimote.led.one = 1;
			if (wiimote.force.x < -0.5)
				wiimote.led.two = 1;
			if (wiimote.force.x > 0.5)
				wiimote.led.three = 1;
			if (wiimote.force.x > 1.0)
				wiimote.led.four = 1;
			break;
		case 1:	/* absolute force meter */
			if (accmag > 0.5)
				wiimote.led.one = 1;
			if (accmag > 1.0)
				wiimote.led.two = 1;
			if (accmag > 1.5)
				wiimote.led.three = 1;
			if (accmag > 2.0)
				wiimote.led.four = 1;
			break;
		}

		printf("%f\t%f\t%f\t%f\n", (double)t.tv_sec + (double)t.tv_usec / 1e6,
				wiimote.force.x, wiimote.force.y,
				wiimote.force.z);
	}
}