#ifndef INCLUDED_ASTARENGINE
#define INCLUDED_ASTARENGINE

#include "ps/Vector2D.h"
#include "ps/Player.h"
#include <queue>

#include "renderer/TerrainOverlay.h"
#include "ps/Game.h"
#include "ps/World.h"
#include "EntityManager.h"

#include "dcdt/se/se_dcdt.h"

class AStarNode
{
	public:
	float f, g, h;
	AStarNode* parent;
	CVector2D coord;
	bool operator <(const AStarNode& rhs) const { return f<rhs.f; }
	bool equals(const AStarNode& rhs) const 
	{ 
		return ( coord.x==rhs.coord.x ) && ( coord.y==rhs.coord.y ); 
	}
};
class AStarGoalBase;
class AStarGoalLowLevel;
struct AStarNodeComp
{
  bool operator()(const AStarNode* n1, const AStarNode* n2) const
  {
    return (*n2) < (*n1);
  }
};

class CVector2D_hash_compare
{
public:
	static const size_t bucket_size = 4;
	static const size_t min_buckets = 16;
	size_t operator() (const CVector2D& Key) const
	{
		return (size_t)(Key.x + Key.y*1024.f);
	}
	bool operator() (const CVector2D& _Key1, const CVector2D& _Key2) const
	{
		return (_Key1.x < _Key2.x) || (_Key1.x==_Key2.x && _Key1.y < _Key2.y);
	}
};
typedef STL_HASH_MAP<CVector2D, AStarNode*, CVector2D_hash_compare> ASNodeHashMap;

class PriQueue 
	: public std::priority_queue<AStarNode*, std::vector<AStarNode*>, AStarNodeComp>
{
public:
	// Promote a node in the PQ, or if it doesn't exist, add it
	void promote(AStarNode* node);
	void clear();
};

typedef unsigned char AStarNodeFlag;

class PathFindingTerrainOverlay;

class CAStarEngine
{
public:
	CAStarEngine();
	CAStarEngine(AStarGoalBase* goal);
	virtual ~CAStarEngine();

	void SetGoal(AStarGoalBase* goal) { mGoal = goal; }

	bool FindPath( const CVector2D& src, const CVector2D& dest, HEntity entity, float radius=0.0f );
	std::vector<CVector2D> GetLastPath();

	// The maximum number of nodes that will be expanded before failure is declared
	void SetSearchLimit( int limit );

	//Kai:added tile overlay for pathfinding
	PathFindingTerrainOverlay* pathfindingOverlay;
	
	SrPolygon pol;
    
	//void TAStarTest();

	

protected:
	AStarGoalBase* mGoal;
private:
	int mSearchLimit;
	bool mSolved;
	std::vector<CVector2D> mPath;
	AStarNodeFlag *mFlags;
	long mFlagArraySize;

	ASNodeHashMap mClosed;
	PriQueue mOpen;

	std::vector<AStarNode*> freeNodes;
	std::vector<AStarNode*> usedNodes;

	// AddToOpen will promote if the node already is in Open
	void AddToOpen( AStarNode* );
	AStarNode* RemoveBestOpenNode();

	void AddToClosed( AStarNode* );
	void RemoveFromClosed( AStarNode* );
	AStarNode* GetFromClosed( const CVector2D& );

	void ClearOpen();
	void ClearClosed();

	void ConstructPath( AStarNode* );

	AStarNode* GetFreeASNode();
	void Cleanup();

	inline AStarNodeFlag* GetFlag(const CVector2D&);
	inline bool IsClear(AStarNodeFlag*);
	inline bool IsClosed(AStarNodeFlag*);
	inline bool IsOpen(AStarNodeFlag*);
	inline void SetClosedFlag(AStarNodeFlag*);
	inline void ClearClosedFlag(AStarNodeFlag*);
	inline void SetOpenFlag(AStarNodeFlag*);
	inline void ClearOpenFlag(AStarNodeFlag*);
	inline bool IsPassable(AStarNodeFlag*);
	inline bool IsBlocked(AStarNodeFlag*);
	inline void SetPassableFlag(AStarNodeFlag*);
	inline void SetBlockedFlag(AStarNodeFlag*);

};

/**
 * An A* goal consists of a destination tile and a radius within which a
 * unit must get to the destination. The radius is necessary because for
 * actions on a target unit, like attacking or building, the destination
 * tile is obstructed by that unit and what we really want is not to get
 * to that tile but to get close enough to perform our action.
 **/
class AStarGoalBase
{
public:
	AStarGoalBase() {}
	virtual void SetDestination( const CVector2D& ) = 0;
	virtual void SetRadius( float r ) = 0;
	virtual float DistanceToGoal( const CVector2D& ) = 0;
	virtual bool IsAtGoal( const CVector2D& ) = 0;
	virtual float GetTileCost( const CVector2D&, const CVector2D& ) = 0;
	virtual bool IsPassable( const CVector2D&, HEntity entity) = 0;
	virtual std::vector<CVector2D> GetNeighbors( const CVector2D&, HEntity entity) = 0;
	virtual CVector2D GetCoord( const CVector2D& ) = 0;
	virtual CVector2D GetTile( const CVector2D& ) = 0;
	virtual float GetRadius() = 0;
};

class AStarGoalLowLevel : public AStarGoalBase
{
public:
	AStarGoalLowLevel(): radius(0.0f) {}
	void SetDestination( const CVector2D& dest );
	void SetRadius( float r );
	float DistanceToGoal( const CVector2D& loc );
	bool IsAtGoal( const CVector2D& loc );
	float GetTileCost( const CVector2D& loc1, const CVector2D& loc2 );
	bool IsPassable( const CVector2D& loc, HEntity entity);
	std::vector<CVector2D> GetNeighbors( const CVector2D& loc, HEntity entity);
	CVector2D GetCoord( const CVector2D& loc);
	CVector2D GetTile( const CVector2D& loc);
	float GetRadius();
private:
	CVector2D coord;
	float radius;
};

class CAStarEngineLowLevel : public CAStarEngine
{
public:
	CAStarEngineLowLevel()
	{
		mGoal = new AStarGoalLowLevel;
	}
	virtual ~CAStarEngineLowLevel()
	{
		delete mGoal;
	}
};


#endif