/////////////////////////////////////////////////////////////////////////////
// Name:        test_mod_wiimotes_basic.cpp
// Author:      Cesar Mauri Loba
// 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 memory leak when a reconnect is issued while motion plus

#include "spcore/coreruntime.h"
#include "testcommon/testcommon.h"
#include "3rdparty/nvwa/debug_new.h"

#include "mod_wiimotes/wiimotes_types.h"

#include "spcore/basictypes.h"

#include <ostream>
#include <sstream>
#include <iostream>
#include <math.h>

#include <stdio.h>
#include <stdlib.h>

using namespace spcore;
using namespace mod_wiimotes;

class VoidClass{};

VoidClass voidClassInstance;

static
void DumpStatus (const CTypeWiimotesStatus & msg)
{
	std::cout << "Dumping status message\n";
	std::cout << "Status: " << msg.GetGeneralStatus() << std::endl;
	if (msg.GetGeneralStatus() == CTypeWiimotesStatus::CONNECTED) {
		std::cout << "Connected count:" << msg.GetConnectedCount() << std::endl;
		for (unsigned int i= 0; i< msg.GetMaxCount(); ++i) {
			if (msg.IsConnected(i)) {
				std::cout << "Wiimote " << i+1 << " is connected\n";
				if (msg.HasNunchuk(i)) std::cout << "Nunchuck found\n";
				if (msg.HasBalanceBoard(i)) std::cout << "Balance board\n";
				if (msg.HasMotionPlus(i)) std::cout << "Motion plus\n";
				if (msg.IsAccelerometersEnabled(i)) std::cout << "Accelerometers enabled\n";
				if (msg.IsMotionPlusEnabled(i)) std::cout << "Motion plus enabled\n";
			}
			else
				std::cout << "Wiimote " << i+1 << " is NOT connected\n";
		}
	}
}

static
void DumpStatus (const CTypeWiimotesAccelerometer & msg)
{
	std::cout << "Dumping acc\n";

	std::cout << "Forces: (" << msg.GetForceX() << ", " << msg.GetForceY() << ", " << msg.GetForceZ() << ")\n";
	if (msg.IsOrientationAccurate()) {
		std::cout << "Orientation: (Pitch: " << msg.GetPitch() << ", Roll: " << msg.GetRoll() << ")\n";
	}

	calls_per_second();
}

static
void DumpStatus (const CTypeWiimotesButtons & msg)
{
	std::cout << "Dumping buttons\n";

	if (msg.IsPressedA()) std::cout << "IsPressedA\n";
	if (msg.IsPressedB()) std::cout << "IsPressedB\n";
	if (msg.IsPressedOne()) std::cout << "IsPressedOne\n";
	if (msg.IsPressedTwo()) std::cout << "IsPressedTwo\n";
	if (msg.IsPressedMinus()) std::cout << "IsPressedMinus\n";
	if (msg.IsPressedPlus()) std::cout << "IsPressedPlus\n";
	if (msg.IsPressedHome()) std::cout << "IsPressedHome\n";
	if (msg.IsPressedUp()) std::cout << "IsPressedUp\n";
	if (msg.IsPressedDown()) std::cout << "IsPressedDown\n";
	if (msg.IsPressedLeft()) std::cout << "IsPressedLeft\n";
	if (msg.IsPressedRight()) std::cout << "IsPressedRight\n";
	if (msg.IsPressedC()) std::cout << "IsPressedC\n";
	if (msg.IsPressedZ()) std::cout << "IsPressedZ\n";
}

static
void DumpStatus (const CTypeWiimotesBalanceBoard & msg)
{
//	std::cout << "Dumping balance board\n";

	//std::cout << msg.GetTopLeft() << ", " << msg.GetTopRight() << std::endl;
	//std::cout << msg.GetBottomLeft() << ", " << msg.GetBottomRight() << std::endl;

	printf ("%.2f\t %.2f\t %.2f\t %.2f\t %.2f\t %.2f\n",
		msg.GetTopLeft(), msg.GetTopRight(), msg.GetBottomLeft(), msg.GetBottomRight(),
		msg.GetCenterOfMassX(), msg.GetCenterOfMassY());

	//std::cout << "(X: " << msg.GetCenterOfMassX() << ", Y: " << msg.GetCenterOfMassY() << ")\n";
}

static
void DumpStatus (const CTypeWiimotesMotionPlus & msg)
{
//	std::cout << "Dumping motion plus\n";

//	std::cout << msg.GetXSpeed() << "\t" << msg.GetYSpeed() << "\t" << msg.GetZSpeed() << std::endl;

	printf ("%.2f\t %.2f\t %.2f\n", msg.GetXSpeed(), msg.GetYSpeed(), msg.GetZSpeed());
}


// Status input pin
class InputPinStatus : public CInputPinWriteOnly<CTypeWiimotesStatus, VoidClass> {
public:
	InputPinStatus (VoidClass & component) : CInputPinWriteOnly<CTypeWiimotesStatus, VoidClass>("test_status", component) {}
	virtual int DoSend(const CTypeWiimotesStatus & msg) {
		DumpStatus (msg);
		return 0;
	}
};

// CTypeWiimotesAccelerometer
class InputPinAccelerometer : public CInputPinWriteOnly<CTypeWiimotesAccelerometer, VoidClass> {
public:
	InputPinAccelerometer (VoidClass & component) : CInputPinWriteOnly<CTypeWiimotesAccelerometer, VoidClass>("test_acc", component) {}
	virtual int DoSend(const CTypeWiimotesAccelerometer & msg) {
		DumpStatus (msg);
		return 0;
	}
};


// CTypeWiimotesButtons
class InputPinButtons : public CInputPinWriteOnly<CTypeWiimotesButtons, VoidClass> {
public:
	InputPinButtons (VoidClass & component) : CInputPinWriteOnly<CTypeWiimotesButtons, VoidClass>("test_buttons", component) {}
	virtual int DoSend(const CTypeWiimotesButtons & msg) {
		DumpStatus (msg);
		return 0;
	}
};

// BalanceBoard
class InputPinBalanceBoard : public CInputPinWriteOnly<CTypeWiimotesBalanceBoard, VoidClass> {
public:
	InputPinBalanceBoard (VoidClass & component) : CInputPinWriteOnly<CTypeWiimotesBalanceBoard, VoidClass>("test_balance_board", component) {}
	virtual int DoSend(const CTypeWiimotesBalanceBoard & msg) {
		DumpStatus (msg);
		return 0;
	}
};

// MotionPlus
class InputPinMotionPlus : public CInputPinWriteOnly<CTypeWiimotesMotionPlus, VoidClass> {
public:
	InputPinMotionPlus (VoidClass & component) : CInputPinWriteOnly<CTypeWiimotesMotionPlus, VoidClass>("test_motion_plus", component) {}
	virtual int DoSend(const CTypeWiimotesMotionPlus & msg) {
		DumpStatus (msg);
		return 0;
	}
};

bool g_exit_loop= false;

void test_wiimotes_basic()
{
	ICoreRuntime* cr= getSpCoreRuntime();



	//
	// Create wiimotes configuration component
	//
	SmartPtr<spcore::IComponent> config= cr->CreateComponent("wiimotes_config", "wc", 0, NULL);
	if (config.get()== NULL) ExitErr("error creating wiimotes_config");

	// Create & connect input pin to dump status
	SmartPtr<IInputPin> statusPin(new InputPinStatus(voidClassInstance), false);
	if (statusPin.get()== NULL) ExitErr("error creating input status test pin");
	IOutputPin* status_opin= config->FindOutputPin(*config, "status");
	if (!status_opin) ExitErr("output pin status not found");
	if (status_opin->Connect (*statusPin)) ExitErr("error connecting pin");

	//
	// Create wiimotes input component
	//
	SmartPtr<spcore::IComponent> wiinput= cr->CreateComponent("wiimotes_input", "wi", 0, NULL);
	if (wiinput.get()== NULL) ExitErr("error creating wiimotes_input");


	// Create & connect input pin to dump nunchuck
	SmartPtr<IInputPin> nuncPin(new InputPinAccelerometer(voidClassInstance), false);
	if (nuncPin.get()== NULL) ExitErr("error creating input nunchuck test pin");
	IOutputPin* nunc_opin= wiinput->FindOutputPin(*wiinput, "nunchuck_accelerometers");
	if (!nunc_opin) ExitErr("output pin nunchuck not found");
	if (nunc_opin->Connect (*nuncPin)) ExitErr("error connecting pin");


	// Create & connect input pin to dump buttons
	SmartPtr<IInputPin> btnPin(new InputPinButtons(voidClassInstance), false);
	if (btnPin.get()== NULL) ExitErr("error creating input button test pin");
	IOutputPin* btn_opin= wiinput->FindOutputPin(*wiinput, "buttons");
	if (!btn_opin) ExitErr("output pin buttons not found");
	if (btn_opin->Connect (*btnPin)) ExitErr("error connecting pin");

	// Create & connect input pin to dump balance board
	SmartPtr<IInputPin> bbPin(new InputPinBalanceBoard(voidClassInstance), false);
	if (bbPin.get()== NULL) ExitErr("error creating input balance board test pin");
	IOutputPin* bb_opin= wiinput->FindOutputPin(*wiinput, "balance_board");
	if (!bb_opin) ExitErr("output pin balance board not found");
	if (bb_opin->Connect (*bbPin)) ExitErr("error connecting pin");


	// Create & connect input pin to dump accelerometers
	SmartPtr<IInputPin> accPin(new InputPinAccelerometer(voidClassInstance), false);
	if (accPin.get()== NULL) ExitErr("error creating input acc test pin");
	IOutputPin* acc_opin= wiinput->FindOutputPin(*wiinput, "accelerometers");
	if (!acc_opin) ExitErr("output pin acc not found");
	//if (acc_opin->Connect (*accPin)) ExitErr("error connecting pin");



	// Create & connect input pin to dump motion plus
	SmartPtr<IInputPin> mpPin(new InputPinMotionPlus(voidClassInstance), false);
	if (mpPin.get()== NULL) ExitErr("error creating input motion plus test pin");
	IOutputPin* mp_opin= wiinput->FindOutputPin(*wiinput, "motion_plus");
	if (!mp_opin) ExitErr("output pin motion plus not found");
//	if (mp_opin->Connect (*mpPin)) ExitErr("error connecting pin");

	//
	// Find "reconnect" input pin
	//
	IInputPin* reconnect_ipin= config->FindInputPin(*config, "reconnect");
	if (!reconnect_ipin) ExitErr("reconnect input pin not found");
	SmartPtr<CTypeBool> dummyValue= CTypeBool::CreateInstance();
	if (!dummyValue.get()) ExitErr("cannot create boolean value");

	//
	// Run them all
	//
	config->Initialize();
	wiinput->Initialize();
	config->Start();
	wiinput->Start();

	int key= 0;
	while (key!= 'q' && key!= 'Q') {
		if (key!= '\n') {
			#ifdef WIN32
				Sleep (1000);
			#else
				sleep (1);
			#endif
			std::cout << "Keystrokes:\n";
			std::cout << "\t q -> quit\n";
			std::cout << "\t r -> reconnect\n";
			std::cout << "\t a -> enable accelerometers\n";
			std::cout << "\t m -> enable motion plus\n";
			std::cout << "\t d -> disable accelerometers/motion plus\n";
		}

		key= getch_no_block();
		while (!key && !g_exit_loop) {
			#ifdef WIN32
				Sleep (100);
			#else
				usleep (100000);
			#endif
			key= getch_no_block();
		}
		if (g_exit_loop) break;

		switch (key) {
			case 'r': case 'R':
				// Reconnect
				reconnect_ipin->Send(dummyValue);
				break;
			case 'a': case 'A':
				wiinput->Stop();
				acc_opin->Connect (*accPin);
				wiinput->Start();
				break;
			case 'm': case 'M':
				wiinput->Stop();
				mp_opin->Connect (*mpPin);
				wiinput->Start();
				break;
			case 'd': case 'D':
				wiinput->Stop();
				acc_opin->Disconnect (*accPin);
				mp_opin->Disconnect(*mpPin);
				wiinput->Start();
				break;
		}
	}

	//
	// Finish them all
	//
	std::cout << "Begin finish\n";
	wiinput->Stop();
	config->Stop();
	wiinput->Finish();
	config->Finish();
	std::cout << "End finish\n";
}


// ----------------------------------------------------------------------------
// main
// ----------------------------------------------------------------------------
#ifndef TEST_NO_MAIN
int main(int, char *[]) {

	// Initialize spcore
	ICoreRuntime* cr= getSpCoreRuntime();
	if (!cr) return false;

	// Load module
	int retval= cr->LoadModule("mod_wiimotes");
	DumpCoreRuntime(cr);
	if (retval!= 0) ExitErr("error loading mod_wiimotes");

	test_wiimotes_basic();

	// Free spcore
	freeSpCoreRuntime();

	// If execution reaches this => all tests ok
	return 0;
}
#endif
