#include "OISConfig.h"
#ifdef OIS_WIN32_WIIMOTE_SUPPORT
/*
The zlib/libpng License

Copyright (c) 2005-2007 Phillip Castaneda (pjcast -- www.wreckedgames.com)

This software is provided 'as-is', without any express or implied warranty. In no event will
the authors be held liable for any damages arising from the use of this software.

Permission is granted to anyone to use this software for any purpose, including commercial
applications, and to alter it and redistribute it freely, subject to the following
restrictions:

    1. The origin of this software must not be misrepresented; you must not claim that
		you wrote the original software. If you use this software in a product,
		an acknowledgment in the product documentation would be appreciated but is
		not required.

    2. Altered source versions must be plainly marked as such, and must not be
		misrepresented as being the original software.

    3. This notice may not be removed or altered from any source distribution.
*/
#include "OISWiiMote.h"
#include "OISWiiMoteFactoryCreator.h"
#include "OISException.h"
#include "OISWiiMoteForceFeedback.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include <limits.h>

using namespace OIS;

//-----------------------------------------------------------------------------------//
WiiMote::WiiMote(InputManager* creator, int id, bool buffered, WiiMoteFactoryCreator* local_creator) :
	JoyStick("cWiiMote", buffered, id, creator),
	mWiiCreator(local_creator),
	mtInitialized(false),
	mRingBuffer(OIS_WII_EVENT_BUFFER),
	mtLastButtonStates(0),
	mtLastPOVState(0),
	mtLastX(0.0f),
	mtLastY(1.0f),
	mtLastZ(0.0f),
	mtLastNunChuckX(0.0f),
	mtLastNunChuckY(1.0f),
	mtLastNunChuckZ(0.0f),
	mLastNunChuckXAxis(0),
	mLastNunChuckYAxis(0),
	_mWiiMoteMotionDelay(5),
	mRumble(0)
{
	mRumble = new WiiMoteForceFeedback(mWiiMote);
}

//-----------------------------------------------------------------------------------//
WiiMote::~WiiMote()
{
	delete mRumble;

	if( mWiiMote.IsConnected() )
	{
		mWiiMote.StopDataStream();
		mWiiMote.Disconnect();
	}
	mWiiCreator->_returnWiiMote(mDevID);
}

//-----------------------------------------------------------------------------------//
void WiiMote::_initialize()
{
	if( mWiiMote.ConnectToDevice(mDevID) == false )
		OIS_EXCEPT(E_InputDisconnected, "Error connecting to WiiMote!");

	if( mWiiMote.StartDataStream() == false )
		OIS_EXCEPT(E_InputDisconnected, "Error starting WiiMote data stream!");

	//Fill in joystick information
	mState.mVectors.clear();
	mState.mButtons.clear();
	mState.mAxes.clear();

	if( mWiiMote.IsNunChuckAttached() )
	{	//Setup for WiiMote + nunChuck
		mState.mVectors.resize(2);
		mState.mButtons.resize(9);
		mState.mAxes.resize(2);
		mState.mAxes[0].absOnly = true;
		mState.mAxes[1].absOnly = true;
	}
	else
	{	//Setup for WiiMote
		mState.mVectors.resize(1);
		mState.mButtons.resize(7);
	}

	mPOVs = 1;
	mState.clear();
	mtInitialized = true;
}

//-----------------------------------------------------------------------------------//
void WiiMote::_threadUpdate()
{
	//Leave early if nothing is setup yet
	if( mtInitialized == false )
		return;

	//Oops, no room left in ring buffer.. have to wait for client app to call Capture()
	if( mRingBuffer.GetWriteAvailable() == 0 )
		return;

	WiiMoteEvent newEvent;
	newEvent.clear();

	//Update read
	mWiiMote.HeartBeat();

	//Get & check current button states
	const cWiiMote::tButtonStatus &bState = mWiiMote.GetLastButtonStatus();
	_doButtonCheck(bState.m1, 0, newEvent.pushedButtons, newEvent.releasedButtons);	//1
	_doButtonCheck(bState.m2, 1, newEvent.pushedButtons, newEvent.releasedButtons);	//2
	_doButtonCheck(bState.mA, 2, newEvent.pushedButtons, newEvent.releasedButtons);	//A
	_doButtonCheck(bState.mB, 3, newEvent.pushedButtons, newEvent.releasedButtons);	//B
	_doButtonCheck(bState.mPlus, 4, newEvent.pushedButtons, newEvent.releasedButtons);//+
	_doButtonCheck(bState.mMinus, 5, newEvent.pushedButtons, newEvent.releasedButtons);//-
	_doButtonCheck(bState.mHome, 6, newEvent.pushedButtons, newEvent.releasedButtons);//Home

	//Check POV
	newEvent.povChanged = _doPOVCheck(bState, newEvent.povDirection);

	//Do motion check on main orientation - accounting for sensitivity factor
	mWiiMote.GetCalibratedAcceleration(newEvent.x, newEvent.y, newEvent.z);
	//Normalize new vector (old vector is already normalized)
	float len = sqrt((newEvent.x*newEvent.x) + (newEvent.y*newEvent.y) + (newEvent.z*newEvent.z));
	newEvent.x /= len;
	newEvent.y /= len;
	newEvent.z /= len;

	//Get new angle
	float angle = acos((newEvent.x * mtLastX) + (newEvent.y * mtLastY) + (newEvent.z * mtLastZ));
	if( angle > (mVector3Sensitivity * (M_PI / 180.0)) )
	{	//Store for next check
		mtLastX = newEvent.x;
		mtLastY = newEvent.y;
		mtLastZ = newEvent.z;

		if( _mWiiMoteMotionDelay <= 0 )
			newEvent.movement = true; //Set flag as moved
		else
			--_mWiiMoteMotionDelay;
	}

	//Act on NunChuck Data
	if( mWiiMote.IsNunChuckAttached() )
	{
		const cWiiMote::tChuckReport &bState = mWiiMote.GetLastChuckReport();
		_doButtonCheck(bState.mButtonC, 7, newEvent.pushedButtons, newEvent.releasedButtons); //C
		_doButtonCheck(bState.mButtonZ, 8, newEvent.pushedButtons, newEvent.releasedButtons); //Z

		mWiiMote.GetCalibratedChuckAcceleration(newEvent.nunChuckx, newEvent.nunChucky, newEvent.nunChuckz);
		//Normalize new vector (old vector is already normalized)
		float len = sqrt((newEvent.nunChuckx*newEvent.nunChuckx) +
			             (newEvent.nunChucky*newEvent.nunChucky) +
						 (newEvent.nunChuckz*newEvent.nunChuckz));

		newEvent.nunChuckx /= len;
		newEvent.nunChucky /= len;
		newEvent.nunChuckz /= len;

		float angle = acos((newEvent.nunChuckx * mtLastNunChuckX) +
			               (newEvent.nunChucky * mtLastNunChuckY) +
						   (newEvent.nunChuckz * mtLastNunChuckZ));

		if( angle > (mVector3Sensitivity * (M_PI / 180.0)) )
		{	//Store for next check
			mtLastNunChuckX = newEvent.nunChuckx;
			mtLastNunChuckY = newEvent.nunChucky;
			mtLastNunChuckZ = newEvent.nunChuckz;

			if( _mWiiMoteMotionDelay <= 0 )
				newEvent.movementChuck = true;
		}

		//Ok, Now check both NunChuck Joystick axes for movement
		float tempX = 0.0f, tempY = 0.0f;
		mWiiMote.GetCalibratedChuckStick(tempX, tempY);

		//Convert to int and clip
		newEvent.nunChuckXAxis = (int)(tempX * JoyStick::MAX_AXIS);
		if( newEvent.nunChuckXAxis > JoyStick::MAX_AXIS )
			newEvent.nunChuckXAxis = JoyStick::MAX_AXIS;
		else if( newEvent.nunChuckXAxis < JoyStick::MIN_AXIS )
			newEvent.nunChuckXAxis = JoyStick::MIN_AXIS;

		newEvent.nunChuckYAxis = (int)(tempY * JoyStick::MAX_AXIS);
		if( newEvent.nunChuckYAxis > JoyStick::MAX_AXIS )
			newEvent.nunChuckYAxis = JoyStick::MAX_AXIS;
		else if( newEvent.nunChuckYAxis < JoyStick::MIN_AXIS )
			newEvent.nunChuckYAxis = JoyStick::MIN_AXIS;

		//Apply a little dead-zone dampner
		int xDiff = newEvent.nunChuckXAxis - mLastNunChuckXAxis;
		if( xDiff > 1500 || xDiff < -1500 )
		{
			mLastNunChuckXAxis = newEvent.nunChuckXAxis;
			newEvent.nunChuckXAxisMoved = true;
		}

		int yDiff = newEvent.nunChuckYAxis - mLastNunChuckYAxis;
		if( yDiff > 1500 || yDiff < -1500 )
		{
			mLastNunChuckYAxis = newEvent.nunChuckYAxis;
			newEvent.nunChuckYAxisMoved = true;
		}
	}

	//Ok, put entry in ringbuffer if something changed
	if(newEvent.pushedButtons || newEvent.releasedButtons || newEvent.povChanged || newEvent.movement ||
	   newEvent.movementChuck || newEvent.nunChuckXAxisMoved || newEvent.nunChuckYAxisMoved)
	{
		mRingBuffer.Write(&newEvent, 1);
	}

	//mWiiMote.PrintStatus();
}

//-----------------------------------------------------------------------------------//
void WiiMote::_doButtonCheck(bool new_state, int ois_button, unsigned int &pushed, unsigned int &released)
{
	const bool old_state = ((mtLastButtonStates & ( 1L << ois_button )) == 0) ? false : true;

	//Check to see if new state and old state are the same, and hence, need no change
	if( new_state == old_state )
		return;

	//Ok, so it changed... but how?
	if( new_state )
	{	//Ok, new state is pushed, old state was not pushed.. so send button press
		mtLastButtonStates |= 1 << ois_button; //turn the bit flag on
		pushed |= 1 << ois_button;
	}
	else
	{	//Ok, so new state is not pushed, and old state was pushed.. So, send release
		mtLastButtonStates &= ~(1 << ois_button); //turn the bit flag off
		released |= 1 << ois_button;
	}
}

//-----------------------------------------------------------------------------------//
bool WiiMote::_doPOVCheck(const cWiiMote::tButtonStatus &bState, unsigned int &newPosition)
{
	newPosition = Pov::Centered;

	if( bState.mUp )
		newPosition |= Pov::North;
	else if( bState.mDown )
		newPosition |= Pov::South;

	if( bState.mLeft )
		newPosition |= Pov::West;
	else if( bState.mRight )
		newPosition |= Pov::East;

	//Was there a change?
	if( mtLastPOVState != newPosition )
	{
		mtLastPOVState = newPosition;
		return true;
	}

	return false;
}

//-----------------------------------------------------------------------------------//
void WiiMote::setBuffered(bool buffered)
{
	mBuffered = buffered;
}

//-----------------------------------------------------------------------------------//
void WiiMote::capture()
{
	//Anything to read?
	int entries = mRingBuffer.GetReadAvailable();
	if( entries <= 0 )
		return;

	WiiMoteEvent events[OIS_WII_EVENT_BUFFER];
	if( entries > OIS_WII_EVENT_BUFFER )
		entries = OIS_WII_EVENT_BUFFER;

	mRingBuffer.Read(events, entries);

	//Loop through each event
	for( int i = 0; i < entries; ++i )
	{
		//Any movement changes in the main accellerometers?
		if( events[i].movement )
		{
			mState.mVectors[0].x = events[i].x;
			mState.mVectors[0].y = events[i].y;
			mState.mVectors[0].z = events[i].z;
			if( mBuffered && mListener )
				if( !mListener->vector3Moved( JoyStickEvent( this, mState ), 0 ) ) return;
		}

		//Check NunChuck movements
		if( events[i].movementChuck )
		{
			mState.mVectors[1].x = events[i].nunChuckx;
			mState.mVectors[1].y = events[i].nunChucky;
			mState.mVectors[1].z = events[i].nunChuckz;
			if( mBuffered && mListener )
				if( !mListener->vector3Moved( JoyStickEvent( this, mState ), 1 ) ) return;
		}

		if( events[i].nunChuckXAxisMoved )
		{
			mState.mAxes[0].abs = events[i].nunChuckXAxis;

			if( mBuffered && mListener )
				if( !mListener->axisMoved( JoyStickEvent( this, mState ), 0 ) ) return;
		}

		if( events[i].nunChuckYAxisMoved )
		{
			mState.mAxes[1].abs = events[i].nunChuckYAxis;

			if( mBuffered && mListener )
				if( !mListener->axisMoved( JoyStickEvent( this, mState ), 1 ) ) return;
		}

		//Has the hat swtich changed?
		if( events[i].povChanged )
		{
			mState.mPOV[0].direction = events[i].povDirection;
			if( mBuffered && mListener )
				if( !mListener->povMoved( JoyStickEvent( this, mState ), 0 ) ) return;
		}

		//Check for any pushed/released events for each button bit
		int buttons = (int)mState.mButtons.size();
		for( int b = 0; b < buttons; ++b )
		{
			unsigned bit_flag = 1 << b;
			if( (events[i].pushedButtons & bit_flag) != 0 )
			{	//send event
				mState.mButtons[b] = true;
				if( mBuffered && mListener )
					if( !mListener->buttonPressed( JoyStickEvent( this, mState ), b ) ) return;
			}

			if( (events[i].releasedButtons & bit_flag) != 0 )
			{	//send event
				mState.mButtons[b] = false;
				if( mBuffered && mListener )
					if( !mListener->buttonReleased( JoyStickEvent( this, mState ), b ) ) return;
			}
		}
	}
}

//-----------------------------------------------------------------------------------//
Interface* WiiMote::queryInterface(Interface::IType type)
{
	if( type == Interface::ForceFeedback && mtInitialized )
		return mRumble;

	return 0;
}
#endif