//
// Asura_Physics_ForceField_Conveyor.cpp
//
// Created by KJFL - 17 November 2006
//
// Copyright (C) 2006 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

// This file's header.
#include "Physics/ForceFields/Asura_Physics_ForceField_Conveyor.h"

#include "Debugging/Asura_DebugLineRender.h"

#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_Broadphase.h"
#include "Physics/Asura_Physics_RigidBody.h"

#include "Core/Asura_Timers.h"

Asura_Physics_ForceField_Conveyor::Asura_Physics_ForceField_Conveyor( const Asura_AVector3& xVelocity, const Asura_OBB& xVolume )
: m_xOBB( xVolume )
, m_xVelocity( xVelocity )
{
}

void Asura_Physics_ForceField_Conveyor::Update( void )
{
	Asura_Physics_Broadphase_Query xQuery;

	Asura_Bounding_Box xAABB;
	m_xOBB.CalculateEnclosingAABB( xAABB );

	// get me a list of Actors and Dynamics
	xQuery.MakeQuery( xAABB, false, false, true, true );

	// calc the displacement we'll give to each object
	const Asura_AVector3 xDisp = m_xVelocity*Asura_Physics_System::GetTimeSlice();

	for ( const Asura_Physics_Broadphase_Entry* pxEntry = xQuery.GetFirstEntry(); pxEntry; pxEntry = pxEntry->GetNextEntry() )
	{
		Asura_Physics_RigidBodyInstance* pxBody = pxEntry->GetInstance();

		// do a final OBB vs OBB test - previously it's only been AABB vs AABB etc.
		Asura_OBB xBodyOBB( pxBody->GetHalfExtents(), pxBody->GetCollisionMeshCentre(), pxBody->GetOrientation() );

		if ( xBodyOBB.Intersects( m_xOBB ) )
		{
			// move the object and make sure it's awake
			pxBody->SetPosition( pxBody->GetRenderingPosition() + xDisp );
			pxBody->WakeUp();
		}

	}
}


void Asura_Physics_ForceField_Conveyor::DebugRender( void )
{
#if !defined ASURA_NO_DEBUG_OUTPUT

	// Draw the OBB of the field - perhaps this code can be moved to Asura_DebugLineRender
	Asura_Bounding_Box xAABB;
	xAABB.MinX = -m_xOBB.GetHalfExtentX();
	xAABB.MaxX = +m_xOBB.GetHalfExtentX();
	xAABB.MinY = -m_xOBB.GetHalfExtentY();
	xAABB.MaxY = +m_xOBB.GetHalfExtentY();
	xAABB.MinZ = -m_xOBB.GetHalfExtentZ();
	xAABB.MaxZ = +m_xOBB.GetHalfExtentZ();

	Asura_Vector_3 xCentre( m_xOBB.GetCentre() );
	Asura_Matrix_3x3 xOrient;
	m_xOBB.GetOrientation().ToMat3( xOrient );

	Asura_DebugLineRender::AddBoundingBox( xAABB, xCentre, xOrient, 0xffff0000 );


	// Now draw lots of little lines inside the OBB moving in the direction of the field.
	// To avoid storing lots of data we'll use a random number generator with a fixed seed.
	// We may want to tone this down (or remove it!) if it costs too much renderwise.
	Asura_RandomGenerator xRandom( 0 );

	float fLineLength = 0.1f;
	u_int uNumberOfLines = 100; 

	// get this frame's offset for our lines
	Asura_AVector3 xOffset( m_xVelocity * Asura_Timers::GetLocalScaledTime() * 0.5f );

	// rotate it into the OBB's frame
	xOffset = m_xOBB.GetOrientation().ApplyTranspose( xOffset );

	Asura_AVector3 xHalfExtents = m_xOBB.GetHalfExtents();

	// make a little vector for each line segment
	Asura_Vector_3 xLineOffset( m_xVelocity );
	xLineOffset.Normalise();
	xLineOffset *= fLineLength;

	for (u_int u=0; u<uNumberOfLines; ++u)
	{
		float fX = xRandom.GetFloat() * m_xOBB.GetHalfExtentX() + xOffset.GetX();
		float fY = xRandom.GetFloat() * m_xOBB.GetHalfExtentY() + xOffset.GetY();
		float fZ = xRandom.GetFloat() * m_xOBB.GetHalfExtentZ() + xOffset.GetZ();

		fX = Asura_Maths::FloatMod( fX, m_xOBB.GetHalfExtentX() );
		fY = Asura_Maths::FloatMod( fY, m_xOBB.GetHalfExtentY() );
		fZ = Asura_Maths::FloatMod( fZ, m_xOBB.GetHalfExtentZ() );

		if (fX<0.0f) fX += m_xOBB.GetHalfExtentX();
		if (fY<0.0f) fY += m_xOBB.GetHalfExtentY();
		if (fZ<0.0f) fZ += m_xOBB.GetHalfExtentZ();

		Asura_AVector3 xPointInBox( fX, fY, fZ );

		xPointInBox = xPointInBox*2.0f - xHalfExtents;

		xPointInBox = xPointInBox * m_xOBB.GetOrientation() + m_xOBB.GetCentre();
		
		Asura_Vector_3 xStart( xPointInBox );
		u_int uColour = xRandom.GetIntU() | 0xff000000;

		Asura_DebugLineRender::AddLine( xStart, xStart+xLineOffset, uColour );
	}

#endif // !defined ASURA_NO_DEBUG_OUTPUT
}

const Asura_OBB& Asura_Physics_ForceField_Conveyor::GetOBB( void ) const
{
	return m_xOBB;
}

const Asura_AVector3& Asura_Physics_ForceField_Conveyor::GetVelocity( void ) const
{
	return m_xVelocity;
}

void Asura_Physics_ForceField_Conveyor::SetOBB( const Asura_OBB& xVolume )
{
	m_xOBB = xVolume;
}

void Asura_Physics_ForceField_Conveyor::SetVelocity( const Asura_AVector3& xVelocity )
{
	m_xVelocity = xVelocity;
}
