//
// Tools_RagdollJoint_Object.cpp
//
// Created by KJFL - 27 October 2008
//
// Copyright (C) 2008 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

#include "Ragdoll/Tools_RagdollJoint_Object.h"

#include "Interface/Asura_Dialog.h"
#include "Interface/Asura_Overlay.h"
#include "Interface/Asura_Overlay_System.h"
#include "Physics/Asura_Physics_Ragdoll_Description.h"
#include "Rendering/Asura_Render.h"
#include "Rendering/Asura_Text_Output.h"


// Tools
#include "Misc/Tools_Render.h"
#include "Objects/Tools_Object_System.h"
#include "Objects/Tools_Object_Gizmo.h"
#include "Ragdoll/Tools_Mode_RagdollEditor.h"
#include "Ragdoll/Tools_RagdollCollision_Object.h"

Asura_List<Tools_RagdollJoint_Object> Tools_RagdollJoint_Object::s_xList;
Asura_Vector_2 Tools_RagdollJoint_Object::s_xPropertiesOverlayPosition(30,10);
Tools_RagdollJoint_Object* Tools_RagdollJoint_Object::s_pxCurrentlyEditedObject=NULL;

static const float fRADIUS = 0.03f;

static float ToRadianHalfAngle( float f )
{
	return Asura_Maths::DegToRad( f )/2.0f;
}
static float FromRadianHalfAngle( float f )
{
	return Asura_Maths::RadToDeg(f)*2.0f;
}

Tools_RagdollJoint_Object::Tools_RagdollJoint_Object( Asura_Physics_Ragdoll_Desc_Joint* pxJoint, const char* szNodeName )
:Tools_Object(TOT_RagdollJoint)
, m_pxJoint( pxJoint )
, m_pxCol1Dialog( 0 )
, m_pxCol2Dialog( 0 )
{
	m_xName = "Joint_";
	m_xName += szNodeName;

	m_fTwistLimit = FromRadianHalfAngle( pxJoint->GetTwistLimit() );
	m_fSwingLimitX = FromRadianHalfAngle( pxJoint->GetSwingLimitX() );
	m_fSwingLimitY = FromRadianHalfAngle( pxJoint->GetSwingLimitY() );
	m_bAnglesModified = false;

	s_xList.AddToList( this );

	for ( u_int u=0; u<3; ++u )
	{
		m_apxRotateGizmos[u] = 0;
	}
}

Tools_RagdollJoint_Object::~Tools_RagdollJoint_Object()
{
	s_xList.RemoveFromListIfPresent( this );
	
	Tools_Mode_RagdollEditor::DeleteJointObjectCallback( this );
}

const char* Tools_RagdollJoint_Object::GetName( void )
{
	return m_xName.GetString();
}

void Tools_RagdollJoint_Object::UpdatePosition( const Asura_Vector_3& xPos, const Asura_Matrix_3x3& xMat )
{
	Position = xPos;
	Orientation = xMat;

	m_pxJoint->SetJointOrientation( Asura_AMatrix3x3( Orientation ) );
}

void Tools_RagdollJoint_Object::Update( void )
{
	if (!m_pxJoint) return;

	if ( s_pxCurrentlyEditedObject != this ) return;

	if (m_bAnglesModified)
	{
		m_pxJoint->Tools_SetTwistLimit( ToRadianHalfAngle(m_fTwistLimit) );
		m_pxJoint->Tools_SetSwingLimitX( ToRadianHalfAngle(m_fSwingLimitX) );
		m_pxJoint->Tools_SetSwingLimitY( ToRadianHalfAngle(m_fSwingLimitY) );
	}

	m_pxJoint->SetJointOrientation( Asura_AMatrix3x3( Orientation ) );

	u_int uIndexA = m_pxJoint->GetObjectA();
	u_int uIndexB = m_pxJoint->GetObjectB();

	if ( !m_pxJoint->IsValid() ) return;

	Asura_AVector3 xPositionA, xPositionB;
	Asura_AMatrix3x3 xMatrixA, xMatrixB;

	const Tools_RagdollCollision_Object* pxObjectA = Tools_Mode_RagdollEditor::GetToolsCollisionObject( uIndexA );
	const Tools_RagdollCollision_Object* pxObjectB = Tools_Mode_RagdollEditor::GetToolsCollisionObject( uIndexB );

	pxObjectA->GetRagdollObjectOrientationAndPosition( xMatrixA, xPositionA );
	pxObjectB->GetRagdollObjectOrientationAndPosition( xMatrixB, xPositionB );

	// Need to find joint position in space of collision objects
	Asura_AVector3 xJointPosition(Position);
	Asura_AMatrix3x3 xJointOrientation( Orientation );
#if 1
	{
		xMatrixA.Transpose();
		Asura_AVector3 xRel = (xJointPosition - xPositionA) * xMatrixA;
		Asura_AMatrix3x3 xTransform = xJointOrientation * xMatrixA;

		GetJoint()->SetPositionRelativeToA( xRel );
		GetJoint()->SetJointSpaceRelativeToA( xTransform );
	}
#endif
#if 0
	{
		xMatrixB.Transpose();
		Asura_AVector3 xRel = (xJointPosition - xPositionB) * xMatrixB;
		Asura_AMatrix3x3 xTransform = xJointOrientation * xMatrixB;

		xRel = xRel * xTransform;
//		GetJoint()->SetPositionRelativeToB( xRel );
//		GetJoint()->SetJointSpaceRelativeToB( xTransform );
	}
	
#endif
	
}

void Tools_RagdollJoint_Object::Render( void )
{
	const Asura_Vector_3 xSizes( fRADIUS, fRADIUS, fRADIUS );

	// Render the node as a grey sphere if not selected
	u_int uNodeColour = 0xff3f3f3f;

	// Match what the Tools does for object selection - red for prime selection, blue for rest of a group selection
	if ( RenderAsSelected() )
	{
		uNodeColour = 0xffff0000;
		
		// KJFL - 15 January 2009 - TODO
		// beginnings of a cone renderer - sort of thing to move out to 
		// the new debugging primitive renderer when it's created.
		{
			Asura_AVector3 xPoint1( Position );
			
			float fR1 = Asura_Maths::Sine( m_pxJoint->GetSwingLimitX() );
			float fR2 = Asura_Maths::Sine( m_pxJoint->GetSwingLimitY() );

			Asura_AMatrix3x3 xOrient( Orientation );

			const u_int uNUMBER_OF_SUBDIVISIONS=32;
			Asura_AVector3 axPoints[uNUMBER_OF_SUBDIVISIONS];
			for ( u_int u=0; u<uNUMBER_OF_SUBDIVISIONS; ++u)
			{
				float fAngle = ((float)u/(float)uNUMBER_OF_SUBDIVISIONS)* 2.0f * Asura_Maths::Pi;

				float fX, fY;

				Asura_Maths::SinCos( fAngle, fY, fX );

				fX *= fR1;
				fY *= fR2;
				float fZ = Asura_Maths::Sqrt( 1.0f - fX*fX - fY*fY );

				Asura_AVector3 xDisp( fX, fY, fZ );

				xDisp *= 0.2f;

				axPoints[u] = xPoint1 + xDisp * xOrient;

				Asura_Render::RenderLine( xPoint1, axPoints[u], 0xffffff00 );
			}

			for ( u_int u=0; u<uNUMBER_OF_SUBDIVISIONS; ++u)
			{
				u_int v = (u+1)%uNUMBER_OF_SUBDIVISIONS;
				Asura_Render::RenderLine( axPoints[u], axPoints[v], 0xffffffff );
			}
		}

		// KJFL - 20 January 2009 - Twist representation
		{
			Asura_AVector3 xPoint1( Position );
			Asura_AMatrix3x3 xOrient( Orientation );

			const u_int uNUMBER_OF_SUBDIVISIONS=32;
			Asura_AVector3 axPoints[uNUMBER_OF_SUBDIVISIONS];
			for ( u_int u=0; u<uNUMBER_OF_SUBDIVISIONS; ++u)
			{
				float fAngle = (((float)u/(float)uNUMBER_OF_SUBDIVISIONS) - 0.5f)*m_pxJoint->GetTwistLimit()*2.0f;

				float fX, fY;

				Asura_Maths::SinCos( fAngle, fY, fX );

				Asura_AVector3 xDisp( fX, fY, 1.0f );
			
				xDisp *= 0.23f;

				axPoints[u] = xPoint1 + xDisp * xOrient;
			}
			
			for ( u_int u=0; u<uNUMBER_OF_SUBDIVISIONS-1; ++u)
			{
				u_int v = (u+1);
				Asura_Render::RenderLine( axPoints[u], axPoints[v], 0xff00ff00 );
			}
		}

	}
	else if (IsGroupSelected())
	{
		uNodeColour = 0xff0000ff;
	}
	
	Tools_Render::RenderSpheroid( Position, Orientation, xSizes, uNodeColour, true );
}

	

void Tools_RagdollJoint_Object::PrintInfo(float& x,float& y)
{
	Tools_Object::PrintInfo(x,y);
}

bool Tools_RagdollJoint_Object::IsSelectable()
{
	return true;
}

void Tools_RagdollJoint_Object::OnSelection()
{
	s_pxCurrentlyEditedObject = this;
	CreatePropertiesOverlay();

	for ( u_int u=0; u<3; ++u )
	{
		m_apxRotateGizmos[u] = new Tools_Object_RotateGizmo( this, u );
	}
}

void Tools_RagdollJoint_Object::OnDeselection()
{
	DeletePropertiesOverlay();
	s_pxCurrentlyEditedObject = NULL;

	for ( u_int u=0; u<3; ++u )
	{
		delete m_apxRotateGizmos[u];
		m_apxRotateGizmos[u] = 0;
	}
}

void Tools_RagdollJoint_Object::OnGroupDeselection()
{
	DeletePropertiesOverlay();
	s_pxCurrentlyEditedObject = NULL;
}

bool Tools_RagdollJoint_Object::ObjectIsInLOS(const Asura_Vector_3& xAlpha, const Asura_Vector_3& xBeta, float& fLowest)
{
	// Do LOS test against the sphere
	float fDist = fLowest;
	bool bInLOS = Asura_Maths::RayIntersectsSphere( xAlpha, xBeta, Position, fRADIUS, &fDist );
	if ( bInLOS )
	{
		fLowest = fDist;
	}												   

	return bInLOS;
}

void Tools_RagdollJoint_Object::UpdateDialogLabels( void )
{
	Asura_String xName1("Col1: ");
	Asura_String xName2("Col2: ");	

	Asura_Assert( m_pxJoint, "No associated Joint" );
	if (!m_pxJoint) return;

	xName1 += Tools_Mode_RagdollEditor::GetNameOfCollisionObject( m_pxJoint->GetObjectA() );
	xName2 += Tools_Mode_RagdollEditor::GetNameOfCollisionObject( m_pxJoint->GetObjectB() );
	
	m_pxCol1Dialog->UpdateLabel( xName1.GetString() );
	m_pxCol2Dialog->UpdateLabel( xName2.GetString() );
}

void Tools_RagdollJoint_Object::CreatePropertiesOverlay( void )
{
	Asura_Overlay* pxOverlay = new Asura_Overlay( GetName(), false, &s_xPropertiesOverlayPosition);
	pxOverlay->m_uID = TOOLS_OVERLAY_SELECTEDOBJECT;

	Asura_Dialog* pxDialog;
	Asura_Vector_2 xOffset = Asura_Vector_2(1.0f,3.0f);

	pxDialog = new Asura_Function_Dialog( "Set Collision Objects", xOffset, &Tools_RagdollJoint_Object::ChooseCollisionObject );
	pxOverlay->AddDialog( pxDialog );
	xOffset.y+=Asura_Dialog::LargeSpacing();

	m_pxCol1Dialog = new Asura_StringDisplay_Dialog( "", xOffset );
	pxOverlay->AddDialog( m_pxCol1Dialog );
	xOffset.y+=Asura_Dialog::DefaultSpacing();

	m_pxCol2Dialog = new Asura_StringDisplay_Dialog( "", xOffset );
	pxOverlay->AddDialog( m_pxCol2Dialog );
	xOffset.y+=Asura_Dialog::LargeSpacing();

	pxDialog = new Asura_Function_Dialog( "Revert Orientation", xOffset, &Tools_RagdollJoint_Object::RevertOrientation );
	pxOverlay->AddDialog( pxDialog );
	xOffset.y+=Asura_Dialog::LargeSpacing();
	
	pxDialog = new Asura_Numerical_Dialog( "Twist Limit", xOffset, &m_fTwistLimit, 0.0f, 170.0f, 1.0f, "%1.0f", &m_bAnglesModified );
	pxOverlay->AddDialog( pxDialog );
	xOffset.y+=Asura_Dialog::DefaultSpacing();

	pxDialog = new Asura_Numerical_Dialog( "Swing Limit X", xOffset, &m_fSwingLimitX, 0.0f, 170.0f, 1.0f, "%1.0f", &m_bAnglesModified );
	pxOverlay->AddDialog( pxDialog );
	xOffset.y+=Asura_Dialog::DefaultSpacing();

	pxDialog = new Asura_Numerical_Dialog( "Swing Limit Y", xOffset, &m_fSwingLimitY, 0.0f, 170.0f, 1.0f, "%1.0f", &m_bAnglesModified );
	pxOverlay->AddDialog( pxDialog );
	xOffset.y+=Asura_Dialog::DefaultSpacing();

	UpdateDialogLabels();

 	Asura_Overlay_System::SubmitNewOverlay(pxOverlay,false);
	
}

void Tools_RagdollJoint_Object::DeletePropertiesOverlay( void )
{
	m_pxCol1Dialog = 0;
	m_pxCol2Dialog = 0;
	Asura_Overlay_System::CloseOverlays(TOOLS_OVERLAY_SELECTEDOBJECT);
}

void Tools_RagdollJoint_Object::ChooseCollisionObject( void )
{
	Tools_JointChooseCollisionObject* pxChooseObject = new Tools_JointChooseCollisionObject( s_pxCurrentlyEditedObject );

	Tools_Object_System::ChooseObject( "Please choose TWO collision objects to attach to this joint.", pxChooseObject );
}

void Tools_RagdollJoint_Object::RevertOrientation( void )
{
	if (s_pxCurrentlyEditedObject)
	{
		Asura_Physics_Ragdoll_Desc_Joint* pxJoint = s_pxCurrentlyEditedObject->GetJoint();

		pxJoint->SetJointOrientation( pxJoint->GetOriginalJointOrientation() );
		pxJoint->GetOriginalJointOrientation().ToMat3( s_pxCurrentlyEditedObject->Orientation );
	}
}

bool Tools_JointChooseCollisionObject::CanBeSelected( Tools_Object* pxObject )
{
	if (!pxObject) return false;

	return ( pxObject->Type == TOT_RagdollCollision );
}

void Tools_JointChooseCollisionObject::ObjectSelected( Tools_Object* pxObject )
{
	if (!pxObject) return;

	const List<Tools_Object*>* pxList = GetSelectionList();

	if (!pxList) return;

	if ( pxList->size() !=2 )
	{
		// TODO - warning message about needing 2 collision objects
		return;
	}

	Tools_Object* pxObjectA = pxList->first_entry();
	Tools_Object* pxObjectB = pxList->last_entry();

	if ( pxObjectA == pxObjectB || pxObjectA == NULL || pxObjectB == NULL )
	{
		return;
	}
	if ( pxObjectA->Type != TOT_RagdollCollision )
	{
		return;
	}
	if ( pxObjectB->Type != TOT_RagdollCollision )
	{
		return;
	}

	Tools_RagdollCollision_Object* pxColA = reinterpret_cast<Tools_RagdollCollision_Object*>(pxObjectA);
	Tools_RagdollCollision_Object* pxColB = reinterpret_cast<Tools_RagdollCollision_Object*>(pxObjectB);

	// We always want ObjectA do be higher in the hierarchy than ObjectB
	if ( pxColA->GetCollisionObject()->GetHierarchyNode() > pxColB->GetCollisionObject()->GetHierarchyNode() )
	{
		Tools_RagdollCollision_Object* pxSwapTemp = pxColA;
		pxColA = pxColB;
		pxColB = pxSwapTemp;
	}

	{
		u_int uIndex = pxColA->GetCollisionObjectIndex();
	
		Asura_AVector3 xPosition;
		Asura_AMatrix3x3 xMatrix;
	
		pxColA->GetRagdollObjectOrientationAndPosition( xMatrix, xPosition );
	
		m_pxJoint->GetJoint()->SetObjectA( uIndex );
		
		// Need to find joint position in space of collision objects
		Asura_AVector3 xJointPosition(m_pxJoint->Position);
		Asura_AMatrix3x3 xJointOrientation = m_pxJoint->GetJoint()->GetJointOrientation();

		xMatrix.Transpose();
		Asura_AVector3 xRel = (xJointPosition - xPosition) * xMatrix;
		xJointOrientation = xJointOrientation * xMatrix;

		m_pxJoint->GetJoint()->SetPositionRelativeToA( xRel );
		m_pxJoint->GetJoint()->SetJointSpaceRelativeToA( xJointOrientation );
	}
	{
		u_int uIndex = pxColB->GetCollisionObjectIndex();
	
		Asura_AVector3 xPosition;
		Asura_AMatrix3x3 xMatrix;
	
		pxColB->GetRagdollObjectOrientationAndPosition( xMatrix, xPosition );
	
		m_pxJoint->GetJoint()->SetObjectB( uIndex );
		
		// Need to find joint position in space of collision objects
		Asura_AVector3 xJointPosition(m_pxJoint->Position);
		Asura_AMatrix3x3 xJointOrientation = m_pxJoint->GetJoint()->GetOriginalJointOrientation();

		xMatrix.Transpose();
		Asura_AVector3 xRel = (xJointPosition - xPosition) * xMatrix;
		xJointOrientation = xJointOrientation * xMatrix;

		m_pxJoint->GetJoint()->SetPositionRelativeToB( xRel );
		m_pxJoint->GetJoint()->SetJointSpaceRelativeToB( xJointOrientation );
	}
	
	m_pxJoint->UpdateDialogLabels();
	
}
