/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
*                                                                          *
***************************************************************************/

#pragma once

#include "glmIOrientedGraph.h"
#include "glmExtendedOrientedGraph.h"
#include "glmStringOperators.h"
#include "glmTypes.h"
#include "glmVector3.h"
#include "glmPODArray.h"
#include "glmIRoadmapCosts.h"
#include "glmSet.h"

namespace glm
{
	/**
	* \brief A* algorithm on any specialization of OrientedGraph.
	*
	* \ingroup graphs
	* \author Golaem <dev@golaem.com>
	*/
	// template <class ScalarType, class NodeId, class ArcId, class NodeData, class ArcData, class Cost, class Heuristic>
	// used with <float,			RoadmapNodeId, RoadmapArcId, RoadmapNode, RoadmapArc, ArcOriginValueFunctor, NodeHeuristicFunctor>
	class GLM_SDK_API AStar
	{
	public:

		GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

		/**
		* \brief internal class for managing temporary data
		*/
		struct EndRoadmapArc
		{

			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			bool operator<(const EndRoadmapArc& other) const
			{
				return _endNode < other._endNode;
			}

			//struct compare {
			//	bool operator()(const EndRoadmapArc& a, const EndRoadmapArc& b) const {
			//	  return a._endNode < b._endNode;
			//	}   
			//};

			int		_endNode;
			float	_endArcCost;
			int		_endArcId;
		};

	public:
		/**
		* \brief Constructor from a given graph.
		* \param[in] graph The graph on which computation are performed.
		* \param[in] costFunction An instance of the ArcOriginValueFunctor functor used to retrieve the cost of an arc.
		* \param[in] heuristicFunction An instance of the NodeHeuristicFunctor functor used to underestimate the cost of joining two nodes.
		*/
		AStar(const ExtendedOrientedGraph & graph, const IRoadmapCosts& costs);

		/**
		* \brief Destructor.
		*/
		~AStar();

		/**
		* \brief Compute the shortest path between the source and the target.
		*
		* \param[in] source The source node.
		* \param[in] target The target node.
		* \param[out] path The computed path as a list of arcs from the source to the target.
		*
		* \return true if a valid path was found, false otherwise.
		* \pre The given identifiers must be valid within the underlying graph.
		*/
		//template <class Path>
		//bool compute(
		//	int source, 
		//	int target, 
		//	Path & path);

		/**
		* \brief Compute the shortest path between the source and the target, assuming the graphe is an ExtendedOrientedGraph with origin and destination as extents
		*
		* \param[in] source The source node.
		* \param[in] target The target node.
		* \param[out] path The computed path as a list of arcs from the TARGET TO THE SOURCE (reverse order)
		*
		* \return true if a valid path was found, false otherwise.
		* \pre The given identifiers must be valid within the underlying graph.
		*/
		template <class Path>
		bool computeWithExtended(
			int source,
			int target,
			Path & path,
			bool forceStartIsUsable = false);

		void setCosts(const IRoadmapCosts& costs);

		void computeArcCostWithExtended(const RoadmapArcId& arcId, float & cost, bool & usable, bool skipUsable = false) const;
		void computeArcCostWithBaseGraph(const OrientedGraph& orientedGraph, const RoadmapArcId& arcId, float & cost, bool & usable) const;

	private:
		GLM_DISALLOW_COPY_AND_ASSIGN(AStar);

		struct ExploreData
		{
			GLM_CLASS_STAT_ID( glm::MemoryStatIds::Simulation);

			float _heuristic;
			int  _nodeToExplore;
		};

		void pushInQueue(const ExploreData& exploreData);
		const ExploreData& getQueueTop() const;
		void popQueueTop();

		/**
		* \brief Initialize the data structures.
		*/
		void init();

		/**
		* \brief Conditionnally change the reaching cost of a node.
		*
		* The change is effective only if the node has not yet been noted or if the current
		* cost is more than the given one.
		*
		* \param id	The node id.
		* \param previous The predecessor arc id.
		* \param cost The cost.
		* \return True if the cost was changed.
		*/
		bool conditionnalSetCost(int id, int previous, float cost);

		/**
		* \brief The graph on which the A* is working.
		*/
		const ExtendedOrientedGraph* _graph;

		const IRoadmapCosts* _roadmapCosts;

		///**
		//* \brief The cost function.
		//*/
		//const ArcOriginValueFunctor * _costFunction;

		///**
		//* \brief The heuristic function.
		//*/
		//const NodeHeuristicFunctor * _heuristicFunction;

		/**
		* \brief Table dating the last cost modification of any graph node.
		*/
		glm::PODArray<unsigned int> _date;

		/**
		* \brief Table keeping the cost to reach each nodes.
		*/
		glm::PODArray<float>	_cost;

		/**
		* \brief Table containing each node predecessor having given the best cost to reach it.
		*/
		glm::PODArray<int> _bestPredecessor;

		glm::PODArray<ExploreData>	_toExplore;

		glm::GlmSet< EndRoadmapArc >::SetNodePool _endArcsPool; // avoids reallocation on set at each planification
		glm::GlmSet< EndRoadmapArc > _endArcs;

		/**
		* \brief The current date.
		*/
		unsigned int _currentDate;

	};

	//-----------------------------------------------------------------------------
	// compute
	//-----------------------------------------------------------------------------
	template <class Path>
	bool AStar::computeWithExtended(
		int source,
		int target,
		Path & path,
		bool forceStartIsUsable)
	{
		GLM_DEBUG_ASSERT_MSG(_graph->hasNode(source), "The provided source node #%d is not defined in the underlying graph.", source);
		GLM_DEBUG_ASSERT_MSG(_graph->hasNode(target), "The provided target node #%d is not defined in the underlying graph.", target);

		path.clear();

		//Init variables.
		float heuristic;
		float arcCost;
		bool arcUsable;

		init();

		GLM_DEBUG_ASSERT(_toExplore.empty());

		// get target as position for heuristic (avoids using extended graph in main loop)
		glm::Vector3 targetPosition = _graph->getNode(target).getNavigationPositionLocal();

		//Setting up the priority queue for the algorithm.
		heuristic = _roadmapCosts->getCostHeuristic(*_graph, source, targetPosition);
		conditionnalSetCost(source, int(-1), 0.0); // source node cost 0 to reach

		//----------
		// First iteration : store arcs to explore from baseGraph
		//----------

		ExploreData exploreNodeToAdd;
		ExploreData currentExploration;

		// Iterating through the successors of the node.
		const glm::PODArray<int> & sourceSuccessors = _graph->getNodeSuccessors(source);
		for (size_t iSuccessor = 0, successorsCount = sourceSuccessors.size(); iSuccessor < successorsCount; ++iSuccessor)
		{
			int aidSuccessor = sourceSuccessors[iSuccessor];
			int nidSuccessor = _graph->getArcExtremity(aidSuccessor);

			computeArcCostWithExtended(aidSuccessor, arcCost, arcUsable, forceStartIsUsable);
			if (arcUsable && conditionnalSetCost(nidSuccessor, aidSuccessor, _cost[source] + arcCost))
			{
				heuristic = _roadmapCosts->getCostHeuristic(*_graph, nidSuccessor, targetPosition);

				exploreNodeToAdd._heuristic = _cost[nidSuccessor] + heuristic;
				exploreNodeToAdd._nodeToExplore = nidSuccessor;

				pushInQueue(exploreNodeToAdd);
			}
		}

		//----------		
		// compute potential end nodes
		//----------

		// buid a set of all valid arcs :
		_endArcs.clear();
		EndRoadmapArc endArcCandidate;
		const glm::PODArray<int> & targetPredecessors = _graph->getNodePredecessors(target);
		for (size_t iPredecessor = 0, predecessorsCount = targetPredecessors.size(); iPredecessor < predecessorsCount; ++iPredecessor)
		{
			endArcCandidate._endArcId = targetPredecessors[iPredecessor];
			computeArcCostWithExtended(endArcCandidate._endArcId, endArcCandidate._endArcCost, arcUsable);

			if (arcUsable)
			{
				endArcCandidate._endNode = _graph->getArcOrigin(endArcCandidate._endArcId);
				_endArcs.insert(endArcCandidate);
			}
		}

		//----------		
		// use basegraph only to compute path
		//----------

		const OrientedGraph& basegraph = _graph->getBaseGraph();

		EndRoadmapArc lookupRoadmapArc;
		typename glm::GlmSet< EndRoadmapArc >::const_iterator foundIte;

		while (!_toExplore.empty())
		{
			currentExploration = getQueueTop();
			popQueueTop();

			int nidCurrent = currentExploration._nodeToExplore;

			lookupRoadmapArc._endNode = nidCurrent;
			foundIte = _endArcs.find(lookupRoadmapArc);

			// if an end node, setConditionalCost and test for pathFound :
			if (foundIte != _endArcs.end())
			{
				conditionnalSetCost(target, foundIte.getValue()._endArcId, (_cost[nidCurrent] + foundIte.getValue()._endArcCost));
			}

			// need to evaluate stopSearch each time as cost[target] does not change, but currentExploration.first does !
			// stop when target _date has been updated, meaning we reached target, and target cost is <= heuristic, which means best path.
			bool stopSearch = _date[target] == _currentDate && _cost[target] <= currentExploration._heuristic;

			if (!stopSearch)
			{
				// Iterating through the successors of the node.
				const glm::PODArray<int> & successors = basegraph.getNodeSuccessors(nidCurrent);
				for (size_t iSuccessor = 0, successorCount = successors.size(); iSuccessor < successorCount; ++iSuccessor)
				{
					int aidSuccessor = successors[iSuccessor];
					int nidSuccessor = basegraph.getArcExtremity(aidSuccessor);

					this->computeArcCostWithBaseGraph(basegraph, aidSuccessor, arcCost, arcUsable); // must use basegraph
					if (arcUsable && conditionnalSetCost(nidSuccessor, aidSuccessor, _cost[nidCurrent] + arcCost))
					{
						heuristic = _roadmapCosts->getCostHeuristic(basegraph, nidSuccessor, targetPosition);

						exploreNodeToAdd._heuristic = _cost[nidSuccessor] + heuristic;
						exploreNodeToAdd._nodeToExplore = nidSuccessor;

						pushInQueue(exploreNodeToAdd);
					}
				}
			}
		}

		//----------		
		// end using basegraph
		//----------

		bool pathFound = false;
		if (_date[target] == _currentDate) // a path is found, maybe by default after last heuristic was better than target path.
		{
			pathFound = true;
			int nidCurrent = target;
			while (nidCurrent != source)
			{
				int aidCurrent = _bestPredecessor[nidCurrent];
				nidCurrent = _graph->getArcOrigin(aidCurrent);
				path.push_back(aidCurrent);
			}
		}

		return pathFound;
	}

}

