/** @file	wiimote_head_tracker.C
	@brief	Simple server application to provide tracking from a Wii Remote

	@date	2009-2010

	@author
	Based on client_and_server.C as included in the VRPN distribution.

	@author
	Ryan Pavlik
	<rpavlik@iastate.edu> and <abiryan@ryand.net>
	http://academic.cleardefinition.com/
	Iowa State University Virtual Reality Applications Center
	Human-Computer Interaction Graduate Program

	See ASME paper WINVR2010-3771 for more details on this system.
*/
/*
	Copyright Iowa State University 2009-2010
	Distributed under the Boost Software License, Version 1.0.
	(See accompanying comment below or copy at
	http://www.boost.org/LICENSE_1_0.txt)

	Boost Software License - Version 1.0 - August 17th, 2003

	Permission is hereby granted, free of charge, to any person or organization
	obtaining a copy of the software and accompanying documentation covered by
	this license (the "Software") to use, reproduce, display, distribute,
	execute, and transmit the Software, and to prepare derivative works of the
	Software, and to permit third-parties to whom the Software is furnished to
	do so, all subject to the following:

	The copyright notices in the Software and this entire statement, including
	the above license grant, this restriction and the following disclaimer,
	must be included in all copies of the Software, in whole or in part, and
	all derivative works of the Software, unless such copies or derivative
	works are solely in the form of machine-executable object code generated by
	a source language processor.

	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
	SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
	FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
	ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
	DEALINGS IN THE SOFTWARE.
*/

#include <iostream>
#include <string>

using namespace std;

#include <vrpn_Configure.h>

#define FATAL_ERROR cerr << endl << "Press 'enter' to exit..." << endl;	cin.get(); return -1;

#if defined(VRPN_USE_WIIUSE)
#include "vrpn_Tracker.h"
#include "vrpn_WiiMote.h"
#include "vrpn_Tracker_WiimoteHead.h"

#undef RP_PROFILING

const char* TRACKER_NAME = "Tracker0";
const char* WIIMOTE_NAME = "WiimoteForHead";
const char* WIIMOTE_REMOTE_NAME = "*WiimoteForHead";
const int	CONNECTION_PORT = vrpn_DEFAULT_LISTEN_PORT_NO;  // Port for connection to listen on
const int	TRACKER_FREQUENCY = 60;
const int	DEBUG_DISPLAY_INTERVAL = 3; // # of seconds between status displays

#ifdef RP_PROFILING
int reports = 0;
const int	MAX_REPORTS = 3000;
#endif

vrpn_Tracker_WiimoteHead* wmtkr;
vrpn_WiiMote* wiimote;
vrpn_Tracker_Remote* tkr;
vrpn_Connection* connection;
long sender_id;
long need_wiimote = false;
long wiimote_timeout = 10;

struct timeval last_display;

static	double	duration(struct timeval t1, struct timeval t2) {
	return (t1.tv_usec - t2.tv_usec) / 1000000.0 +
	       (t1.tv_sec - t2.tv_sec);
}

/*****************************************************************************
 *
   Callback handler
 *
 *****************************************************************************/

void	VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t) {
	static int count = 0;
#ifdef RP_PROFILING
	reports++;
#endif
	count++;
	if (count > 300) {
		printf("\n(%5f, %5f, %5f) ((%5f, %5f, %5f) %5f)\n",
			   t.pos[0], t.pos[1], t.pos[2], t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
		struct timeval now;
		vrpn_gettimeofday(&now, NULL);
		double interval = duration(now, last_display);
		double frequency = count / interval;
		count = 0;
		printf("Update frequency:\t%5.2f Hz (%5.2f sec)\n",
			frequency,
			interval);
		last_display = now;
	}
}

int main(int argc, char* argv []) {
	int wmnum = -1;
	if (argc == 1) {
		cout << "Note: Using Wiimote 0 by default... to specify another [0-3]:" << endl;
		cout <<  "Usage: " << argv[0] << " [WIIMOTENUM]" << endl;
		wmnum = 0;
	} else if (argc == 2) {
		wmnum = atoi(argv[1]);
		if (wmnum < 0 || wmnum > 3) {
			// improper # provided - pretend as if we had too many arguments
			// so we error out.
			argc = 3;
		} else {
			cout << "Using specified Wiimote number " << wmnum << endl;
		}
	}

	if (argc > 2) {
		cerr <<  "ERROR: Please run this with no arguments or optionally a Wiimote number (0-3)" << endl;
		cerr <<  "Usage: " << argv[0] << " [WIIMOTENUM]" << endl;
		FATAL_ERROR;
	}

	// explicitly open the connection
	connection = vrpn_create_server_connection(CONNECTION_PORT);

	if (!connection) {
		cerr << "Could not create VRPN server connection!" << endl;
		FATAL_ERROR;
	}

	// Message not valid on Windows, where you must remove existing pairings
	// and create a new one without a passcode before each time you start
	// the tracker
#ifndef _WIN32
	cout << endl;
	cout << "************************************************" << endl;
	cout << "Press the 1 and 2 buttons on your Wiimote now..." << endl;
	cout << "************************************************" << endl << endl;
#endif

	wiimote = new vrpn_WiiMote(WIIMOTE_NAME, connection, wmnum);
	if (!wiimote) {
		cerr << "Could not create Wiimote server named " << WIIMOTE_NAME << endl;
		FATAL_ERROR;
	}

	cout << endl;
	cout << "************************************************" << endl;
	cout << "Wiimote successfully paired, starting tracker..." << endl;
	cout << "************************************************" << endl << endl;

	wmtkr = new vrpn_Tracker_WiimoteHead(TRACKER_NAME, connection, WIIMOTE_REMOTE_NAME, 60.0);
	if (!wmtkr) {
		cerr <<  "Could not create Wiimote Head Tracker named " << TRACKER_NAME << endl;
		FATAL_ERROR;
	}

	cout << endl;
	cout << "************************************************" << endl;
	cout << "Tracker's name is: " << TRACKER_NAME << endl;
	cout << "************************************************" << endl << endl;

	vrpn_gettimeofday(&last_display, NULL);

	// Open the tracker remote using this connection
	tkr = new vrpn_Tracker_Remote(TRACKER_NAME, connection);
	if (!wmtkr) {
		cerr <<  "Could not create a remote for our own tracker " << TRACKER_NAME << endl;
		FATAL_ERROR;
	}

	// Set up the tracker callback handler
	tkr->register_change_handler(NULL, handle_pos);

	/*
	 * main interactive loop
	 */
	while (1) {
		// Let the tracker server, client and connection do their things
		wiimote->mainloop();
		wmtkr->mainloop();
		tkr->mainloop();
		connection->mainloop();
		// Sleep for 1ms so we don't eat the CPU
		vrpn_SleepMsecs(1);
#ifdef RP_PROFILING
		// gprof on Linux requires a clean exit
		if (reports >= MAX_REPORTS) {
			cout << "RP_PROFILING defined at build time and MAX_REPORTS=";
			cout << MAX_REPORTS << " reached:" << endl;
			cout << "Exiting cleanly." << endl;
			return 0;
		}
#endif
	}

	return 0;
}   /* main */

#else

// if not defined(VRPN_USE_WIIUSE)
int main(int argc, char* argv []) {
	cerr << "Error: This app doesn't do anything if you didn't compile the" << endl;
	cerr << "VRPN server lib against WiiUse." << endl;
	FATAL_ERROR;
}

#endif // defined(VRPN_USE_WIIUSE)
