#ifndef _JFXMAP_ASTAR_H_
#define _JFXMAP_ASTAR_H_

// 参考：原文链接：https://blog.csdn.net/u012234115/article/details/47152137
#include <algorithm>
#include <list>
#include <memory>
#include <math.h>

#include "HdLib.h"

namespace jf {

    struct LinkNode;
    typedef std::shared_ptr<LinkNode> ShPtr_LinkNode;

    struct LinkNode {
        LinkNode(LinkEndpoint _stnode): leNode(_stnode),G(0),H(0),F(0),splnParent(nullptr){}
        LinkNode(): leNode(LinkEndpoint()),G(0),H(0),F(0),splnParent(nullptr) {}

        LinkEndpoint leNode;        // 节点（取车道的终点）
        double G;                   // G 表示从上一点移动到路网上指定点的移动耗费
        double H;                   // H 表示从指定点移动到目的点的预计耗费
        double F;                   // F=G+H
        ShPtr_LinkNode splnParent;  // parent的指针
    };

    struct LaneNode;
    typedef std::shared_ptr<LaneNode> ShPtr_LaneNode;

    struct LaneNode {
        LaneEndpoint leNode;        // 节点（取车道的终点）
        double G;                   // G 表示从上一点移动到路网上指定点的移动耗费
        double H;                   // H 表示从指定点移动到目的点的预计耗费
        double F;                   // F=G+H
        ShPtr_LaneNode splnParent;  // parent的指针
        LaneNode(LaneEndpoint _stnode): leNode(_stnode),G(0),H(0),F(0),splnParent(nullptr){}
        LaneNode():leNode(LaneEndpoint()),G(0),H(0),F(0),splnParent(nullptr){}
    };


    class AStar {
    public:
        AStar();

    public:
        /**
         * @brief 道路Link级别寻路
         * @param leStartNode：起点
         * @param vctleEndNode：终点集
         * @param vctleOutLinkPath：寻路结果
         * @param leOutEndNode：该次寻路使用的终点
         * @param nMaxSteps：最大寻路步骤
         * @param bHighPrecision：是否将终点集中的终点按照距离优先使用
         * @return 寻路是否成功
         */
        bool GetPath(const LinkEndpoint &leStartNode, const std::vector<LinkEndpoint> &vctleEndNode,
                     std::vector<LinkEndpoint> &vctleOutLinkPath, LinkEndpoint &leOutEndNode, int nMaxSteps = 65535,
                     bool bHighPrecision = false);

        /**
         * @brief 车道Lane级别寻路
         * @param leStartNode：起点
         * @param vctleEndNode：终点集
         * @param vctleOutLanePath：寻路结果
         * @param leOutEndNode：该次寻路使用的终点
         * @param nMaxSteps：最大寻路步骤
         * @param bHighPrecision：是否将终点集中的终点按照距离优先使用
         * @param bLastLinkChangeLane：是否允许终点的link变道
         * @return 寻路是否成功
         */
        bool GetPath(const LaneEndpoint &leStartNode, const std::vector<LaneEndpoint> &vctleEndNode,
                     std::vector<LaneEndpoint> &vctleOutLanePath, LaneEndpoint &leOutEndNode, int nMaxSteps = 65535,
                     bool bHighPrecision = false, bool bLastLinkChangeLane = false);

        /**
         * @brief 道路Link级别寻路
         * @param leStartNode：起点
         * @param leEndNode：终点
         * @param vctleOutLinkPath：寻路结果
         * @param nMaxSteps：最大寻路步骤
         * @return 寻路是否成功
         */
        bool GetPath(const LinkEndpoint &leStartNode, const LinkEndpoint &leEndNode,
                     std::vector<LinkEndpoint> &vctleOutLinkPath, int nMaxSteps = 65535);

        /**
         * @brief 车道Lane级别寻路
         * @param leStartNode：起点
         * @param leEndNode：终点
         * @param vctleOutLanePath：寻路结果
         * @param nMaxSteps：最大寻路步骤
         * @return 寻路是否成功
         */
        bool GetPath(const LaneEndpoint &leStartNode, const LaneEndpoint &leEndNode,
                     std::vector<LaneEndpoint> &vctleOutLanePath, int nMaxSteps = 65535);

    private:
        // 道路级别寻路
        bool FindLinkPath(const ShPtr_LinkNode &splnStartNode, const std::vector<ShPtr_LinkNode> &vctslEndNode,
                          ShPtr_LinkNode &splnResult, LinkEndpoint &leOutEndNode, int nMaxSteps);

        // 车道级别寻路
        bool FindLanePath(const ShPtr_LaneNode &splnStartNode, const std::vector<ShPtr_LaneNode> &vctslEndNode,
                          const std::vector<LinkEndpoint> &vctleLinkPath, ShPtr_LaneNode &splnResult,
                          LaneEndpoint &leOutEndNode, int nMaxSteps);

        //从开启列表中返回F值最小的节点
        bool GetLeastFNode(ShPtr_LinkNode &splnNode);

        //从开启列表中返回F值最小的节点
        bool GetLeastFNode(ShPtr_LaneNode &splnNode);

        // 根据当前节点，获取下一个节点
        bool GetNextNodes(const ShPtr_LinkNode &splnCurrentNode, std::vector<ShPtr_LinkNode> &vctslNextNodes);

        // 根据当前节点，获取下一个节点
        bool GetNextNodes(const ShPtr_LaneNode &splnCurrentNode, const std::vector<LinkEndpoint> &vctleLinkPath,
                          std::vector<ShPtr_LaneNode> &vctslNextNodes);

        //判断开启/关闭列表中是否包含某点
        ShPtr_LinkNode IsInList(const std::list<ShPtr_LinkNode> &lstspNodes, const ShPtr_LinkNode &splnNode);

        //判断开启/关闭列表中是否包含某点
        ShPtr_LaneNode IsInList(const std::list<ShPtr_LaneNode> &lstspNodes, const ShPtr_LaneNode &splnNode);

        //计算FGH值
        double CalcG(const ShPtr_LinkNode &spStart, const ShPtr_LinkNode &splnNode);

        double CalcG(const ShPtr_LaneNode &spStart, const ShPtr_LaneNode &splnNode);

        double CalcH(const ShPtr_LinkNode &splnNode, const ShPtr_LinkNode &splnEnd);

        double CalcH(const ShPtr_LaneNode &splnNode, const ShPtr_LaneNode &splnEnd);

        double CalcF(const ShPtr_LinkNode &splnNode);

        double CalcF(const ShPtr_LaneNode &splnNode);

    private:
        std::list<ShPtr_LinkNode> m_lstslOpenLinkNodes;   //开放列表
        std::list<ShPtr_LaneNode> m_lstslOpenLaneNodes;   //开放列表
        std::list<ShPtr_LinkNode> m_lstslCloseLinkNodes;  //关闭列表
        std::list<ShPtr_LaneNode> m_lstslCloseLaneNodes;  //关闭列表

    };
}  // namespace jf

#endif  // _JFXMAP_ASTAR_H_
