#include "Mix/Tool/Win32/Core/Dynamics/Design/Part.h"

#include "Mix/Tool/Win32/Core/Dynamics/World.h"
#include "Mix/Tool/Win32/Core/Dynamics/Joint.h"
#include "Mix/Tool/Win32/Core/Dynamics/RigidBody.h"
#include "Mix/Tool/Win32/Core/Dynamics/Design/Actor.h"

namespace Mix{ namespace Tool{ namespace Win32{ namespace Dynamics{ namespace Design{

Part::Part(	Actor* pSubject,
			Part* pParent,
			Basic::COORDINATE_SYSTEM coordinateSystem,
			const D3DXVECTOR3& localPos,
			const D3DXMATRIX& worldMat,
			const D3DXQUATERNION& centerRot,
			const D3DXVECTOR3& centerPos ) : Basic( pSubject, coordinateSystem, localPos, worldMat, centerRot, centerPos ),
m_pParent( pParent ),
m_pJoint( NULL ),
m_JointLocalPivotAPos( 0.0f, 0.0f, 0.0f )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ローカル行列の作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMATRIX invParentWorldMat;

	if( m_pParent != NULL )
	{
		D3DXMatrixInverse( &invParentWorldMat, NULL, &( m_pParent->GetWorldMatrix() ) );
	}
	else
	{
		D3DXMatrixInverse( &invParentWorldMat, NULL, &( m_pSubject->GetWorldMatrix() ) );
	}

	D3DXMatrixMultiply( &m_InitalLocalMat, &GetWorldMatrix(), &invParentWorldMat );

	D3DXMatrixIdentity( &m_JointLocalPivotBMat );
}

Part::~Part( void )
{
	Mix::Tool::Win32::Dynamics::World* pWorld = GetWorldPtr();

	for( std::vector<Part*>::iterator it = m_ChildList.begin(); it != m_ChildList.end(); ++it )
	{
		MIX_DELETE( ( *it ) );
	}

	if( ( pWorld != NULL ) &&
		( m_pJoint != NULL ) )
	{
		pWorld->RemoveJoint( m_pJoint );
	}
}

void Part::OnWorldChanged( const Basic::WORLD_CHANGED_EVENT_ARGS& args )
{
	Part::ChildList::iterator it_child_begin = m_ChildList.begin();
	Part::ChildList::iterator it_child_end = m_ChildList.end();
	Part::ChildList::iterator it_child;

	////////////////////////////////////////////////////////////////////////////////////////////////////

	Basic::OnWorldChanged( args );

	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( ( args.pOldWorld != args.pNewWorld ) &&
		( GetMode() != Basic::EDIT ) )
	{
		if( args.pOldWorld != NULL )
		{
			args.pOldWorld->RemoveJoint( m_pJoint );
		}

		if( args.pNewWorld != NULL )
		{
			args.pNewWorld->AddJoint( m_pJoint );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( it_child = it_child_begin; it_child != it_child_end; ++it_child )
	{
		Part* pChild = ( *it_child );
		pChild->OnWorldChanged( args );
	}
}

void Part::OnModeChanged( const Basic::MODE_CHANGED_EVENT_ARGS& args )
{
	Mix::Tool::Win32::Dynamics::World* pWorld = GetWorldPtr();

	Part::ChildList::iterator it_child_begin = m_ChildList.begin();
	Part::ChildList::iterator it_child_end = m_ChildList.end();
	Part::ChildList::iterator it_child;

	////////////////////////////////////////////////////////////////////////////////////////////////////

	Basic::OnModeChanged( args );

	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( ( pWorld != NULL ) &&
		( m_pJoint != NULL ) )
	{
		if( ( args.oldMode != Basic::EDIT ) &&
			( args.newMode == Basic::EDIT ) )
		{
			pWorld->RemoveJoint( m_pJoint );
		}
		else if(	( args.oldMode == Basic::EDIT ) &&
					( args.newMode != Basic::EDIT ) )
		{
			pWorld->AddJoint( m_pJoint );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( it_child = it_child_begin; it_child != it_child_end; ++it_child )
	{
		Part* pChild = ( *it_child );
		pChild->OnModeChanged( args );
	}
}

void Part::OnColliderChanged( void )
{
	Part::ChildList::iterator it_child_begin = m_ChildList.begin();
	Part::ChildList::iterator it_child_end = m_ChildList.end();
	Part::ChildList::iterator it_child;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Basic を処理
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Basic::OnColliderChanged();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ジョイントのフレームを再設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_pJoint != NULL )
	{
		Joint_UpdateFrame();
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 子を処理
	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( it_child = it_child_begin; it_child != it_child_end; ++it_child )
	{
		Part* pChild = ( *it_child );
		pChild->OnColliderChanged();
	}
}

void Part::OnRefresh( void )
{
	Part::ChildList::iterator it_child_begin = m_ChildList.begin();
	Part::ChildList::iterator it_child_end = m_ChildList.end();
	Part::ChildList::iterator it_child;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Basic を処理
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Basic::OnRefresh();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 親のボディが無効な場合の ピボットB の設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( ( GetMode() != Basic::EDIT ) &&
		( m_pJoint != NULL ) &&
		( m_pJoint->GetRigidBodyA() != NULL ) &&
		( m_pJoint->GetRigidBodyB() == NULL ) )
	{
#if 1
		D3DXMATRIX pivotBMat;

		if( m_pParent != NULL )
		{
			D3DXMatrixMultiply( &pivotBMat, &m_JointLocalPivotBMat, &( m_pParent->GetWorldMatrix() ) );
		}
		else
		{
			D3DXMatrixMultiply( &pivotBMat, &m_JointLocalPivotBMat, &( m_pSubject->GetWorldMatrix() ) );
		}

		m_pJoint->SetPivotB( D3DXVECTOR3( pivotBMat._41, pivotBMat._42, pivotBMat._43 ) );
#else
		D3DXVECTOR3 pivotB;

		if( m_pParent != NULL )
		{
			D3DXVec3TransformCoord( &pivotB, &m_LocalPos, &( m_pParent->GetWorldMatrix() ) );
		}
		else
		{
			D3DXVec3TransformCoord( &pivotB, &m_LocalPos, &( m_pSubject->GetWorldMatrix() ) );
		}

		m_pJoint->SetPivotB( pivotB );
#endif
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 子を処理
	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( it_child = it_child_begin; it_child != it_child_end; ++it_child )
	{
		Part* pChild = ( *it_child );
		pChild->OnRefresh();
	}
}

bool Part::OnRefreshWorldTransform( D3DXMATRIX& worldMat )
{
	//自身のボディが無効な場合の子のジョイントフレームの ピボットB の設定用に初期ワールドトランスフォームを求める

	if( m_pParent != NULL )
	{
		D3DXMatrixMultiply( &worldMat, &m_InitalLocalMat, &( m_pParent->GetWorldMatrix() ) );
	}
	else
	{
		D3DXMatrixMultiply( &worldMat, &m_InitalLocalMat, &( m_pSubject->GetWorldMatrix() ) );
	}

	return true;
/*
	if( m_pParent == NULL )
	{
		return GetWorldMatrix();
	}

	D3DXMATRIX tempMat;

	return *D3DXMatrixMultiply( &tempMat, &m_InitalLocalMat, &( m_pParent->GetWorldMatrix() ) );
*/
}

Part* Part::GetParentPtr( void )
{
	return m_pParent;
}

Part* Part::AddChild(	const D3DXVECTOR3& localPos,
						const D3DXMATRIX& worldMat,
						const D3DXQUATERNION& centerRot,
						const D3DXVECTOR3& centerPos )
{
	Part* pChildPartDesigner = new Mix::Tool::Win32::Dynamics::Design::Part( m_pSubject, this, m_CoordinateSystem, localPos, worldMat, centerRot, centerPos );

	if( pChildPartDesigner != NULL )
	{
		m_ChildList.push_back( pChildPartDesigner );
		pChildPartDesigner->OnWorldChanged( Basic::WORLD_CHANGED_EVENT_ARGS( NULL, GetWorldPtr() ) );
	}

	return pChildPartDesigner;
}

Part* Part::AddChild(	const D3DXVECTOR3& localPos,
						const D3DXQUATERNION& worldRot,
						const D3DXVECTOR3& worldPos,
						const D3DXQUATERNION& centerRot,
						const D3DXVECTOR3& centerPos )
{
	D3DXMATRIX worldMat;

	D3DXMatrixRotationQuaternion( &worldMat, &worldRot );

	worldMat._41 = worldPos.x;
	worldMat._42 = worldPos.y;
	worldMat._43 = worldPos.z;

	return AddChild( localPos, worldMat, centerRot, centerPos );
}

int Part::GetChildCount( void ) const
{
	return static_cast<int>( m_ChildList.size() );
}

Part* Part::GetChildPtr( int index )
{
	if( ( index < 0 ) ||
		( static_cast<int>( m_ChildList.size() ) <= index ) )
	{
		return NULL;
	}

	return m_ChildList[index];
}

Mix::Tool::Win32::Dynamics::Joint* Part::Joint_Get( void ) const
{
	return m_pJoint;
}

bool Part::Joint_Set( Mix::Tool::Win32::Dynamics::Joint* pJoint )
{
	if( m_pJoint == pJoint )
	{
		return true;
	}

	Mix::Tool::Win32::Dynamics::World* pWorld = GetWorldPtr();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 直前のジョイントをワールドから外す
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( ( pWorld != NULL ) &&
		( m_pJoint != NULL ) &&
		( GetMode() != Basic::EDIT ) )
	{
		pWorld->RemoveJoint( m_pJoint );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 新規のジョイントを設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pJoint = pJoint;

	if( m_pJoint != NULL )
	{
		Joint_UpdateFrame();

		if( ( pWorld != NULL ) &&
			( GetMode() != Basic::EDIT ) )
		{
			pWorld->AddJoint( m_pJoint );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// サブジェクトに対して、キャストモーションの更新を要求
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pSubject->UpdateCastMotionPart();

	////////////////////////////////////////////////////////////////////////////////////////////////////

	return true;
}

const D3DXVECTOR3& Part::Joint_GetLocalPivotA( void ) const
{
	return m_JointLocalPivotAPos;
}

void Part::Joint_SetLocalPivotA( const D3DXVECTOR3& localPivot )
{
	m_JointLocalPivotAPos = localPivot;

	Joint_UpdateFrame();
}

const D3DXMATRIX& Part::Joint_GetLocalPivotB( void ) const
{
	return m_JointLocalPivotBMat;
}

void Part::Draw(	Mix::Tool::Win32::Graphics::LineHelper* pLineHelper,
							bool bSensor,
							bool bBody,
							bool bJoint,
							const D3DXVECTOR4& selectedColor,
							const D3DXVECTOR4& sensorColor,
							const D3DXVECTOR4& bodyColor,
							const D3DXVECTOR4& jointAxisColor,
							const D3DXVECTOR4& jointBallColor,
							const D3DXVECTOR4& jointLimitColor,
							float jointScale,
							float axisScale,
							bool bProcChild )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ベーシック
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Basic::Draw(	pLineHelper,
					bSensor,
					bBody,
					selectedColor,
					sensorColor,
					bodyColor,
					axisScale );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ジョイント
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( ( bJoint == true ) &&
		( m_pJoint != NULL ) )
	{
		m_pJoint->Draw(	pLineHelper,
						Mix::Tool::Win32::Lerp_B( jointAxisColor, selectedColor ),
						Mix::Tool::Win32::Lerp_B( jointBallColor, selectedColor ),
						Mix::Tool::Win32::Lerp_B( jointLimitColor, selectedColor ),
						jointScale );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 子を処理
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( bProcChild == true )
	{
		Part::ChildList::iterator it_child_begin = m_ChildList.begin();
		Part::ChildList::iterator it_child_end = m_ChildList.end();
		Part::ChildList::iterator it_child;

		for( it_child = it_child_begin; it_child != it_child_end; ++it_child )
		{
			Part* pChild = ( *it_child );

			pChild->Draw(	pLineHelper,
							bSensor,
							bBody,
							bJoint,
							selectedColor,
							sensorColor,
							bodyColor,
							jointAxisColor,
							jointBallColor,
							jointLimitColor,
							jointScale,
							axisScale, true );
		}
	}
}

void Part::Joint_UpdateFrame( void )
{
	if( Collider_IsAvailable() )
	{
		D3DXVECTOR3 pivotA = Joint_ComputePivot( this );
		Mix::Tool::Win32::Dynamics::Design::Part* pParent = m_pParent;

		while(	( pParent != NULL ) &&
				( pParent->Collider_IsAvailable() == false ) )
		{
			pParent = pParent->GetParentPtr();
		}

		if( pParent != NULL )
		{
			D3DXVECTOR3 pivotB = Joint_ComputePivot( pParent );

			D3DXMatrixIdentity( &m_JointLocalPivotBMat );

			m_pJoint->SetInitalRigidBodyTransform(	this->Collider_GetInitalRotation(), this->Collider_GetInitalPosition(),
													pParent->Collider_GetInitalRotation(), pParent->Collider_GetInitalPosition() );

			m_pJoint->SetFrame( this->Collider_GetRigidBodyPtr(), pParent->Collider_GetRigidBodyPtr(), pivotA, pivotB );
		}
		else if(	( m_pSubject != NULL ) &&
					( m_pSubject->Collider_IsAvailable() == true ) )
		{
			D3DXVECTOR3 pivotB = Joint_ComputePivot( m_pSubject );

			D3DXMatrixIdentity( &m_JointLocalPivotBMat );

			m_pJoint->SetInitalRigidBodyTransform(	this->Collider_GetInitalRotation(), this->Collider_GetInitalPosition(),
													m_pSubject->Collider_GetInitalRotation(), m_pSubject->Collider_GetInitalPosition() );

			m_pJoint->SetFrame( this->Collider_GetRigidBodyPtr(), m_pSubject->Collider_GetRigidBodyPtr(), pivotA, pivotB );
		}
		else
		{
			if( m_pParent != NULL )
			{
				m_JointLocalPivotBMat = Joint_ComputeLocalPivotMatrix( m_pParent );
			}
			else
			{
				m_JointLocalPivotBMat = Joint_ComputeLocalPivotMatrix( m_pSubject );
			}

			m_pJoint->SetInitalRigidBodyTransform(	this->Collider_GetInitalRotation(), this->Collider_GetInitalPosition(),
													this->Collider_GetInitalRotation(), this->Collider_GetInitalPosition() );

			m_pJoint->SetFrame( this->Collider_GetRigidBodyPtr(), pivotA );
		}
	}
}

D3DXMATRIX Part::Joint_ComputeLocalPivotMatrix( Mix::Tool::Win32::Dynamics::Design::Basic* pBase )
{
	D3DXMATRIX tempMat;
	D3DXMATRIX invBaseWorldMat;
	D3DXMATRIX localPivotMat;

	D3DXMatrixInverse( &invBaseWorldMat, NULL, &( pBase->m_InitalWorldMat ) );

	D3DXMatrixTranslation( &localPivotMat, m_JointLocalPivotAPos.x, m_JointLocalPivotAPos.y, m_JointLocalPivotAPos.z );
	D3DXMatrixMultiply( &tempMat, &localPivotMat, &m_InitalWorldMat );
	D3DXMatrixMultiply( &localPivotMat, &tempMat, &invBaseWorldMat );

	return localPivotMat;
}

D3DXVECTOR3 Part::Joint_ComputePivot( Mix::Tool::Win32::Dynamics::Design::Basic* pBase )
{
	D3DXVECTOR3 pivot;

	D3DXMATRIX tempMat;
	D3DXMATRIX invBodyMat;
	D3DXMATRIX pivotMat;

	D3DXMatrixMultiply( &tempMat, &( pBase->Collider_GetCenterMatrix() ), &( pBase->m_InitalWorldMat ) );
	D3DXMatrixInverse( &invBodyMat, NULL, &tempMat );

#if 1

	D3DXMatrixTranslation( &pivotMat, m_JointLocalPivotAPos.x, m_JointLocalPivotAPos.y, m_JointLocalPivotAPos.z );
	D3DXMatrixMultiply( &tempMat, &pivotMat, &m_InitalWorldMat );
	D3DXMatrixMultiply( &pivotMat, &tempMat, &invBodyMat );

	pivot.x = pivotMat._41 * pBase->m_WorldS.x;
	pivot.y = pivotMat._42 * pBase->m_WorldS.y;
	pivot.z = pivotMat._43 * pBase->m_WorldS.z;

#else

	D3DXVec3TransformCoord( &pivot, &D3DXVECTOR3( m_InitalWorldMat._41, m_InitalWorldMat._42, m_InitalWorldMat._43 ), &invBodyMat );

	pivot.x *= pBase->m_WorldS.x;
	pivot.y *= pBase->m_WorldS.y;
	pivot.z *= pBase->m_WorldS.z;

#endif

	return pivot;
}

}}}}}
