/* 
 * 
 * Confidential Information of Telekinesys Research Limited (t/a Havok).  Not for disclosure or distribution without Havok's
 * prior written consent.This software contains code, techniques and know-how which is confidential and proprietary to Havok.
 * Level 2 and Level 3 source code contains trade secrets of Havok. Havok Software (C) Copyright 1999-2008 Telekinesys Research Limited t/a Havok. All Rights Reserved. Use of this software is subject to the terms of an end user license agreement.
 * 
 */

#include <Demos/demos.h>

#include <Demos/DemoCommon/Utilities/Character/CharacterUtils.h>

#include <Physics/Utilities/CharacterControl/CharacterRigidBody/hkpCharacterRigidBody.h>
#include <Physics/Utilities/CharacterControl/StateMachine/hkpDefaultCharacterStates.h>

#include <Physics/Collide/Filter/Group/hkpGroupFilter.h>
#include <Physics/Collide/Shape/Convex/Capsule/hkpCapsuleShape.h>
#include <Physics/Collide/Query/CastUtil/hkpWorldRayCastInput.h>
#include <Physics/Collide/Query/CastUtil/hkpWorldRayCastOutput.h>
#include <Physics/Collide/Query/Collector/RayCollector/hkpAllRayHitCollector.h>

#include <Physics/Dynamics/Entity/hkpRigidBody.h>
#include <Physics/Dynamics/World/hkpWorld.h>

#include <Common/Visualize/hkDebugDisplay.h>
#include <Graphics/Common/Camera/hkgCamera.h>


#if defined(HK_PLATFORM_WIN32)
	#include <windows.h>
#endif


#define INPUT_DEADZONE 0.2f // PS2 style .. large
static void HK_CALL _deadzone(float& stickX, float& stickY)
{
	const float deadzoneScale = 1.0f/(1.0f-INPUT_DEADZONE);
	stickX = ( stickX < -INPUT_DEADZONE) ? (stickX + INPUT_DEADZONE) * deadzoneScale : (
		( stickX > INPUT_DEADZONE ) ? (stickX - INPUT_DEADZONE) * deadzoneScale : 0);
	stickY = ( stickY < -INPUT_DEADZONE) ? (stickY + INPUT_DEADZONE) * deadzoneScale : (
		( stickY > INPUT_DEADZONE ) ? (stickY - INPUT_DEADZONE) * deadzoneScale : 0);
}


void HK_CALL CharacterUtils::getUserInputForCharacter( hkDemoEnvironment* env, float& deltaAngle, float& posX, float& posY )
{
#if defined(HK_PLATFORM_WIN32)
	// Only run this function if the window is in focus, because it grabs the mouse
	if ( GetForegroundWindow() != env->m_window->getPlatformHandle() )
		return;
#endif

	// Mouse
	deltaAngle = 0.f;
	posX = 0.f;
	posY = 0.f;
	const hkgPad& pad = *(env->m_gamePad);

	bool haveProperGamePad = env->m_window->hasGamePads() && !(env->m_options->m_forceKeyboardGamepad);

	// No gamepad (win32 and ps3 default at the mo)
	if (!haveProperGamePad)
	{
		const hkReal MOUSE_SENSITIVITY = 1.0f;
		int mouseX = env->m_window->getMouse().getPosX();
		int halfWindowWidth = env->m_window->getWidth() >> 1;

		hkReal inc  = hkReal(halfWindowWidth - mouseX) / halfWindowWidth;
		inc = (inc < -1.0f) ? -1.0f : inc;
		inc = (inc >  1.0f) ?  1.0f : inc;
		deltaAngle = 3.4f * inc * MOUSE_SENSITIVITY;

		// Snap mouse back to middle
		env->m_window->setMousePosition(env->m_window->getWidth() >> 1, env->m_window->getHeight() >> 1);

		if (env->m_window->getKeyboard().getKeyState( 'A'))
			posX=1.f;
		if (env->m_window->getKeyboard().getKeyState( 'D'))
			posX=-1.f;
		if (env->m_window->getKeyboard().getKeyState( 'W'))
			posY=-1.f;
		if (env->m_window->getKeyboard().getKeyState( 'S'))
			posY=1.f;

	}
	else if (pad.isValid())
	{
		if ( (pad.getButtonState() & HKG_PAD_BUTTON_L1) == 0)
		{
			const hkReal PAD_SENSITIVITY = 0.05f;
			float stickX = pad.getStickPosX(1);
			float stickY = pad.getStickPosY(1);
			_deadzone(stickX, stickY);
			deltaAngle = 3.4f * -stickX * PAD_SENSITIVITY;
			posY = -stickY;
		}

		{
			float stickZ = pad.getStickPosX(0);
			float stickZZ = 0;
			_deadzone(stickZ, stickZZ);
			posX = -stickZ; // strafe on stick0
		}
	}

	// dpad == dir keys on PC
	{
		if ((pad.getButtonState() & HKG_PAD_DPAD_UP) != 0)
			posY=-1.f;
		if ((pad.getButtonState() & HKG_PAD_DPAD_LEFT) != 0)
			posX=1.f;
		if ((pad.getButtonState() & HKG_PAD_DPAD_RIGHT) != 0)
			posX=-1.f;
		if ((pad.getButtonState() & HKG_PAD_DPAD_DOWN) != 0)
			posY=1.f;
	}

	// Normalize the movement vector
	hkReal lenSqd = posY * posY + posX * posX;
	if (lenSqd > HK_REAL_MIN)
	{
		lenSqd = hkMath::sqrt(lenSqd);
		posY /= lenSqd;
		posX /= lenSqd;
	}
}


hkpCharacterContext* HK_CALL CharacterUtils::createDefaultCharacterContext()
{
	hkpCharacterState* state;
	hkpCharacterStateManager* manager = new hkpCharacterStateManager();

	state = new hkpCharacterStateOnGround();
	manager->registerState( state,	HK_CHARACTER_ON_GROUND);
	state->removeReference();

	state = new hkpCharacterStateInAir();
	manager->registerState( state,	HK_CHARACTER_IN_AIR);
	state->removeReference();

	state = new hkpCharacterStateJumping();
	manager->registerState( state,	HK_CHARACTER_JUMPING);
	state->removeReference();

	state = new hkpCharacterStateClimbing();
	manager->registerState( state,	HK_CHARACTER_CLIMBING);
	state->removeReference();

	hkpCharacterContext* characterProxyContext = new hkpCharacterContext(manager, HK_CHARACTER_ON_GROUND);
	manager->removeReference();
	return characterProxyContext;
}



void HK_CALL CharacterUtils::displayCharacterState(hkpCharacterContext* context, hkDemoEnvironment* env )
{

	// Rigid body controller state
	hkpCharacterStateType stateRBC = context->getState();
	char * stateStrRBC;

	switch (stateRBC)
	{
	case HK_CHARACTER_ON_GROUND:
		stateStrRBC = "On Ground";	break;
	case HK_CHARACTER_JUMPING:
		stateStrRBC = "Jumping"; break;
	case HK_CHARACTER_IN_AIR:
		stateStrRBC = "In Air"; break;
	case HK_CHARACTER_CLIMBING:
		stateStrRBC = "Climbing"; break;
	default:
		stateStrRBC = "Other";	break;
	}

	char buffer[255];
	hkString::snprintf(buffer, 255, "State: %s", stateStrRBC);
	env->m_textDisplay->outputText(buffer, 20, 450, 0xffffffff);
}


void HK_CALL CharacterUtils::setCharacterInput( const hkpCharacterRigidBody* characterRb, hkpCharacterInput& input, const hkVector4& gravity )
{
	//
	// Copy some values from the character controller
	//
	input.m_velocity = characterRb->getLinearVelocity();
	input.m_position = characterRb->getPosition();
	input.m_up = characterRb->m_up;
	input.m_characterGravity = gravity;

	//
	// Call get support and set the relevant values in the character input
	//

	hkpSurfaceInfo ground;
	// XXX this parameter should not be needed for check support
	hkStepInfo stepInfo;
	stepInfo.m_deltaTime = 1.f / 60.f;
	stepInfo.m_invDeltaTime = 60.f;
	characterRb->checkSupport( stepInfo, ground);

	input.m_isSupported = (ground.m_supportedState == hkpSurfaceInfo::SUPPORTED);
	input.m_surfaceNormal = ground.m_surfaceNormal;
	input.m_surfaceVelocity = ground.m_surfaceVelocity;
	input.m_surfaceMotionType = ground.m_surfaceMotionType;

}



SimpleCharacter::SimpleCharacter( const hkVector4& position, const hkVector4& up )
{
	HK_ASSERT( 0, up.isNormalized3() );
	//  Create character
	{
		hkVector4 vertexA; vertexA.setMul4(.4f, up );
		hkVector4 vertexB; vertexB.setMul4(-.4f, up);
		hkpShape* characterShape = new hkpCapsuleShape(vertexA, vertexB, .6f);

		// Construct a character rigid body
		hkpCharacterRigidBodyCinfo info;
		info.m_shape = characterShape;
		info.m_position = position;
		info.m_up = up;

		m_characterRb = new hkpCharacterRigidBody( info );

		characterShape->removeReference();
	}

	m_currentAngle = HK_REAL_PI * 0.5f; 
	m_gravity.setMul4(-16, up);

	m_characterRbContext = CharacterUtils::createDefaultCharacterContext();
}


SimpleCharacter::SimpleCharacter( hkpCharacterRigidBody* characterRb, hkpCharacterContext* context )
{ 
	m_characterRb = characterRb; 
	m_currentAngle = HK_REAL_PI * 0.5f; 
	m_characterRbContext = context;

	characterRb->addReference();
	context->addReference();

	m_gravity.setMul4(-16, characterRb->m_up);
	//m_hasUserControl = false;
}

SimpleCharacter::~SimpleCharacter()
{	
	m_characterRb->removeReference();
	m_characterRbContext->removeReference();
};

void SimpleCharacter::update( hkReal timestep, const CharacterControls& controls, bool showForwardDirection )
{
	hkpCharacterInput input;
	hkpCharacterOutput output;

	input.m_atLadder = false;
	input.m_stepInfo.m_deltaTime = timestep;
	input.m_stepInfo.m_invDeltaTime = 1.0f / timestep;

	input.m_forward = controls.m_forward;
	input.m_inputLR = controls.m_straffeLeftRight;
	input.m_inputUD = controls.m_forwardBack;
	input.m_wantJump = controls.m_wantJump;

	CharacterUtils::setCharacterInput( m_characterRb, input, m_gravity );

	m_characterRbContext->update(input, output);		

	m_characterRb->setLinearVelocity(output.m_velocity, timestep);

	if (showForwardDirection)
	{
		hkVector4 start; start.setAdd4( m_characterRb->getPosition(), m_characterRb->m_up );
		hkVector4 end; end = start; end.addMul4( 1.f, controls.m_forward );
		HK_DISPLAY_LINE( start, end, hkColor::BLUE );
	}
}



UserControlledSimpleCharacter::UserControlledSimpleCharacter( const hkVector4& position, const hkVector4& up )
:	SimpleCharacter( position, up )
{
}

UserControlledSimpleCharacter::UserControlledSimpleCharacter( hkpCharacterRigidBody* characterRb, hkpCharacterContext* context )
: SimpleCharacter( characterRb, context )
{ 
	m_hasUserControl = true;
	m_makeOccludingObjectsTransparent = true;
}

void UserControlledSimpleCharacter::update( hkReal timestep, hkDemoEnvironment* env, hkpWorld* world )
{
	CharacterControls controls;
	if ( m_hasUserControl )
	{
		setInputFromUserControls( env, controls );
	}
	else
	{
		controls.m_forward.setZero4();
		controls.m_forwardBack = 0;
		controls.m_straffeLeftRight = 0;
		controls.m_wantJump = false;
	}

	SimpleCharacter::update( timestep, controls, false );

	if ( m_hasUserControl )
	{
		updateCamera( world, env );
	}
}


void UserControlledSimpleCharacter::setInputFromUserControls( hkDemoEnvironment* env, CharacterControls& controls )
{
	// Get user input data
	hkReal deltaAngle;

	CharacterUtils::getUserInputForCharacter(env, deltaAngle, controls.m_straffeLeftRight, controls.m_forwardBack );

	m_currentAngle += deltaAngle;
	hkQuaternion currentOrient; currentOrient.setAxisAngle(m_characterRb->m_up, m_currentAngle);
 
	controls.m_wantJump =  env->m_window->getMouse().wasButtonPressed(HKG_MOUSE_LEFT_BUTTON);
	controls.m_forward.set(1,0,0);
	controls.m_forward.setRotatedDir( currentOrient, controls.m_forward );
}

//// Objects colors
#define NORMAL_GRAY				hkColor::rgbFromChars( 192, 192, 192, 255 )
#define TRANSPARENT_GREY		hkColor::rgbFromChars( 192, 192, 192, 125 )

void UserControlledSimpleCharacter::updateCamera( hkpWorld* world, hkDemoEnvironment* env )
{
	const hkReal height = .7f;
	hkVector4 forward;
	forward.set(1,0,0);
	hkQuaternion currentOrient;
	currentOrient.setAxisAngle(m_characterRb->m_up, m_currentAngle);
	forward.setRotatedDir( currentOrient, forward );

	hkVector4 to;
	to = m_characterRb->getPosition();
	to.addMul4(height, m_characterRb->m_up );

	hkVector4 dir;
	dir.setMul4( height, m_characterRb->m_up );
	dir.addMul4( -4.f, forward);

	hkVector4 from;
	from.setAdd4(to, dir);

	// Make object in the line of sight transparent
	if (m_makeOccludingObjectsTransparent)
	{
		// Cast down to landscape to get an accurate position
		hkpWorldRayCastInput raycastIn;
		hkpWorldRayCastOutput raycastOut;

		// Reverse direction for raycasting
		raycastIn.m_from = to;
		raycastIn.m_to = from;
		raycastIn.m_filterInfo = hkpGroupFilter::calcFilterInfo(0);

		hkpAllRayHitCollector collector;

		world->castRay( raycastIn, collector);

		for (int i = 0; i < m_transparentObjects.getSize(); ++i )
		{
			HK_SET_OBJECT_COLOR(m_transparentObjects[i], NORMAL_GRAY );
		}
		m_transparentObjects.clear();

		// Loop over all collected objects
		for(int i=0; i < collector.getHits().getSize();i++)
		{				
			hkpWorldRayCastOutput raycastOut = collector.getHits()[i];
			hkpRigidBody* rb = hkGetRigidBody(raycastOut.m_rootCollidable);
			if (rb && rb->isFixedOrKeyframed())
			{
				HK_SET_OBJECT_COLOR((hkUlong)raycastOut.m_rootCollidable, TRANSPARENT_GREY );

				m_transparentObjects.pushBack((hkUlong)raycastOut.m_rootCollidable);
			}
		}
	}

	hkgWindow* w = env->m_window;
	HK_ASSERT(0, w != HK_NULL );

	env->m_window->getContext()->lock();
	for(int i = 0; i < w->getNumViewports(); ++i)
	{
		hkgCamera* c = w->getViewport(i)->getCamera();

		float upN[3];
		hkgVec3Copy(upN, &m_characterRb->m_up(0));
		hkgVec3Normalize(upN);
		// set up camera
		c->setFrom(&from(0));
		c->setTo(&to(0));
		c->setUp(upN);
		c->orthogonalize();
		c->computeModelView();
		c->computeProjection();
		//c->setHandednessMode(HKG_CAMERA_HANDEDNESS_MODE( rightHanded ? HKG_CAMERA_HANDEDNESS_RIGHT : HKG_CAMERA_HANDEDNESS_LEFT) );
		w->getViewport(i)->setFlyModeUp( upN );
	}
	env->m_window->getContext()->unlock();
}

/*
* Havok SDK - NO SOURCE PC DOWNLOAD, BUILD(#20080529)
*
* Confidential Information of Havok.  (C) Copyright 1999-2008 
* Telekinesys Research Limited t/a Havok. All Rights Reserved. The Havok
* Logo, and the Havok buzzsaw logo are trademarks of Havok.  Title, ownership
* rights, and intellectual property rights in the Havok software remain in
* Havok and/or its suppliers.
*
* Use of this software for evaluation purposes is subject to and indicates 
* acceptance of the End User licence Agreement for this product. A copy of 
* the license is included with this software and is also available at
* www.havok.com/tryhavok
*
*/
