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

#pragma once

#include "glmAbstractRoadmapQuery.h"
#include "glmNavigationMeshLocation.h"

namespace glm
{
class NavigationPath;
class NavigationMeshLocation;
class IRoadmapCosts;

/**
* \defgroup PathLib Path Planning
*
* \ingroup Queries
*
* \author Golaem <dev@golaem.com> 
*
* \section pathPlanning_planpath Plan a path in a spatial environment
*
* The Golaem Path spatial environment framework provides the abstract class AbstractPathPlanner as a common
* interface for path planning queries between two \ref NavigationMeshLocation locations. In the current 
* version, a single implementation is provided: PathPlanner, computing the shortest path using the A* algorithm.
* This section explains how to plan a path in a spatial environment.
*
* The path planning query relies on an instance of SpatialEnvironment, named here \c myEnvironment. The origin 
* and the destination of the path planning should be valid instances of NavigationMeshLocation. Instances
* of NavigationMeshLocation, \c myOrigin and \c myDestination, and set up at a given position. 
* \code
* const glm::SpatialEnvironment& myEnvironment;
*
* glm::NavigationMeshLocation myOrigin(myEnvironment);
* assert( myOrigin.setLocation(glm::Vector3(10.2,2.,5.6) );
*
* glm::NavigationMeshLocation myDestination(myEnvironment);
* assert( myDestination.setLocation(glm::Vector3(26.,-5.9,-2) );
* \endcode
*
* We can then instanciate the path planner and give it the source and the destination.
* \code
* glm::PathPlanner myPathPlanner(myEnvironment);
* myPathPlanner.setOrigin(myOrigin);
* myPathPlanner.setDestination(myDestination);
* \endcode
*
* Finally the computation can be launched, the success checked and the result retrieved.
* \code
* myPathPlanner();
* assert( myPathPlanner.getStatus() == glm::AbstractPathPlanner::Status::SUCCESS );
* const glm::NavigationPath& myPath = myPathPlanner.getNavigationPath();
* \endcode
*
* The path computation can be done asynchronously, the status of the computation can then be monitored 
* using \ref AbstractPathPlanner::getStatus().
* 
* In the previous example, the returned \ref NavigationPath instance is owned by the path planner, it means
* \c myPath won't be valid once \c myPathPlanner is destroyed. Of course, it is possible to copy \c myPath, but
* it is also possible to provide the path planner with a \ref NavigationPath instance it will fill. In this case
* the path is not owned by the path planner.
*
* \code
* const glm::SpatialEnvironment& myEnvironment;
* glm::NavigationPath myPath;
* {
* glm::PathPlanner myPathPlanner(myEnvironment,myPath);
* [...]
*  myPathPlanner();
* }
* \endcode
*
* \section pathPlanning_customcosts Customize the computed costs
*
* Path planning relies on the traversal costs of the spatial environment's roadmap's nodes and arcs.
* The retrieval of thoses costs is delegated to an instance of IRoadmapCosts. This abstract 
* class defines methods to retrieve the traversal costs and the availability of the roadmap's nodes
* and arcs. DistanceRoadmapCosts is the provided implementation, it evaluates the costs using 3D 
* distances and the availability based on given constraint (steapest navigable slope, for example).
* Instances of AbstractPathPlanner are provided with a default DistanceRoadmapCosts but can be provided
* with another one using the method \ref AbstractPathPlanner::setCosts "setCosts". 
*
*/

/**
* \brief Abstract class defining a path planner over a roadmap.
*
* \ingroup PathLib
*
* \author Golaem <dev@golaem.com>
*
* \see \ref PathLib "Path planning concepts"
* \see \ref Roadmap "Roadmap framework"
*/
class GLM_SDK_API AbstractPathPlanner : public AbstractRoadmapQuery
{
  public:
    GLM_ABSTRACTCLASS_STAT_ID( glm::MemoryStatIds::Simulation)

    /**
	* \name Construction/Destruction.
	*/
    //@{
    /**
	* \brief Constructor from a spatial environment.
	* \param[in] spatialEnvironment The spatial environment in which the path planner will work.
	*/
    AbstractPathPlanner(const SpatialEnvironment& spatialEnvironment);

    /**
	* \brief Constructor from a spatial environment and a navigation path.
	* \param[in] spatialEnvironment The spatial environment in which the path planner will work.
	* \param[out] path The path instance that will be used by the path planner.
	*/
    AbstractPathPlanner(const SpatialEnvironment& spatialEnvironment, NavigationPath& path);

    /**
	* \brief Destructor
	*/
    virtual ~AbstractPathPlanner();
    //@}

    /**
	* \name Origin
	*/
    //@{
    /**
	* \brief Retrieves the origin of the path planning.
	*/
    const NavigationMeshLocation& getOrigin() const;
    /**
	* \brief Sets the origin of the path planning.
	* \param[in] origin The origin, a NavigationMeshLocation built from this location is kept.
	*/
    void setOrigin(const NavigationMeshLocation& origin);
    //@}

    /**
	* \name Destination
	*/
    //@{
    /**
	* \brief Retrieves the destination of the path planning.
	*/
    const NavigationMeshLocation& getDestination() const;
    /**
	* \brief Sets the destination of the path planning.
	* \param[in] destination The destination, a NavigationMeshLocation built from this location is kept.
	*/
    void setDestination(const NavigationMeshLocation& destination);
    //@}

    /**
	* \name Costs
	*/
    //@{
    /**
	* \brief Retrieves the instance of IRoadmapCosts used by the path planner.
	*/
    const IRoadmapCosts& getCosts() const;

    IRoadmapCosts& touchCosts();
    /** 
	* \brief Set the instance of IRoadmapCosts used by the path planner.
	* \param[in] costs An instance of IRoadmapCosts, only a pointer to this instance is kept.
	*/
    virtual void setCosts(IRoadmapCosts& costs, bool transferCostOwnership = false);
    //@}

    /**
	* \name Path
	*/
    //@{
    /**
	* \brief Retrieve the path used by the path planner.
	* \pre A computation must have been launched of a navigation path must have been given.
	*/
    const NavigationPath& getNavigationPath() const;
    /**
	* \brief Set the path the planner will use.
	* \pre The planner's status must be STOPPED.
	*/
    void setNavigationPath(NavigationPath& path, bool transferPathOwnership = false);
    //@}

    /**
	* \brief Launches the execution of the path planning
	* \see AbstractRoadmapQuery::operator()()
	*/
    virtual void doIt(bool continueFromLastQuery = false) override;

    /**
	* \brief Resets the status of the query and the query graph
	* \see AbstractRoadmapQuery::reset
	*/
    virtual void reset();

	bool _allowCollisionAtStart;

  protected:
    /**
	* \brief Computes a path between the current origin and destination locations
	* \param[out] path The computed path.
	* \return True if the path planning is successful.
	*/
    virtual bool planPath(NavigationPath& path) = 0;

    /// The origin of the path planning
    std::pair<RoadmapNodeId, NavigationMeshLocation> _origin;

    /// The destination of the path planning
    std::pair<RoadmapNodeId, NavigationMeshLocation> _destination;

    Vector3 _cumulativeUnfoldPosition;
    Quaternion _cumulativeUnfoldRotation;
		
  private:
    /**
	* \brief Forbidden copy constructor.
	*/
    AbstractPathPlanner(const AbstractPathPlanner&);

    /**
	* \brief Forbidden asignment operator.
	*/
    AbstractPathPlanner& operator=(const AbstractPathPlanner&);

    /// The navigation path to compute
    NavigationPath* _navigationPath;

    /// True if the navigation path is owned by the path planner, false otherwise
    bool _ownsNavigationPath;

    /// The roadmap weights provider
    IRoadmapCosts* _costs;

    /// True if the path planner owns the weights provider
    bool _ownsCosts;
};

//*****************************************************************************
// AbstractPathPlanner definitions
//*****************************************************************************

//-----------------------------------------------------------------------------
inline const IRoadmapCosts& AbstractPathPlanner::getCosts() const
{
    return *_costs;
}

//-----------------------------------------------------------------------------
inline IRoadmapCosts& AbstractPathPlanner::touchCosts()
{
    return *_costs;
}
} // namespace glm
