#include "Mix/Private/Dynamics/Utility.h"
#include "Mix/Graphics/Utility/IPerspectiveRenderer.h"

////////////////////////////////////////////////////////////////////////////////////////////////////
// ユーティリティ
////////////////////////////////////////////////////////////////////////////////////////////////////

namespace Mix{ namespace Dynamics{

	Mix::Quaternion ToMixQuaternion( const btQuaternion& quat )
	{
		return Mix::Quaternion( quat.x(), quat.y(), quat.z(), quat.w() );
	}

	Mix::Vector3 ToMixVector3( const btVector3& vec )
	{
		return Mix::Vector3( vec.x(), vec.y(), vec.z() );
	}

	Mix::Matrix4x4 ToMixMatrix4x4( const btTransform& tr )
	{
		return Mix::Matrix4x4( ToMixQuaternion( tr.getRotation() ), ToMixVector3( tr.getOrigin() ) );
	}

	btQuaternion ToBulletQuaternion( const Mix::Quaternion& quat )
	{
		return btQuaternion( quat.x, quat.y, quat.z, quat.w );
	}

	btVector3 ToBulletVector3( const Mix::Vector3& vec )
	{
		return btVector3( vec.x, vec.y, vec.z );
	}

}}

////////////////////////////////////////////////////////////////////////////////////////////////////
// ユーティリティ : デバッグ
////////////////////////////////////////////////////////////////////////////////////////////////////

namespace Mix{ namespace Dynamics{ namespace Debug{

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// カラー
	////////////////////////////////////////////////////////////////////////////////////////////////////

	static Mix::Vector4 g_Colors[Mix::Dynamics::DDC_MAX] =
	{
		Mix::Vector4( 1.0f, 0.0f, 0.0f, 1.0f ), //DDC_RIGIDBODY_AROUSAL
		Mix::Vector4( 1.0f, 1.0f, 0.0f, 0.8f ), //DDC_RIGIDBODY_ACTIVE
		Mix::Vector4( 0.0f, 1.0f, 0.0f, 0.6f ), //DDC_RIGIDBODY_SLEEPY
		Mix::Vector4( 0.0f, 1.0f, 1.0f, 0.4f ), //DDC_RIGIDBODY_REST
		Mix::Vector4( 0.0f, 0.0f, 1.0f, 0.4f ), //DDC_RIGIDBODY_DISABLE

		Mix::Vector4( 1.0f, 0.5f, 0.5f, 0.8f ), //DDC_SENSOR

		Mix::Vector4( 1.0f, 1.0f, 0.5f, 0.8f ), //DDC_KINEMATIC_CHARACTER

		Mix::Vector4( 1.0f, 0.5f, 0.0f, 0.8f ), //DDC_JOINT_FRAME
		Mix::Vector4( 0.5f, 0.5f, 1.0f, 0.8f ), //DDC_JOINT_LIMIT

	};

	static const Mix::Vector4 g_UnknownColor( 0.1f, 0.1f, 0.1f, 1.0f );

	const Mix::Vector4& GetColor( Mix::Dynamics::DEBUG_DRAW_COLOR_TYPE type )
	{
		return g_Colors[type];
	}

	void SetColor( Mix::Dynamics::DEBUG_DRAW_COLOR_TYPE type, const Mix::Vector4& color )
	{
		g_Colors[type] = color;
	}

	Mix::Vector4 GetColor( Mix::Dynamics::DEBUG_DRAW_COLOR_TYPE type, Float32 opacity )
	{
		const Mix::Vector4& color = g_Colors[type];
		return Mix::Vector4( color.r, color.g, color.b, color.a * opacity );
	}

	Mix::Vector4 GetRigidBodyColor( btRigidBody* pRigidBody, Float32 opacity )
	{
		Mix::Vector4 color;

		switch( pRigidBody->getActivationState() )
		{
		case DISABLE_DEACTIVATION:
			color = g_Colors[Mix::Dynamics::DDC_RIGIDBODY_AROUSAL];
			break;
		case ACTIVE_TAG:
			color = g_Colors[Mix::Dynamics::DDC_RIGIDBODY_ACTIVE];
			break;
		case WANTS_DEACTIVATION:
			color = g_Colors[Mix::Dynamics::DDC_RIGIDBODY_SLEEPY];
			break;
		case ISLAND_SLEEPING:
			color = g_Colors[Mix::Dynamics::DDC_RIGIDBODY_REST];
			break;
		case DISABLE_SIMULATION:
			color = g_Colors[Mix::Dynamics::DDC_RIGIDBODY_DISABLE];
			break;

		default:
			color = g_UnknownColor;
		}

		color.a *= opacity;

		return color;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 描画
	////////////////////////////////////////////////////////////////////////////////////////////////////

	void DrawPivot( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
					const btTypedConstraint* pConstraint,
					const btVector3& pivotA,
					const btVector3& pivotB,
					Float32 minSize )
	{
		const btRigidBody* pRigidBodyA = &( pConstraint->getRigidBodyA() );
		const btRigidBody* pRigidBodyB = &( pConstraint->getRigidBodyB() );

		Mix::Vector3 pointA;
		Mix::Vector3 pointB;

		if( pRigidBodyA != &( btTypedConstraint::getFixedBody() ) )
		{
			btTransform tr;
			btQuaternion rot;
			btVector3 pos;

			Mix::Matrix4x4 mat;

			if( pRigidBodyA->isKinematicObject() == true )
			{
				pRigidBodyA->getMotionState()->getWorldTransform( tr );
			}
			else
			{
				tr = pRigidBodyA->getWorldTransform();
			}

			rot = tr.getRotation();
			pos = tr.getOrigin();

			mat.SetRotation( Mix::Quaternion( rot.x(), rot.y(), rot.z(), rot.w() ) );
			mat.m30 = pos.x();
			mat.m31 = pos.y();
			mat.m32 = pos.z();
			mat.m33 = 1.0f;

			pointA = mat * ToMixVector3( pivotA );
		}
		else
		{
			pointA = ToMixVector3( pivotA );
		}

		if( pRigidBodyB != &( btTypedConstraint::getFixedBody() ) )
		{
			btTransform tr;
			btQuaternion rot;
			btVector3 pos;

			Mix::Matrix4x4 mat;

			if( pRigidBodyB->isKinematicObject() == true )
			{
				pRigidBodyB->getMotionState()->getWorldTransform( tr );
			}
			else
			{
				tr = pRigidBodyB->getWorldTransform();
			}

			rot = tr.getRotation();
			pos = tr.getOrigin();

			mat.SetRotation( Mix::Quaternion( rot.x(), rot.y(), rot.z(), rot.w() ) );
			mat.m30 = pos.x();
			mat.m31 = pos.y();
			mat.m32 = pos.z();
			mat.m33 = 1.0f;

			pointB = mat * ToMixVector3( pivotB );
		}
		else
		{
			pointB = ToMixVector3( pivotB );
		}

		pPerspectiveRenderer->AddSphere( ( pointA + pointB ) * 0.5f, minSize + ( pointA - pointB ).GetLength() * 0.5f );
	}

	void DrawBox(	Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
					const btVector3& bbMin,
					const btVector3& bbMax )
	{
		Mix::Geometry::AABB aabb( ToMixVector3( bbMin ), ToMixVector3( bbMax ) );

		aabb.ComputePoints();
		pPerspectiveRenderer->AddBox( aabb );
	}

	void DrawArc(	Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
					const btVector3& center,
					const btVector3& normal,
					const btVector3& axis,
					Float32 radiusA,
					Float32 radiusB,
					Float32 minAngle,
					Float32 maxAngle,
					Float32 stepDegrees )
	{
		Boolean bDrawSect = ( minAngle <= maxAngle );

		if( bDrawSect == false )
		{
			minAngle = 0.0f;
			maxAngle = SIMD_2_PI;
		}

		DrawArc(	pPerspectiveRenderer,
					center,
					normal,
					axis,
					radiusA,
					radiusB,
					minAngle,
					maxAngle,
					bDrawSect,
					stepDegrees );
	}

	void DrawArc(	Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
					const btVector3& center,
					const btVector3& normal,
					const btVector3& axis,
					Float32 radiusA,
					Float32 radiusB,
					Float32 minAngle,
					Float32 maxAngle,
					Boolean bDrawSect,
					Float32 stepDegrees )
	{
		const btVector3& vx = axis;
		btVector3 vy = normal.cross( axis );
		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
		Int32 nSteps = static_cast<Int32>( ( maxAngle - minAngle ) / step );

		if( nSteps == 0 )
		{
			nSteps = 1;
		}

		btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);

		if( bDrawSect == True )
		{
			pPerspectiveRenderer->AddLine( ToMixVector3( center ), ToMixVector3( prev ) );
		}

		for( Int32 i = 1; i <= nSteps; i++ )
		{
			btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
			btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);

			pPerspectiveRenderer->AddLine( ToMixVector3( prev ), ToMixVector3( next ) );

			prev = next;
		}

		if( bDrawSect == True )
		{
			pPerspectiveRenderer->AddLine( ToMixVector3( center ), ToMixVector3( prev ) );
		}
	}

	void DrawSpherePatch(	Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer,
							const btVector3& center,
							const btVector3& up,
							const btVector3& axis,
							Float32 radius,
							Float32 minTh,
							Float32 maxTh,
							Float32 minPs,
							Float32 maxPs,
							Float32 stepDegrees ,
							Boolean bDrawCenter )
	{
		btVector3 vA[74];
		btVector3 vB[74];
		btVector3 *pvA = vA;
		btVector3 *pvB = vB;
		btVector3 *pT;
		btVector3 npole = center + up * radius;
		btVector3 spole = center - up * radius;
		btVector3 arcStart;
		btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
		const btVector3& kv = up;
		const btVector3& iv = axis;
		btVector3 jv = kv.cross(iv);
		Boolean drawN = False;
		Boolean drawS = False;

		if( minTh <= -SIMD_HALF_PI )
		{
			minTh = -SIMD_HALF_PI + step;
			drawN = True;
		}

		if( maxTh >= SIMD_HALF_PI )
		{
			maxTh = SIMD_HALF_PI - step;
			drawS = True;
		}

		if( minTh > maxTh )
		{
			minTh = -SIMD_HALF_PI + step;
			maxTh =  SIMD_HALF_PI - step;
			drawN = True;
			drawS = True;
		}

		Int32 n_hor = static_cast<Int32>( ( maxTh - minTh ) / step ) + 1;
		if( n_hor < 2 )
		{
			n_hor = 2;
		}

		btScalar step_h = ( maxTh - minTh ) / btScalar( n_hor - 1 );
		Boolean bClosed = False;

		if( minPs > maxPs )
		{
			minPs = -SIMD_PI + step;
			maxPs =  SIMD_PI;
			bClosed = True;
		}
		else if( ( maxPs - minPs ) >= SIMD_PI * btScalar( 2.f ) )
		{
			bClosed = True;
		}
		else
		{
			bClosed = False;
		}

		int n_vert = static_cast<int>( ( maxPs - minPs ) / step ) + 1;
		if( n_vert < 2 )
		{
			n_vert = 2;
		}

		btScalar step_v = ( maxPs - minPs ) / btScalar( n_vert - 1 );

		for( Int32 i = 0; i < n_hor; i++ )
		{
			btScalar th = minTh + btScalar( i ) * step_h;
			btScalar sth = radius * btSin( th );
			btScalar cth = radius * btCos( th );

			for( Int32 j = 0; j < n_vert; j++ )
			{
				btScalar psi = minPs + btScalar( j ) * step_v;
				btScalar sps = btSin(psi);
				btScalar cps = btCos(psi);

				pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;

				if( i > 0 )
				{
					pPerspectiveRenderer->AddLine( ToMixVector3( pvA[j] ), ToMixVector3( pvB[j] ) );
				}
				else if( drawS == True )
				{
					pPerspectiveRenderer->AddLine( ToMixVector3( spole ), ToMixVector3( pvB[j] ) );
				}

				if( j > 0 )
				{
					pPerspectiveRenderer->AddLine( ToMixVector3( pvB[j-1] ), ToMixVector3( pvB[j] ) );
				}
				else
				{
					arcStart = pvB[j];
				}

				if( ( i == ( n_hor - 1 ) ) &&
					( drawN == True ) )
				{
					pPerspectiveRenderer->AddLine( ToMixVector3( npole ), ToMixVector3( pvB[j] ) );
				}
				
				if( bDrawCenter == True )
				{
					if( bClosed == True )
					{
						if( j == ( n_vert - 1 ) )
						{
							pPerspectiveRenderer->AddLine( ToMixVector3( arcStart ), ToMixVector3( pvB[j] ) );
						}
					}
					else
					{
						if( ( ( i == 0 ) || ( i == ( n_hor  - 1 ) ) ) &&
							( ( j == 0 ) || ( j == ( n_vert - 1 ) ) ) )
						{
							pPerspectiveRenderer->AddLine( ToMixVector3( center ), ToMixVector3( pvB[j] ) );
						}
					}
				}
			}

			pT = pvA;
			pvA = pvB;
			pvB = pT;
		}
	}

}}}
