//========= Copyright © Valve Corporation, All rights reserved. ============//
#include "feagglomerator.h"
#include "bitvec.h"

CFeAgglomerator::CFeAgglomerator( uint nReserveNodes )
{
	m_Clusters.EnsureCapacity( nReserveNodes * 2 );
	m_Clusters.SetCount( nReserveNodes );
	m_Clusters.FillWithValue( NULL ); // client needs to set all the nodes
/*
	for ( uint i = 0; i < nReserveNodes; ++i )
	{
		m_Clusters[ i ] = new CCluster;
	}
*/
}


CFeAgglomerator::~CFeAgglomerator()
{
	m_Clusters.PurgeAndDeleteElements();
}



CFeAgglomerator::CCluster::CCluster( int nIndex, int nChildLeafs ) :m_nParent( -1 ), m_nIndex( nIndex ), m_nChildLeafs( nChildLeafs )
{
	m_nChild[ 0 ] = -1;
	m_nChild[ 1 ] = -1;
}


bool CFeAgglomerator::CCluster::HasLinks()const
{
	return m_Links.Count() > 0;
}

float CFeAgglomerator::CCluster::GetBestCost()const
{
	if ( HasLinks() )
	{
		return m_Links.ElementAtHead().m_flCost;
	}
	else
	{
		return FLT_MAX;
	}
}

void CFeAgglomerator::CCluster::RemoveLink( CCluster *pOtherCluster )
{
	for ( int i = 0; i < m_Links.Count(); ++i )
	{
		if ( m_Links.Element( i ).m_pOtherCluster == pOtherCluster )
		{
			m_Links.RemoveAt( i );
			return;
		}
	}
	Assert( !"Not found" );
}

const CFeAgglomerator::CLink *CFeAgglomerator::CCluster::FindLink( CCluster *pOtherCluster )
{
	for ( int i = 0; i < m_Links.Count(); ++i )
	{
		const CLink &link = m_Links.Element( i );
		if ( link.m_pOtherCluster == pOtherCluster )
		{
			return &link;
		}
	}
	return NULL;
}

//
// The cost of a node of the tree is the probability of its collision with another bounding box, times number of points to test.
// The probably of collision is the probability of their minkowski sum containing the origin, which is proportional to the volume of the minkowski sum.
// It boils down to guessing the size of the other bounding box. Having no better heuristc, we can just say it'll probably be a box of average size 12x12x12 or something
// 
float CFeAgglomerator::CCluster::ComputeCost( const Vector &vSize, int nChildLeafs )
{
	return ( vSize.x + 12 ) * ( vSize.y + 12 ) * ( vSize.z + 12 ) * nChildLeafs;
}

void CFeAgglomerator::CCluster::AddLink( CCluster *pOtherCluster )
{
#ifdef _DEBUG
	for ( int i = 0; i < m_Links.Count(); i++ )
	{
		Assert( m_Links.Element( i ).m_pOtherCluster != pOtherCluster );
	}
#endif

	CLink link;
	link.m_pOtherCluster = pOtherCluster;
	//
	// Note: GetSurfaceArea() is not a valid heuristic for cost here. E.g. 2 horizontal points will have 0 cost, which is clearly wrong
	// (m_Aabb + pOtherCluster->m_Aabb ).GetSurfaceArea()
	//
	link.m_flCost = ComputeCost( ( m_Aabb + pOtherCluster->m_Aabb ).GetSize(), m_nChildLeafs + pOtherCluster->m_nChildLeafs );
	m_Links.Insert( link );
}



void CFeAgglomerator::AddLink( CCluster* pCluster0, CCluster *pCluster1, ClustersPriorityQueue_t &queue )
{
	float flBestDist0 = pCluster0->GetBestCost();
	float flBestDist1 = pCluster1->GetBestCost();

	pCluster0->AddLink( pCluster1 );
	pCluster1->AddLink( pCluster0 );

	float flNewBestDist0 = pCluster0->GetBestCost();
	float flNewBestDist1 = pCluster1->GetBestCost();
	
	if ( flNewBestDist0 != flBestDist0 )
	{
		queue.RevaluateElement( pCluster0->m_nPriorityIndex );
	}

	if ( flNewBestDist1 != flBestDist1 )
	{
		queue.RevaluateElement( pCluster1->m_nPriorityIndex );
	}
}

// register a link between the nodes. 
// Call this to register all links between all nodes before building agglomerated clusters
void CFeAgglomerator::LinkNodes( int nNode0, int nNode1 )
{
	if ( nNode0 == nNode1 )
		return;
	CCluster* pCluster0 = m_Clusters[ nNode0 ];
	CCluster *pCluster1 = m_Clusters[ nNode1 ];
	if ( pCluster0->FindLink( pCluster1 ) )
	{
		AssertDbg( pCluster1->FindLink( pCluster0 ) );
	}
	else
	{
		// link not duplicated, create new link
		pCluster0->AddLink( pCluster1 );
		pCluster1->AddLink( pCluster0 );
	}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
// Build agglomerated clusters; cannot call this function twice, because it will try to agglomerate clusters at all levels (nodes to root) the 2nd time
// 
void CFeAgglomerator::Build( bool bSingleRoot )
{
	ClustersPriorityQueue_t queue;
	for ( int i = 0; i < m_Clusters.Count(); ++i )
	{
		queue.Insert( m_Clusters[ i ] );
	}
	Process( queue );
	// create links of all-with-all
	// this will be ultra-slow if we have degenerate case (like 1000 small disconnected pieces of cloth)
	if ( bSingleRoot && queue.Count() > 1 )
	{
		for ( int i = queue.Count(); i-- > 1; )
		{
			for ( int j = i; j-- > 0; )
			{
				queue.Element( i )->AddLink( queue.Element( j ) );
				queue.Element( j )->AddLink( queue.Element( i ) );
			}			
		}
		// the old queue order is destroyed; create a new queue (we could re-heapify the old queue
		ClustersPriorityQueue_t queue2;
		for ( int i = queue.Count(); i-- > 0; )
		{
			queue2.Insert( queue.Element( i ) );
		}

		Process( queue2 );
		Assert( queue2.Count() == 1 );
	}
}


void CFeAgglomerator::Validate( ClustersPriorityQueue_t *pQueue )
{
#ifdef _DEBUG
	if ( pQueue )
	{
		Assert( pQueue->IsHeapified() );
		CVarBitVec used( m_Clusters.Count() );
		for ( int i = 0; i < pQueue->Count(); ++i )
		{
			int nCluster = pQueue->Element( i )->m_nIndex;
			Assert( !used.IsBitSet( nCluster ) );
			used.Set( nCluster );
		}
		for ( int i = 0; i < m_Clusters.Count(); ++i )
		{
			CCluster *pCluster = m_Clusters[ i ];
			if ( !used.IsBitSet( i ) )
			{
				Assert( pCluster->m_Links.Count() == 0 );
			}
		}
	}

	for ( int nIndex = 0; nIndex < m_Clusters.Count(); ++nIndex )
	{
		CCluster *pThisCluster = m_Clusters[ nIndex ];
		Assert( pThisCluster->m_nIndex == nIndex );
		if ( pThisCluster->m_nChild[ 0 ] < 0 )
		{
			Assert( pThisCluster->m_nChild[ 1 ] < 0 && pThisCluster->m_nChildLeafs == 1 );
		}
		else
		{
			Assert( m_Clusters[ pThisCluster->m_nChild[ 0 ] ]->m_nChildLeafs + m_Clusters[ pThisCluster->m_nChild[ 1 ] ]->m_nChildLeafs == pThisCluster->m_nChildLeafs );
		}

		ClusterLinkQueue_t &links = pThisCluster->m_Links;
		Assert( links.IsHeapified() );
		CVarBitVec used( m_Clusters.Count() );
		for ( int i = 0; i < links.Count(); ++i )
		{
			CLink *pThisLink = &links.Element( i );
			CCluster *pOtherCluster = pThisLink->m_pOtherCluster;
			const CLink *pOtherLink = pOtherCluster->FindLink( pThisCluster );
			Assert( pOtherLink );
			Assert( pOtherLink->m_pOtherCluster == pThisCluster && pThisLink->m_flCost == pOtherLink->m_flCost ); // can't have a link with self & the cost of link should be the same from both sides
			Assert( !used.IsBitSet( pOtherCluster->m_nIndex ) );// can't have 2 links to the same cluster
			used.Set( pOtherCluster->m_nIndex ); 
		}
	}
#endif
}


void CFeAgglomerator::Process( ClustersPriorityQueue_t &queue )
{
	while ( queue.Count() > 0 && queue.ElementAtHead()->HasLinks() )
	{
		Validate( &queue );

		// remove the clusters we're merging from priority queue
		CCluster *pChild[ 2 ];
		pChild[ 0 ] = queue.ElementAtHead();
		pChild[ 1 ] = queue.ElementAtHead()->m_Links.ElementAtHead().m_pOtherCluster;

		// remove the children from the queue
		Assert( pChild[ 0 ]->m_nPriorityIndex == 0 );
		queue.RemoveAtHead(); // removing pChild[0]
		queue.RemoveAt( pChild[ 1 ]->m_nPriorityIndex );

		pChild[ 0 ]->m_nPriorityIndex = -1;
		pChild[ 1 ]->m_nPriorityIndex = -1;

		// make the new cluster, link and compute its distances to nearest clusters
		int nParentIndex = m_Clusters.AddToTail();
		CCluster *pParent = new CCluster( nParentIndex, pChild[ 0 ]->m_nChildLeafs + pChild[ 1 ]->m_nChildLeafs );
		m_Clusters[ nParentIndex ] = pParent ;
		pParent->m_Aabb = pChild[ 0 ]->m_Aabb + pChild[ 1 ]->m_Aabb;
		pParent->m_nChild[ 0 ] = pChild[ 0 ]->m_nIndex;
		pParent->m_nChild[ 1 ] = pChild[ 1 ]->m_nIndex;


		pChild[ 0 ]->m_nParent = nParentIndex;
		pChild[ 1 ]->m_nParent = nParentIndex;


		CUtlVectorFixedGrowable< CCluster*, 8 >	reAdd;
		CVarBitVec skipAddLink( m_Clusters.Count() );
		// remove all links to the children, replace them with links to the parent
		{
			ClusterLinkQueue_t &links = pChild[ 0 ]->m_Links;
			for ( int i = 0; i < links.Count(); ++i )
			{
				CCluster *pOther = links.Element( i ).m_pOtherCluster;
				Assert( pOther != pChild[ 0 ] );
				if ( pOther == pChild[ 1 ] )
					continue; // just skip the connection to the other child
				Assert( pOther->m_nPriorityIndex >= 0 );
				// we see this cluster for the first time
				float flOldDistance = pOther->GetBestCost();
				pOther->RemoveLink( pChild[ 0 ] );
				pOther->AddLink( pParent );
				pParent->AddLink( pOther );
				skipAddLink.Set( pOther->m_nIndex );
				float flNewDistance = pOther->GetBestCost();
				if ( flOldDistance != flNewDistance )
				{
					reAdd.AddToTail( pOther );
					queue.RemoveAt( pOther->m_nPriorityIndex );
					pOther->m_nPriorityIndex = -1;
				}
			}
			links.Purge(); // the links don't matter any more, we can free the memory
		}
		{
			ClusterLinkQueue_t &links = pChild[ 1 ]->m_Links;
			for ( int i = 0; i < links.Count(); ++i )
			{
				CCluster *pOther = links.Element( i ).m_pOtherCluster;
				Assert( pOther != pChild[ 1 ] );
				if ( pOther == pChild[ 0 ] )
					continue; // just skip the connection to the other child
				if ( pOther->m_nPriorityIndex >= 0 )
				{
					// we see this cluster for the first time
					float flOldDistance = pOther->GetBestCost();
					pOther->RemoveLink( pChild[ 1 ] );
					// if we saw this pOther already, and didn't remove it from the queue, then we marked it as added
					if ( !skipAddLink.IsBitSet( pOther->m_nIndex ) )
					{
						pOther->AddLink( pParent );
						pParent->AddLink( pOther );
					}
					float flNewDistance = pOther->GetBestCost();
					if ( flOldDistance != flNewDistance )
					{
						queue.RevaluateElement( pOther->m_nPriorityIndex ); // no need to remove, this is the first and last edit of this other element
					}
				}
				else
				{
					// we've seen this cluster before within this loop, just remove the link; we already added the new link to the new parent
					pOther->RemoveLink( pChild[ 1 ] );
				}
			}
			links.Purge(); // the links don't matter any more, we can free the memory
		}

		for ( int nCluster = 0; nCluster < reAdd.Count(); ++nCluster )
		{
			queue.Insert( reAdd[nCluster] );
		}
		queue.Insert( pParent );
		
		Validate( &queue );
	}

	for ( int i = 0; i < queue.Count(); ++i )
	{
		Assert( !queue.Element( i )->HasLinks() );
	}
}