/////////////////////////////////////////////////////////////////////////////
// File:        mod_wiimotes.cpp
// Author:      Cesar Mauri Loba (cesar at crea-si dot com)
// Copyright:   (C) 2011 Cesar Mauri Loba - CREA Software Systems
//
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
/////////////////////////////////////////////////////////////////////////////
// TODO: fix linux working issue, may require permanent pairing
#include "spcore/component.h"
#include "spcore/module.h"
#include "spcore/libimpexp.h"
#include "spcore/basictypes.h"
#include "mod_wiimotes.h"
#include "wwiimotesconfiguration.h"

#include <string>
#include <boost/thread/thread.hpp>
#include <boost/thread/xtime.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <stdio.h>

using namespace spcore;

namespace mod_wiimotes {

static void sleep_miliseconds( unsigned int ms )
{
	boost::xtime xt;
	boost::xtime_get(&xt, boost::TIME_UTC);
	xt.nsec+= ms * 1000000;
	boost::thread::sleep(xt);
}

/*
	wii_use main thread
*/

// wii capture thread
class WiiuseThread
{
public:
	/**
		TODO: filter desired wiimote according to desired type & number
		Device filter flags
	*/
	/*
	enum FilterWiimotes {
		FILTER_WIIMOTE			= 0x100,	// Take only into account Wiimote devices
		FILTER_BALANCE_BOARD	= 0x200		// Take only into account Balance Board devices
	};*/

	enum DesiredFeatures { NONE= 0x0, ACC= 0x1, MOTION_PLUS= 0x2, NUNCHUCK= 0x4 };

	WiiuseThread ()
	: m_wiimotes(NULL)
	, m_Life(true)
	, m_hasListeners(false)
	, m_enabledFeaturesChanged(false)
	, m_statusRequested(false)
	, m_state(DO_CONNECT)
	{
		m_status= CTypeWiimotesStatus::CreateInstance();
		assert (m_status.get());
	}

	~WiiuseThread () {
		Stop();
	}

	void Stop() {
		if (m_Life) {
			m_Life= false;
		}
	}

	/**
		Register a listener

	*/
	//void RegisterListener (WiimoteListener& wl, CTypeWiimotesStatus::EnableFeatures flags, unsigned int filter) {
	void RegisterListener (WiimoteListener& wl, unsigned int desiredFea, unsigned int filter) {
		boost::mutex::scoped_lock mutex(m_mutex);

		assert (filter< CTypeWiimotesStatus::MAXWIIMOTES);
		assert (!(desiredFea & 0xFFF8));

		// Check if already registered
		std::vector<struct ListenerConfiguration>::iterator it= m_listeners.begin();
		for (; it != m_listeners.end() && it->callBack!= &wl; ++it);

		if (it!= m_listeners.end()) {
			// Already registered. Update desired features and filter
			it->df= desiredFea;
			it->filter= filter;
		}
		else {
			// Not registered. Add
			struct ListenerConfiguration lc;
			lc.callBack= &wl;
			lc.df= desiredFea;
			lc.filter= filter;
			m_listeners.push_back (lc);
		}

		m_hasListeners= (m_listeners.size()> 0);
		m_enabledFeaturesChanged= true;
	}

	/**
		Unregister a listener

	*/
	void UnregisterListener (WiimoteListener& wl) {
		boost::mutex::scoped_lock mutex(m_mutex);

		// Check if already registered
		std::vector<struct ListenerConfiguration>::iterator it= m_listeners.begin();
		for (; it != m_listeners.end() && it->callBack!= &wl; ++it);

		if (it== m_listeners.end()) return; // Not found

		m_listeners.erase (it);

		m_hasListeners= (m_listeners.size()> 0);
		m_enabledFeaturesChanged= true;
	}

	// Thread entry point
	void Entry() {
		// Start thread main loop
		while (m_Life) {
			switch (m_state) {
			case DO_CONNECT: DoConnectState(); break;
			case CONNECTED: ConnectedState(); break;
			case DO_RECONNECT:
				Cleanup();
				m_state= DO_CONNECT;
				break;
			case IDLE:
				if (!m_hasListeners) m_state= DO_CONNECT;
				else sleep_miliseconds(500);
				break;
			default: assert (false);
			}

			if (m_statusRequested) NotifyStatus (*m_status);
		}

		Cleanup();
	}

	// Issues a reconnection request. Disconnects all wiimotes and tries to reconnect them
	void Reconnect()
	{
		m_state= DO_RECONNECT;
	}

	// Request wiimotes status
	void ReqStatus()
	{
		boost::mutex::scoped_lock mutex(m_mutex);

		m_statusRequested= true;
	}

private:
	enum WiimoteThreadState { DO_CONNECT= 0, CONNECTED, DO_RECONNECT, IDLE };

	void NotifyStatus (CTypeWiimotesStatus& s) {
		boost::mutex::scoped_lock mutex(m_mutex);

		std::vector<struct ListenerConfiguration>::iterator it= m_listeners.begin();
		for(; it!= m_listeners.end(); ++it) it->callBack->StatusNotification(s);

		m_statusRequested= false;
	}

	void Cleanup() {
		wiiuse_cleanup(m_wiimotes, CTypeWiimotesStatus::MAXWIIMOTES);
		m_wiimotes= NULL;
		m_status->Reset();
	}

	void DoConnectState() {
		ICoreRuntime& core= *getSpCoreRuntime();

		// Are there any listener?
		if (!m_hasListeners) {
			// TODO: avoid polling
			sleep_miliseconds( 500 );
			return;
		}

		//
		// New listeners found.
		//

		//	Initialize an array of wiimote objects.
		assert (!m_wiimotes);
		m_wiimotes =  wiiuse_init(CTypeWiimotesStatus::MAXWIIMOTES);
		if (!m_wiimotes) {
			core.LogMessage(ICoreRuntime::LOG_ERROR, "wiiuse_init failed!", "mod_wiimotes");
			assert (false);
			sleep_miliseconds( 500 );
			return;
		}

		// Change status to say connecting and notify listeners
		assert (m_status->GetGeneralStatus()== CTypeWiimotesStatus::IDLE);
		m_status->SetGeneralStatus (CTypeWiimotesStatus::CONNECTING);
		NotifyStatus (*m_status);

		// Find wiimote devices. Set a timeout of 5 seconds.
		// This will return the number of actual wiimotes that are in discovery mode.
		int found = wiiuse_find(m_wiimotes, CTypeWiimotesStatus::MAXWIIMOTES, 5);
		if (!found) {
			core.LogMessage(ICoreRuntime::LOG_WARNING, "No wiimotes found.", "mod_wiimotes");
			Cleanup();
			m_state= IDLE;
			NotifyStatus (*m_status);
			sleep_miliseconds( 500 );
			return;
		}

		// Now that we found some wiimotes, connect to them.
		// Give the function the wiimote array and the number
		// of wiimote devices we found.
		// This will return the number of established connections to the found wiimotes.
		int connected= wiiuse_connect(m_wiimotes, CTypeWiimotesStatus::MAXWIIMOTES);
		assert (connected<= CTypeWiimotesStatus::MAXWIIMOTES);
		if (!connected) {
			core.LogMessage(ICoreRuntime::LOG_INFO, "Failed to connect to any wiimote.", "mod_wiimotes");
			Cleanup();
			m_state= IDLE;
			NotifyStatus (*m_status);
			sleep_miliseconds( 500 );
			return;
		}

		// Connected. Update status & notify listeners
		m_status->SetGeneralStatus(CTypeWiimotesStatus::CONNECTED);
		m_status->SetConnectedCount(connected);
		for (int i= 0; i< connected; ++i) {
			// Simply set as WIIMOTE_CONNECTED. Extensions are discovered later.
			m_status->SetIsConnected (i, true);
		}
		NotifyStatus (*m_status);

		// Add log
		{
			char buff[100];
			snprintf(buff, 100, "Connected to %i wiimotes (of %i found)", connected, found);
			core.LogMessage(ICoreRuntime::LOG_INFO, buff, "mod_wiimotes");
		}

		// Now set the LEDs
		wiiuse_set_leds(m_wiimotes[0], WIIMOTE_LED_1);
		wiiuse_set_leds(m_wiimotes[1], WIIMOTE_LED_2);
		wiiuse_set_leds(m_wiimotes[2], WIIMOTE_LED_3);
		wiiuse_set_leds(m_wiimotes[3], WIIMOTE_LED_4);

		// And rumble briefly
		for (int i= 0; i< connected; ++i) wiiuse_rumble(m_wiimotes[i], 1);
		sleep_miliseconds( 200 );
		for (int i= 0; i< connected; ++i) wiiuse_rumble(m_wiimotes[i], 0);

		// Finally change thread state
		m_state= CONNECTED;
	}

	// This method should be called every time the enabled features (i.e.
	// accelerometers or motion plus) are changed. Returns if status changed.
	bool UpdateEnabledFeatures () {
		bool changed= false;

		boost::mutex::scoped_lock mutex(m_mutex);

		for (unsigned int i = 0; i < m_status->GetMaxCount(); ++i) {
			if (!m_status->IsConnected(i)) continue;

			// Compute desired features flags
			unsigned int desiredFeatures= 0;
			std::vector<struct ListenerConfiguration>::iterator it= m_listeners.begin();
			for (; it != m_listeners.end(); ++it)
				if (it->filter== i) desiredFeatures|= it->df;

			//
			// Manage continouous mode. It should be enabled whenever accelerometers
			// or motion plus are requested or when nunchuck is requested, is not enabled
			// but is present.
			//
			// It should be disabled when
			//
			if ((desiredFeatures & NUNCHUCK && m_status->HasNunchuk(i) && !m_status->IsNunchuckEnabled(i)) ||
				desiredFeatures & ACC || desiredFeatures & MOTION_PLUS)
				wiiuse_set_flags(m_wiimotes[i], WIIUSE_CONTINUOUS, 0);
			else
				wiiuse_set_flags(m_wiimotes[i], 0, WIIUSE_CONTINUOUS);

			//
			// Nunchuck does not need to be explicitly enabled, just update status
			//
			if (desiredFeatures & NUNCHUCK && m_status->HasNunchuk(i) && !m_status->IsNunchuckEnabled(i))
				m_status->SetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_NUNCHUCK);
			else if (!(desiredFeatures & NUNCHUCK) || !m_status->HasNunchuk(i))
				m_status->UnsetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_NUNCHUCK);

			//
			// Check whether accelerometers need to be enabled or disabled
			//
			if (m_status->IsAccelerometersEnabled(i)) {
				// Acc enabled. Check if need to be disabled
				if (!(desiredFeatures & ACC)) {
					// Disable accelerometers
					wiiuse_motion_sensing(m_wiimotes[i], 0);
					m_status->UnsetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_ACC);
					changed= true;
				}
			}
			else {
				if (desiredFeatures & ACC) {
					// Enable accelerometers
					wiiuse_motion_sensing(m_wiimotes[i], 1);
					m_status->SetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_ACC);
					changed= true;
				}
			}

			//
			// Check whether motion plus needs to be enabled or disabled
			//
			if (m_status->IsMotionPlusEnabled(i)) {
				// Acc enabled. Check if need to be disabled
				if (!(desiredFeatures & MOTION_PLUS)) {
					// Disable motion plus
					wiiuse_set_motion_plus(m_wiimotes[i], 0);
					m_status->UnsetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_MP);
					changed= true;
				}
			}
			else {
				if (desiredFeatures & MOTION_PLUS) {
					// Enable motion plus
					wiiuse_set_motion_plus(m_wiimotes[i], 1);
					m_status->SetEnabledFeature (i, CTypeWiimotesStatus::ENABLED_MP);
					changed= true;
				}
			}
		}
		m_enabledFeaturesChanged= false;

		return changed;
	}

	void HandleGenericEvent (unsigned int wiimote_n)
	{
		// Notify all listeners interested on this wiimote
		boost::mutex::scoped_lock mutex(m_mutex);

		std::vector<struct ListenerConfiguration>::iterator it= m_listeners.begin();
		for(; it!= m_listeners.end(); ++it) {
			if (it->filter== wiimote_n)
				it->callBack->WiimoteNotification (m_wiimotes[wiimote_n]);
		}
	}

	void ConnectedState()
	{
		if (!m_hasListeners) {
			// No more listeners available. Finish
			Cleanup();
			m_state= IDLE;
			return;
		}

		assert (m_status->GetConnectedCount());

		// Poll events
		if (wiiuse_poll(m_wiimotes, CTypeWiimotesStatus::MAXWIIMOTES)) {
			for (unsigned int i = 0; i < CTypeWiimotesStatus::MAXWIIMOTES; ++i) {
				switch (m_wiimotes[i]->event) {
					case WIIUSE_NONE:
						break;

					case WIIUSE_EVENT:
						// a generic event occured
						HandleGenericEvent (i);
						break;

					case WIIUSE_STATUS:
					case WIIUSE_CONNECT:
						// Once connected enable accelerometers or motion
						// plus when needed
						if (UpdateEnabledFeatures()) NotifyStatus (*m_status);
						break;

					case WIIUSE_DISCONNECT:
					case WIIUSE_UNEXPECTED_DISCONNECT:
					{
						// the wiimote disconnected
						int count= m_status->GetConnectedCount() - 1;
						assert (count>= 0);
						if (count== 0) {
							Cleanup();
							m_state= IDLE;
							NotifyStatus (*m_status);
							getSpCoreRuntime()->LogMessage(ICoreRuntime::LOG_WARNING, "All wiimotes disconnected", "mod_wiimotes");
							return;
						}
						else {
							m_status->SetConnectedCount(count);
							m_status->SetIsConnected(i, false);
							NotifyStatus (*m_status);
							getSpCoreRuntime()->LogMessage(ICoreRuntime::LOG_WARNING, "Wiimote disconnected", "mod_wiimotes");
						}
						break;
					}
					case WIIUSE_READ_DATA:
					case WIIUSE_WRITE_DATA:
						break;

					case WIIUSE_NUNCHUK_INSERTED:
						//wiiuse_set_nunchuk_orient_threshold((struct nunchuk_t*)&wiimotes[i]->exp.nunchuk, 90.0f);
						//wiiuse_set_nunchuk_accel_threshold((struct nunchuk_t*)&m_wiimotes[i]->exp.nunchuk, 0);
						//wiiuse_set_nunchuk_accel_threshold(m_wiimotes[i], 0);

						m_status->SetExtension (i, CTypeWiimotesStatus::NUNCHUK);
						UpdateEnabledFeatures();
						NotifyStatus (*m_status);
						break;

					case WIIUSE_CLASSIC_CTRL_INSERTED:
						m_status->SetExtension (i, CTypeWiimotesStatus::CLASSIC);
						NotifyStatus (*m_status);
						break;

					case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
						m_status->SetExtension (i, CTypeWiimotesStatus::GUITAR_HERO);
						NotifyStatus (*m_status);
						break;

					case WIIUSE_BALANCE_BOARD_CTRL_INSERTED:
						m_status->SetExtension (i, CTypeWiimotesStatus::BALANCE_BOARD);
						NotifyStatus (*m_status);
						break;

					case WIIUSE_MOTION_PLUS_ACTIVATED:
						// TODO: detect motion plus presence.
						// FIXME: fix wiiuse library to manage properly nunchuck
						// insertion/extraction when motion plus is enabled
						m_status->SetExtension (i, CTypeWiimotesStatus::MOTION_PLUS);
						NotifyStatus (*m_status);
						break;

					case WIIUSE_NUNCHUK_REMOVED:
					case WIIUSE_CLASSIC_CTRL_REMOVED:
					case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
					case WIIUSE_MOTION_PLUS_REMOVED:
					case WIIUSE_BALANCE_BOARD_CTRL_REMOVED:
						// some expansion was removed
						switch (m_wiimotes[i]->exp.type) {
						case EXP_NONE:
							m_status->SetExtension (i, CTypeWiimotesStatus::NONE);
							break;
						case EXP_NUNCHUK:
							m_status->SetExtension (i, CTypeWiimotesStatus::NUNCHUK);
							break;
						case EXP_CLASSIC:
							m_status->SetExtension (i, CTypeWiimotesStatus::CLASSIC);
							break;
						case EXP_GUITAR_HERO_3:
							m_status->SetExtension (i, CTypeWiimotesStatus::GUITAR_HERO);
							break;
						case EXP_WII_BOARD:
							m_status->SetExtension (i, CTypeWiimotesStatus::BALANCE_BOARD);
							break;
						case EXP_MOTION_PLUS:
							m_status->SetExtension (i, CTypeWiimotesStatus::MOTION_PLUS);
							break;
						default:
							assert (false);
						}
						UpdateEnabledFeatures();
						NotifyStatus (*m_status);
						break;

					default:
						assert (false);
						break;
				}
			}
		}

		if (m_enabledFeaturesChanged)
			if (UpdateEnabledFeatures()) NotifyStatus (*m_status);
	}

private:

	wiimote** m_wiimotes;
	bool volatile m_Life;
	bool volatile m_hasListeners;
	bool volatile m_enabledFeaturesChanged;
	bool volatile m_statusRequested;
	WiimoteThreadState volatile m_state;
	SmartPtr<CTypeWiimotesStatus> m_status;
	boost::mutex m_mutex;
	struct ListenerConfiguration {
		WiimoteListener* callBack;					// object that is called
		unsigned int filter;						// filter to select wiimote, current the wiimote number
		unsigned int df;							// features that the listener asked for
	};
	std::vector<struct ListenerConfiguration> m_listeners;
};


/*
	Thread controller

	This is a singleton class which starts/stops the wiimotes thread and forwards requests
*/
WiiuseThreadController::WiiuseThreadController()
{
	m_worker= new WiiuseThread();
	m_thread= new boost::thread(&WiiuseThread::Entry, m_worker);
	assert (m_thread->joinable());
}

WiiuseThreadController::~WiiuseThreadController()
{
	m_worker->Stop();
	m_thread->join();
	delete m_thread;
	delete m_worker;
}

WiiuseThreadController * WiiuseThreadController::getInstance() {
	if (!g_instance) {
		g_instance= new WiiuseThreadController();
	}
	return g_instance;
}

void WiiuseThreadController::destroyInstance() {
	delete g_instance;
	g_instance= NULL;
}

void WiiuseThreadController::RegisterListener (WiimoteListener& wl, unsigned int df, unsigned int filter) {
	m_worker->RegisterListener(wl, df, filter);
}

void WiiuseThreadController::UnregisterListener (WiimoteListener& wl) {
	m_worker->UnregisterListener(wl);
}

void WiiuseThreadController::Reconnect() {
	m_worker->Reconnect ();
}

void WiiuseThreadController::ReqStatus() {
	m_worker->ReqStatus();
}

WiiuseThreadController * WiiuseThreadController::g_instance= NULL;


/*
	wii_config component

	This manages the wiimotes configuration
*/

class WiimotesConfig : public spcore::CComponentAdapter, public WiimoteListener {
public:
	WiimotesConfig(const char * name, int argc, const char * argv[])
	: CComponentAdapter(name, argc, argv)
	{
		RegisterInputPin (*SmartPtr<InputPinReconnect>(new InputPinReconnect(*this), false));
		RegisterInputPin (*SmartPtr<InputPinReqStatus>(new InputPinReqStatus(*this), false));

		m_oPinStatus= CTypeWiimotesStatus::CreateOutputPin("status");
		if (m_oPinStatus.get()== NULL)
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_config. output pin creation failed.");
		RegisterOutputPin (*m_oPinStatus);

		m_status= CTypeWiimotesStatus::CreateInstance();
		if (m_status.get()== NULL)
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_config. instance creation failed.");
	}

	static const char* getTypeName() { return "wiimotes_config"; };
	virtual const char* GetTypeName() const { return WiimotesConfig::getTypeName(); };

	virtual bool ProvidesExecThread() const { return true; }

	virtual int DoInitialize() {
		if (m_oPinStatus->GetNumComsumers()> 0) {
			WiiuseThreadController::getInstance()->RegisterListener(*this, WiiuseThread::NONE, 0);
		}
		return 0;
	}

	virtual void DoFinish() {
		WiiuseThreadController::getInstance()->UnregisterListener(*this);
	}

	virtual void StatusNotification (const CTypeWiimotesStatus & msg) {
		msg.Clone (m_status.get(), true);
		m_oPinStatus->Send(m_status);
	}
	virtual void WiimoteNotification (wiimote *) {
		// Do nothing
	}

private:
	virtual ~WiimotesConfig()
	{
	}

	// Write-only pin which issues a reconnect event
	class InputPinReconnect : public spcore::CInputPinWriteOnly<CTypeAny, WiimotesConfig> {
	public:
		InputPinReconnect (WiimotesConfig & component) : CInputPinWriteOnly<CTypeAny, WiimotesConfig>("reconnect", component) {}
		virtual int DoSend(const CTypeAny &) {
			WiiuseThreadController::getInstance()->Reconnect();
			return 0;
		}
	};

	// Write-only pin which requests an status notification
	class InputPinReqStatus : public spcore::CInputPinWriteOnly<CTypeAny, WiimotesConfig> {
	public:
		InputPinReqStatus (WiimotesConfig & component) : CInputPinWriteOnly<CTypeAny, WiimotesConfig>("req_status", component) {}
		virtual int DoSend(const CTypeAny &) {
			WiiuseThreadController::getInstance()->ReqStatus();
			return 0;
		}
	};

private:
	SmartPtr<spcore::IOutputPin> m_oPinStatus;
	SmartPtr<CTypeWiimotesStatus> m_status;
};

// WiimotesConfig component factory
typedef ComponentFactory<WiimotesConfig> WiimotesConfigFactory;


/* *****************************************************************************
	wiimotes_input component

		Provides events from diffent Wii remote devices. Currently supports:
		classic Wii remote, nunchuck, motion plus and balance board

	Input pins:
	
	Ouput pins:
		accelerometers (wiimotes_accelerometers)
			Main wiimote accelerometers
			
		nunchuck_accelerometers (wiimotes_accelerometers)
			Nunchuck accelerometers

		buttons (wiimotes_buttons)
			Button events including wiimote, nunchuck and balance board

		balance_board (wiimotes_balance_board)
			Balance board data

		motion_plus (wiimotes_motion_plus)
			Motion plus speed data

	Command line:
		// TODO
		[- r <val> ] (= 1)	reduction (integer)
			Reduction. Wii remotes send events at approximately 95Hz which
			is too much for most applications. So this parameters allows to
			reduce the cadence of events to a half, a third, etc. Reduction
			does not have effect on button events.
		// TODO
***************************************************************************** */
class WiimotesInput : public spcore::CComponentAdapter, public WiimoteListener {
public:
	// TODO: process commandline parameters to:
	//	- reduce fps
	//  - choose which wiimote will be used (numer and/or type)
	WiimotesInput(const char * name, int argc, const char * argv[])
	: CComponentAdapter(name, argc, argv)
	{
		// wiimote accelerometers
		m_oPinWiimoteAcc= CTypeWiimotesAccelerometer::CreateOutputPin("accelerometers");
		if (m_oPinWiimoteAcc.get()== NULL || RegisterOutputPin (*m_oPinWiimoteAcc))
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_input. output pin accelerometers creation/registration failed.");
		m_wiimoteAcc= CTypeWiimotesAccelerometer::CreateInstance();
		if (m_wiimoteAcc.get()== NULL)
			throw std::runtime_error("wiimotes_input. accelerometers instance creation failed.");

		// nunchuck accelerometers
		m_oPinNunchuckAcc= CTypeWiimotesAccelerometer::CreateOutputPin("nunchuck_accelerometers");
		if (m_oPinNunchuckAcc.get()== NULL || RegisterOutputPin (*m_oPinNunchuckAcc))
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_input. output pin nunchuck creation/registration failed.");
		m_nunchuckAcc= CTypeWiimotesAccelerometer::CreateInstance();
		if (m_nunchuckAcc.get()== NULL)
			throw std::runtime_error("wiimotes_input. nunchuck instance creation failed.");

		// buttons
		m_oPinButtons= CTypeWiimotesButtons::CreateOutputPin("buttons");
		if (m_oPinButtons.get()== NULL || RegisterOutputPin (*m_oPinButtons))
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_input. output pin buttons creation/registration failed.");
		m_buttons= CTypeWiimotesButtons::CreateInstance();
		if (m_buttons.get()== NULL)
			throw std::runtime_error("wiimotes_input. buttons instance creation failed.");

		// balance board
		m_oPinBalanceBoard= CTypeWiimotesBalanceBoard::CreateOutputPin("balance_board");
		if (m_oPinBalanceBoard.get()== NULL || RegisterOutputPin (*m_oPinBalanceBoard))
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_input. output pin balance board creation/registration failed.");
		m_balanceBoard= CTypeWiimotesBalanceBoard::CreateInstance();
		if (m_balanceBoard.get()== NULL)
			throw std::runtime_error("wiimotes_input. balance board instance creation failed.");

		// motion plus
		m_oPinMotionPlus= CTypeWiimotesMotionPlus::CreateOutputPin("motion_plus");
		if (m_oPinMotionPlus.get()== NULL || RegisterOutputPin (*m_oPinMotionPlus))
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("wiimotes_input. output pin motion plus creation/registration failed.");
		m_motionPlus= CTypeWiimotesMotionPlus::CreateInstance();
		if (m_motionPlus.get()== NULL)
			throw std::runtime_error("wiimotes_input. motion plus instance creation failed.");
	}

	static const char* getTypeName() { return "wiimotes_input"; };
	virtual const char* GetTypeName() const { return WiimotesInput::getTypeName(); };

	virtual bool ProvidesExecThread() const { return true; }

	virtual int DoStart() {
		unsigned int flags= 0, consumers= 0;

		if (m_oPinWiimoteAcc->GetNumComsumers() > 0) {
			flags|= WiiuseThread::ACC;
			++consumers;
		}

		if (m_oPinNunchuckAcc->GetNumComsumers() > 0) {
			flags|= WiiuseThread::NUNCHUCK;
			++consumers;
		}

		if (m_oPinButtons->GetNumComsumers() > 0) ++consumers;

		if (m_oPinBalanceBoard->GetNumComsumers() > 0) ++consumers;

		if (m_oPinMotionPlus->GetNumComsumers() > 0) {
			flags|= WiiuseThread::MOTION_PLUS;
			++consumers;
		}

		if (consumers) {
			// TODO: currently only takes into account the first wiimote
			WiiuseThreadController::getInstance()->RegisterListener
				(*this, static_cast<WiiuseThread::DesiredFeatures>(flags), 0);
		}
		return 0;
	}

	virtual void DoStop() {
		WiiuseThreadController::getInstance()->UnregisterListener(*this);
	}

	virtual void StatusNotification (const CTypeWiimotesStatus &) {
		// Do nothing
	}
	virtual void WiimoteNotification (wiimote *wm) {
		//
		// Wiimote accelerometers
		//
		if (m_oPinWiimoteAcc->GetNumComsumers() && WIIUSE_USING_ACC(wm)) {
			CTypeWiimotesAccelerometer * result= m_wiimoteAcc.get();
			result->SetForceX (wm->gforce.x);
			result->SetForceY (wm->gforce.y);
			result->SetForceZ (wm->gforce.z);
			result->SetPitch (wm->orient.pitch);
			result->SetRoll (wm->orient.roll);

			m_oPinWiimoteAcc->Send (m_wiimoteAcc);
		}

		//
		// Nunchuck accelerometers
		//
		if (wm->exp.type == EXP_NUNCHUK && m_oPinNunchuckAcc->GetNumComsumers()) {
			CTypeWiimotesAccelerometer * result= m_nunchuckAcc.get();
			result->SetForceX (wm->exp.nunchuk.gforce.x);
			result->SetForceY (wm->exp.nunchuk.gforce.y);
			result->SetForceZ (wm->exp.nunchuk.gforce.z);
			result->SetPitch (wm->exp.nunchuk.orient.pitch);
			result->SetRoll (wm->exp.nunchuk.orient.roll);

			m_oPinNunchuckAcc->Send(m_nunchuckAcc);
		}

		//
		// Buttons
		//
		if (m_oPinButtons->GetNumComsumers()) {
			bool buttons_changed= false;

			if ((wm->btns & 0x1F9F) != m_buttons->GetWiimoteButtons()) {
				m_buttons->SetWiimoteButtons(wm->btns & 0x1F9F);
				buttons_changed= true;
			}

			// Check if nunchuck button state changed
			if (wm->exp.type == EXP_NUNCHUK) {
				if ((wm->exp.nunchuk.btns & NUNCHUK_BUTTON_ALL) != m_buttons->GetNunchuckButtons()) {
					m_buttons->SetNunchuckButtons(wm->exp.nunchuk.btns & NUNCHUK_BUTTON_ALL);
					buttons_changed= true;
				}
			}
			else if (m_buttons->GetNunchuckButtons()) {
				m_buttons->SetNunchuckButtons(0);
				buttons_changed= true;
			}

			// Notify buttons
			if (buttons_changed) m_oPinButtons->Send(m_buttons);
		}

		//
		// Balance board
		//
		if (wm->exp.type == EXP_BALANCE_BOARD && m_oPinBalanceBoard->GetNumComsumers()) {
			CTypeWiimotesBalanceBoard * result= m_balanceBoard.get();

			result->SetTopLeft (wm->exp.bb.tl);
			result->SetTopRight (wm->exp.bb.tr);
			result->SetBottomRight (wm->exp.bb.br);
			result->SetBottomLeft (wm->exp.bb.bl);

			m_oPinBalanceBoard->Send (m_balanceBoard);
		}

		//
		// Motion plus
		//
		if (wm->exp.type == EXP_MOTION_PLUS && m_oPinMotionPlus->GetNumComsumers()) {
			CTypeWiimotesMotionPlus * result= m_motionPlus.get();

			result->SetXSpeed (wm->exp.mp.sx);
			result->SetYSpeed (wm->exp.mp.sy);
			result->SetZSpeed (wm->exp.mp.sz);

			m_oPinMotionPlus->Send (m_motionPlus);
		}
	}

private:
	virtual ~WiimotesInput() {}

private:
	unsigned int m_reduction;
	unsigned int m_storedCount;

	SmartPtr<IOutputPin> m_oPinWiimoteAcc;
	SmartPtr<CTypeWiimotesAccelerometer> m_wiimoteAcc;

	SmartPtr<IOutputPin> m_oPinNunchuckAcc;
	SmartPtr<CTypeWiimotesAccelerometer> m_nunchuckAcc;

	SmartPtr<IOutputPin> m_oPinButtons;
	SmartPtr<CTypeWiimotesButtons> m_buttons;

	SmartPtr<IOutputPin> m_oPinBalanceBoard;
	SmartPtr<CTypeWiimotesBalanceBoard> m_balanceBoard;

	SmartPtr<IOutputPin> m_oPinMotionPlus;
	SmartPtr<CTypeWiimotesMotionPlus> m_motionPlus;
};

// WiimotesConfig component factory
typedef ComponentFactory<WiimotesInput> WiimotesInputFactory;


/* *****************************************************************************
	wii_mp_to_composite
	
		Converts an object wiimotes_motion_plus into a composite of 
		three float which contains the speed values for x, y and z

	Input pins:
		in (wiimotes_motion_plus)

	Ouput pins:
		out (any) containing three float with the information of each axis
***************************************************************************** */
class WiiMpToCompo : public CComponentAdapter {
public:
	static const char* getTypeName() { return "wii_mp_to_composite"; }
	virtual const char* GetTypeName() const { return WiiMpToCompo::getTypeName(); }
    WiiMpToCompo(const char * name, int argc, const char * argv[]) 
	: CComponentAdapter(name, argc, argv) {
		// Input pins
  		if (RegisterInputPin(*SmartPtr<IInputPin>(new InputPinMotionPlus("in", *this), false))!= 0)
			throw std::runtime_error("error creating input pin");
		// Output pin
		m_oPin= CTypeComposite::CreateOutputPin("out");
		if (RegisterOutputPin(*m_oPin)!= 0)	throw std::runtime_error("error registering output pin");
		// Result instance
		m_result= CTypeComposite::CreateInstance();
		m_x= CTypeFloat::CreateInstance();
		m_y= CTypeFloat::CreateInstance();
		m_z= CTypeFloat::CreateInstance();
		m_result->AddChild(m_x);
		m_result->AddChild(m_y);
		m_result->AddChild(m_z);
	}

	int OnValue (const CTypeWiimotesMotionPlus & message) {
		m_x->setValue(message.GetXSpeed());
		m_y->setValue(message.GetYSpeed());
		m_z->setValue(message.GetZSpeed());
		return m_oPin->Send(m_result);
	}

private:
	virtual ~WiiMpToCompo() {}

	class InputPinMotionPlus : public CInputPinWriteOnly<CTypeWiimotesMotionPlus, WiiMpToCompo> {
	public:
		InputPinMotionPlus (const char * name, WiiMpToCompo & component)
		: CInputPinWriteOnly<CTypeWiimotesMotionPlus, WiiMpToCompo>(name, component) {}

		virtual int DoSend(const CTypeWiimotesMotionPlus & message) {
			return this->m_component->OnValue (message);			
		}
	};
	
	SmartPtr<IOutputPin> m_oPin;
	SmartPtr<CTypeComposite> m_result;
	SmartPtr<CTypeFloat> m_x, m_y, m_z;

};

typedef ComponentFactory<WiiMpToCompo> WiiMpToCompoFactory;

/* *****************************************************************************
	wii_bb_to_composite
	
		Converts an object wiimotes_balance_board into a composite of 
		two float which contains the centre of masses

	Input pins:
		in (wiimotes_balance_board)

	Ouput pins:
		out (any) containing two float with the information of each axis
***************************************************************************** */
class WiiBbToCompo : public CComponentAdapter {
public:
	static const char* getTypeName() { return "wii_bb_to_composite"; }
	virtual const char* GetTypeName() const { return WiiBbToCompo::getTypeName(); }
    WiiBbToCompo(const char * name, int argc, const char * argv[]) 
	: CComponentAdapter(name, argc, argv) {
		// Input pins
  		if (RegisterInputPin(*SmartPtr<IInputPin>(new InputPinMotionPlus("in", *this), false))!= 0)
			throw std::runtime_error("error creating input pin");
		// Output pin
		m_oPin= CTypeComposite::CreateOutputPin("out");
		if (RegisterOutputPin(*m_oPin)!= 0)	throw std::runtime_error("error registering output pin");
		// Result instance
		m_result= CTypeComposite::CreateInstance();
		m_x= CTypeFloat::CreateInstance();
		m_y= CTypeFloat::CreateInstance();
		
		m_result->AddChild(m_x);
		m_result->AddChild(m_y);
	}

	int OnValue (const CTypeWiimotesBalanceBoard & message) {
		m_x->setValue(message.GetCenterOfMassX());
		m_y->setValue(message.GetCenterOfMassY());

		return m_oPin->Send(m_result);
	}

private:
	virtual ~WiiBbToCompo() {}

	class InputPinMotionPlus : public CInputPinWriteOnly<CTypeWiimotesBalanceBoard, WiiBbToCompo> {
	public:
		InputPinMotionPlus (const char * name, WiiBbToCompo & component)
		: CInputPinWriteOnly<CTypeWiimotesBalanceBoard, WiiBbToCompo>(name, component) {}

		virtual int DoSend(const CTypeWiimotesBalanceBoard & message) {
			return this->m_component->OnValue (message);			
		}
	};
	
	SmartPtr<IOutputPin> m_oPin;
	SmartPtr<CTypeComposite> m_result;
	SmartPtr<CTypeFloat> m_x, m_y;
};

typedef ComponentFactory<WiiBbToCompo> WiiBbToCompoFactory;


/* *****************************************************************************
	wii_acc_estimate
	
		
	Input pins:
		in (wiimotes_accelerometers)

	Ouput pins:
		out (any) containing three float with the information of each axis
***************************************************************************** */
class WiiAccEstimate : public CComponentAdapter {
public:
	static const char* getTypeName() { return "wii_acc_estimate"; }
	virtual const char* GetTypeName() const { return WiiAccEstimate::getTypeName(); }
    WiiAccEstimate(const char * name, int argc, const char * argv[]) 
	: CComponentAdapter(name, argc, argv) 
	, m_lastX(0)
	, m_lastY(0)
	, m_lastZ(0)
	, m_error(0.08f) {
		// Input pins
  		if (RegisterInputPin(*SmartPtr<IInputPin>(new InputPinAcc("in", *this), false))!= 0)
			throw std::runtime_error("error creating input pin");
		// Output pin
		m_oPin= CTypeComposite::CreateOutputPin("out");
		if (RegisterOutputPin(*m_oPin)!= 0)	throw std::runtime_error("error registering output pin");
		// Result instance
		m_result= CTypeComposite::CreateInstance();
		m_x= CTypeFloat::CreateInstance();
		m_y= CTypeFloat::CreateInstance();
		m_z= CTypeFloat::CreateInstance();
		m_result->AddChild(m_x);
		m_result->AddChild(m_y);
		m_result->AddChild(m_z);
	}

	int OnValue (const CTypeWiimotesAccelerometer & message) {
		float x= message.GetForceX();
		float y= message.GetForceY();
		float z= message.GetForceZ();

		if (fabsf(x - m_lastX)> fabs(m_error * x))
			m_x->setValue(x);
		else
			m_x->setValue(0);

		if (fabsf(y - m_lastY)> fabs(m_error * y))
			m_y->setValue(y);
		else
			m_y->setValue(0);

		if (fabsf(z - m_lastZ)> fabs(m_error * z))
			m_z->setValue(z);
		else
			m_z->setValue(0);

		m_lastX= x;
		m_lastY= y;
		m_lastZ= z;

		return m_oPin->Send(m_result);
	}

private:
	virtual ~WiiAccEstimate() {}

	class InputPinAcc : public CInputPinWriteOnly<CTypeWiimotesAccelerometer,WiiAccEstimate> {
	public:
		InputPinAcc (const char * name, WiiAccEstimate & component)
		: CInputPinWriteOnly<CTypeWiimotesAccelerometer, WiiAccEstimate>(name, component) {}

		virtual int DoSend(const CTypeWiimotesAccelerometer & message) {
			return this->m_component->OnValue (message);			
		}
	};
	
	SmartPtr<IOutputPin> m_oPin;
	SmartPtr<CTypeComposite> m_result;
	SmartPtr<CTypeFloat> m_x, m_y, m_z;
	float m_lastX, m_lastY, m_lastZ;
	float m_error;
};

typedef ComponentFactory<WiiAccEstimate> WiiAccEstimateFactory;


/* *****************************************************************************
	wiimote_config_gui component

	Component which provides an wxPanel to interactively configure wiimotes
***************************************************************************** */
class WiimotesConfigGUI : public spcore::CComponentAdapter {
public:

	WiimotesConfigGUI(const char * name, int argc, const char * argv[]) 
	: CComponentAdapter(name, argc, argv) { }

	static const char* getTypeName() { return "wiimotes_config_gui"; };
	virtual const char* GetTypeName() const { return WiimotesConfigGUI::getTypeName(); };

	virtual wxWindow* GetGUI(wxWindow * parent) {
		wxWindow* w= new WiimotesConfiguration(parent);
		w->SetName(_("Wiimotes Configuration"));
		return w;
	}

private:
	virtual ~WiimotesConfigGUI() { }
};

// WiimotesConfigGUI component factory
typedef ComponentFactory<WiimotesConfigGUI> WiimotesConfigGUIFactory;

/* ******************************************************************************
	type factories
****************************************************************************** */
typedef spcore::SimpleTypeFactory<CTypeWiimotesStatus> CTypeWiimotesStatusFactory;
typedef spcore::SimpleTypeFactory<CTypeWiimotesAccelerometer> CTypeWiimotesAccelerometerFactory;
typedef spcore::SimpleTypeFactory<CTypeWiimotesButtons> CTypeWiimotesButtonsFactory;
typedef spcore::SimpleTypeFactory<CTypeWiimotesBalanceBoard> CTypeWiimotesBalanceBoardFactory;
typedef spcore::SimpleTypeFactory<CTypeWiimotesMotionPlus> CTypeWiimotesMotionPlusFactory;

/* ******************************************************************************
	wii  module
****************************************************************************** */
class WiiMotesModule : public CModuleAdapter {
public:
	WiiMotesModule() {
		//
		// types
		//
		RegisterTypeFactory(SmartPtr<spcore::ITypeFactory>(new CTypeWiimotesStatusFactory(), false));
		RegisterTypeFactory(SmartPtr<spcore::ITypeFactory>(new CTypeWiimotesAccelerometerFactory(), false));
		RegisterTypeFactory(SmartPtr<spcore::ITypeFactory>(new CTypeWiimotesButtonsFactory(), false));
		RegisterTypeFactory(SmartPtr<spcore::ITypeFactory>(new CTypeWiimotesBalanceBoardFactory(), false));
		RegisterTypeFactory(SmartPtr<spcore::ITypeFactory>(new CTypeWiimotesMotionPlusFactory(), false));

		//
		// components
		//
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiimotesConfigFactory(), false));
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiimotesInputFactory(), false));
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiimotesConfigGUIFactory(), false));

		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiiMpToCompoFactory(), false));
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiiBbToCompoFactory(), false));
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new WiiAccEstimateFactory(), false));
	}

	~WiiMotesModule() {
		WiiuseThreadController::destroyInstance();
	}

	virtual const char * GetName() const { return "mod_wiimotes"; }
};

static spcore::IModule* g_module= NULL;
SPEXPORT_FUNCTION spcore::IModule* module_create_instance()
{
	if (g_module== NULL) g_module= new WiiMotesModule();
	return g_module;
}

};
