//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose:
//
// $NoKeywords: $
//=============================================================================//

#include "cbase.h"

#include "ai_movesolver.h"
#include "ndebugoverlay.h"

// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"

//-----------------------------------------------------------------------------

inline float V_round( float f )
{
	return (float)( (int)( f + 0.5 ) );
}

//-----------------------------------------------------------------------------
// CAI_MoveSolver
//-----------------------------------------------------------------------------

// The epsilon used by the solver
const float AIMS_EPS = 0.01;


//-----------------------------------------------------------------------------
// Visualization
//-----------------------------------------------------------------------------
void CAI_MoveSolver::VisualizeRegulations( const Vector& origin )
{
	if ( m_Regulations.Count() )
	{
		CAI_MoveSuggestions regulations;
		regulations.AddVectorToTail( m_Regulations );
		NormalizeSuggestions( &regulations[0], (&regulations[0]) + regulations.Count() );

		Vector side1, mid, side2;
		for (int i = regulations.Count(); --i >= 0; )
		{
			// Compute the positions of the angles...
			float flMinAngle = regulations[i].arc.center - regulations[i].arc.span * 0.5f;
			float flMaxAngle = regulations[i].arc.center + regulations[i].arc.span * 0.5f;

			side1 = UTIL_YawToVector( flMinAngle );
			side2 = UTIL_YawToVector( flMaxAngle );
			mid = UTIL_YawToVector( regulations[i].arc.center );

			// Stronger weighted ones are bigger
			if ( regulations[i].weight < 0 )
			{
				float flLength = 10 + 40 * ( regulations[i].weight * -1.0);
				side1 *= flLength;
				side2 *= flLength;
				mid *= flLength;

				side1 += origin;
				side2 += origin;
				mid += origin;

				NDebugOverlay::Triangle(origin,	mid, side1, 255, 0, 0, 48, true, 0.1f );
				NDebugOverlay::Triangle(origin,	side2, mid, 255, 0, 0, 48, true, 0.1f );
			}
		}
	}
}


//-------------------------------------
// Purpose: The actual solver function. Reweights according to type and sums
//			all the suggestions, identifying the best.
//-------------------------------------
bool CAI_MoveSolver::Solve( const AI_MoveSuggestion_t *pSuggestions, int nSuggestions, AI_MoveSolution_t *pResult)
{
	//---------------------------------
	//
	// Quick out
	//
	if ( !nSuggestions )
		return false;

	if ( nSuggestions == 1 && m_Regulations.Count() == 0 && pSuggestions->type == AIMST_MOVE )
	{
		pResult->dir = pSuggestions->arc.center;
		return true;
	}

	//---------------------------------
	//
	// Setup
	//
	CAI_MoveSuggestions suggestions;

	suggestions.EnsureCapacity( m_Regulations.Count() + nSuggestions );

	suggestions.CopyArray( pSuggestions, nSuggestions);
	suggestions.AddVectorToTail( m_Regulations );

	// Initialize the solver
	const int NUM_SOLUTIONS	= 120;
	const int SOLUTION_ANG	= 360 / NUM_SOLUTIONS;

	COMPILE_TIME_ASSERT( ( 360 % NUM_SOLUTIONS ) == 0 );

	struct Solution_t
	{
		// The sum bias
		float					   bias;
		float					   highBias;
		AI_MoveSuggestion_t *pHighSuggestion;
	};

	Solution_t 	solutions[NUM_SOLUTIONS]	= { 0 };

	//---------------------------------

	// The first thing we do is reweight and normalize the weights into a range of [-1..1], where
	// a negative weight is a repulsion. This becomes a bias for the solver.
	// @TODO (toml 06-18-02): this can be made sligtly more optimal by precalculating regulation adjusted weights
	Assert( suggestions.Count() >= 1 );
	NormalizeSuggestions( &suggestions[0], (&suggestions[0]) + suggestions.Count() );

	//
	// Add the biased suggestions to the solutions
	//
	for ( int iSuggestion = 0; iSuggestion < suggestions.Count(); ++iSuggestion )
	{
		AI_MoveSuggestion_t &current = suggestions[iSuggestion];

		// Convert arc values to solution indices relative to right post. Right is angle down, left is angle up.
		float halfSpan	= current.arc.span * 0.5;
		int   center 	= V_round( ( halfSpan * NUM_SOLUTIONS ) / 360 );
		int   left		= ( current.arc.span * NUM_SOLUTIONS ) / 360;

		float angRight   = current.arc.center - halfSpan;

		if (angRight < 0.0)
			angRight += 360;

		int base = ( angRight * NUM_SOLUTIONS ) / 360;

		// Sweep from left to right, summing the bias. For positive suggestions,
		// the bias is further weighted to favor the center of the arc.
		const float positiveDegradePer180 = 0.05; // i.e., lose 5% of weight by the time hit 180 degrees off center
		const float positiveDegrade       = ( positiveDegradePer180 / ( NUM_SOLUTIONS * 0.5 ) ); 

		for ( int i = 0; i < left + 1; ++i )
		{
			float bias = 0.0;

			if ( current.weight > 0)
			{
				int	iOffset = center - i;
				float degrade = abs( iOffset ) * positiveDegrade;

				if ( ( (current.flags & AIMS_FAVOR_LEFT ) && i > center ) || 
					 ( (current.flags & AIMS_FAVOR_RIGHT) && i < center ) )
				{
					degrade *= 0.9;
				}

				bias = current.weight - ( current.weight * degrade );
			}
			else
				bias = current.weight;

			int iCurSolution = (base + i) % NUM_SOLUTIONS;

			solutions[iCurSolution].bias += bias;
			if ( bias > solutions[iCurSolution].highBias )
			{
				solutions[iCurSolution].highBias        = bias;
				solutions[iCurSolution].pHighSuggestion = &current;
			}
		}
	}

	//
	// Find the best solution
	//
	int   best     = -1;
	float biasBest = 0;

	for ( int i = 0; i < NUM_SOLUTIONS; ++i )
	{
		if ( solutions[i].bias > biasBest )
		{
			best     = i;
			biasBest = solutions[i].bias;
		}
	}

	if ( best == -1 )
		return false; // no solution

	//
	// Construct the results
	//
	float result = best * SOLUTION_ANG;

	// If the matching suggestion is within the solution, use that as the result,
	// as it is valid and more precise.
	const float suggestionCenter = solutions[best].pHighSuggestion->arc.center;

	if ( suggestionCenter > result && suggestionCenter <= result + SOLUTION_ANG )
		result = suggestionCenter;

	pResult->dir = result;

	return true;
}

//-------------------------------------
// Purpose: Adjusts the suggestion weights according to the type of the suggestion,
//			apply the appropriate sign, ensure values are in expected ranges
//-------------------------------------

struct AI_MoveSuggWeights
{
	float min;
	float max;
};

static AI_MoveSuggWeights g_AI_MoveSuggWeights[] = // @TODO (toml 06-18-02): these numbers need tuning
{
	{  0.20,  1.00 },	// AIMST_MOVE
	{ -0.00, -0.25 },	// AIMST_AVOID_DANGER
	{ -0.00, -0.25 },	// AIMST_AVOID_OBJECT
	{ -0.00, -0.25 },	// AIMST_AVOID_NPC
	{ -0.00, -0.25 },	// AIMST_AVOID_WORLD
	{ -1.00, -1.00 },	// AIMST_NO_KNOWLEDGE
	{ -0.60, -0.60 },	// AIMST_OSCILLATION_DETERRANCE
	{  0.00,  0.00 },	// AIMST_INVALID
};

void CAI_MoveSolver::NormalizeSuggestions( AI_MoveSuggestion_t *pBegin, AI_MoveSuggestion_t *pEnd )
{
	while ( pBegin != pEnd )
	{
		const float min = g_AI_MoveSuggWeights[pBegin->type].min;
		const float max = g_AI_MoveSuggWeights[pBegin->type].max;

		Assert( pBegin->weight >= -AIMS_EPS && pBegin->weight <= 1.0 + AIMS_EPS );

		if ( pBegin->weight < AIMS_EPS ) // zero normalizes to zero
			pBegin->weight = 0.0;
		else
			pBegin->weight = ( ( max - min ) * pBegin->weight ) + min;

		while (pBegin->arc.center < 0)
			pBegin->arc.center += 360;

		while (pBegin->arc.center >= 360)
			pBegin->arc.center -= 360;

		++pBegin;
	}
}

//-------------------------------------

bool CAI_MoveSolver::HaveRegulationForObstacle( CBaseEntity *pEntity)
{
   for ( int i = 0; i < m_Regulations.Count(); ++i )
   {
      if ( m_Regulations[i].hObstacleEntity != NULL &&
           pEntity == m_Regulations[i].hObstacleEntity.Get() )
      {
         return true;
      }
   }
   return false;
}

//-----------------------------------------------------------------------------
//
// Commands and tests
//

#ifdef DEBUG
CON_COMMAND(ai_test_move_solver, "Tests the AI move solver system")
{
#ifdef DEBUG
	const float EPS = 0.001;
#endif
	DevMsg( "Beginning move solver tests...\n" );

	CAI_MoveSolver 		solver;
	AI_MoveSolution_t 	solution;
	int					i;

	//
	// Value in, no regulations, should yield value out
	//
	{
	DevMsg( "Simple... " );

	for (i = 0; i < 360; ++i)
	{
		Assert( solver.Solve( AI_MoveSuggestion_t( AIMST_MOVE, 1, i, 180 ), &solution ) );
		Assert( solution.dir == (float)i );

	}

	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

	//
	// Two values in, should yield the first
	//
	{
	DevMsg( "Two positive... " );

	AI_MoveSuggestion_t suggestions[2];

	suggestions[0].Set( AIMST_MOVE, 1.0, 180, 100 );
	suggestions[1].Set( AIMST_MOVE, 0.5, 0, 100 );

	Assert( solver.Solve( suggestions, 2, &solution ) );
	Assert( solution.dir == (float)suggestions[0].arc.center );


	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

	//
	// Two values in, first regulated, should yield the second
	//
	{
	DevMsg( "Avoid one of two... " );

	AI_MoveSuggestion_t suggestions[2];

	solver.AddRegulation(AI_MoveSuggestion_t( AIMST_AVOID_OBJECT, 1, 260, 60 ) );

	suggestions[0].Set( AIMST_MOVE, 1.0, 270, 45 );
	suggestions[1].Set( AIMST_MOVE, 1.0, 0, 45 );

	Assert( solver.Solve( suggestions, 2, &solution ) );
	Assert( solution.dir == (float)suggestions[1].arc.center );


	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

	//
	// No solution
	//
	{
	DevMsg( "No solution... " );

	AI_MoveSuggestion_t suggestions[2];

	suggestions[0].Set( AIMST_MOVE, 1.0, 270, 90 );
	suggestions[1].Set( AIMST_AVOID_OBJECT, 1.0, 260, 180 );

	Assert( !solver.Solve( suggestions, 2, &solution ) );

	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

	//
	// Nearest solution, in tolerance
	//
	{
	DevMsg( "Nearest solution, in tolerance... " );

	AI_MoveSuggestion_t suggestions[2];

	suggestions[0].Set( AIMST_MOVE, 1.0, 278, 90 );
	suggestions[1].Set( AIMST_AVOID_OBJECT, 1.0, 260, 24 );

	Assert( solver.Solve( suggestions, 2, &solution ) );
	Assert( solution.dir == (float)suggestions[0].arc.center );

	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

	//
	// Nearest solution
	//
	{
	DevMsg( "Nearest solution... " );

	AI_MoveSuggestion_t suggestions[2];

	suggestions[0].Set( AIMST_MOVE, 1.0, 270, 90 );
	suggestions[1].Set( AIMST_AVOID_OBJECT, 1.0, 260, 40 );

	Assert( solver.Solve( suggestions, 2, &solution ) );
	Assert( solution.dir - 282 < EPS ); // given 60 solutions

	DevMsg( "pass.\n" );
	solver.ClearRegulations();
	}

}
#endif

//=============================================================================