#include "AStar.h"

using namespace jf;

AStar::AStar() {}
/**
 *  A*计算（Link路径规划）
 */
bool AStar::FindLinkPath(const ShPtr_LinkNode &splnStartNode, const std::vector<ShPtr_LinkNode> &vctslEndNode,
                         ShPtr_LinkNode &splnResult, LinkEndpoint &leOutEndNode, int nMaxSteps) {
    // 用作最大寻路次数
    int nCurSteps = 0;
    m_lstslOpenLinkNodes.push_back(ShPtr_LinkNode(splnStartNode));  // 置入起点,拷贝开辟一个节点，内外隔离
    // 如果Open集为空，则判断寻路失败
    while (!m_lstslOpenLinkNodes.empty()) {
        ++nCurSteps;
        ShPtr_LinkNode spCurrent(new LinkNode());
        GetLeastFNode(spCurrent);                    // 找到F值最小的link
        m_lstslOpenLinkNodes.remove(spCurrent);      // 从开启列表中删除
        m_lstslCloseLinkNodes.push_back(spCurrent);  // 放到关闭列表
        // jf_log_info("AStar::findPath(), put link id = {0} into close list.",
        // 1,找到当前link可以到达的下一link
        std::vector<ShPtr_LinkNode> vctspNexts = {};
        if (false == GetNextNodes(spCurrent, vctspNexts)) {
            // 如果当前link走到了死胡同，则放弃该link，重新寻找F值最小的link
            continue;
        }
        for (auto &splnNode : vctspNexts) {
            // 2,对某一个link，如果它不在开启列表中，加入到开启列表，设置当前link为其父节点，计算F G H
            if (!IsInList(m_lstslOpenLinkNodes, splnNode)) {
                // 设置当前link为其父节点
                splnNode->splnParent = spCurrent;

                // 计算F G H
                splnNode->G = CalcG(spCurrent, splnNode);
                splnNode->H = CalcH(splnNode, vctslEndNode[0]);
                splnNode->F = CalcF(splnNode);

                // jf_log_info("astar::findPath(), push link: {0}",
                // 将当前link放入Open集
                m_lstslOpenLinkNodes.push_back(splnNode);
            }
                // 3，对某一个link，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
            else {
                // 计算G值
                int nTempG = CalcG(spCurrent, splnNode);
                // 如果比原来的小
                if (nTempG < splnNode->G) {
                    // 设置它的父节点为当前点,并更新G和F
                    splnNode->splnParent = spCurrent;

                    splnNode->G = nTempG;
                    splnNode->F = CalcF(splnNode);
                }
            }

            // 检查当前结果
            for (ShPtr_LinkNode splnEndNode : vctslEndNode) {
                // 当前的Open集里面已经有了终点link，寻路结束
                splnResult = IsInList(m_lstslOpenLinkNodes, splnEndNode);
                if (splnResult) {
                    leOutEndNode = splnEndNode->leNode;
                    return true;  // 返回列表里的节点指针，不要用原来传入的endNode指针，因为发生了深拷贝
                }
            }
        }

        // 如果元素太多，则认为寻路失败
        if ((m_lstslOpenLinkNodes.size() > nMaxSteps) || (nCurSteps >= nMaxSteps * 2)) {
            // jf_log_error("AStar::findPath(), too many links: {0}, current link id {1}", m_lstslOpenLinkNodes.size(), spCurrent->leNode.lLinkId.id);
            return false;
        }

        // // 如果元素太多，则删除最初的那些（否则会影响处理速度）
        // while (m_lstOpenLinkNodes.size() > m_lstOpenLinkNodes.max_size() - 1)
        // {
        //     m_lstOpenLinkNodes.sort(CompLinkFDsc);
        //     m_lstOpenLinkNodes.pop_front();
        // }
    }
    return false;
}
/**
 * A*计算（Lane路径规划）
 */
bool AStar::FindLanePath(const ShPtr_LaneNode &splnStartNode, const std::vector<ShPtr_LaneNode> &vctslEndNode,
                         const std::vector<LinkEndpoint> &vctleLinkPath, ShPtr_LaneNode &splnResult, LaneEndpoint &leOutEndNode,
                         int nMaxSteps) {
    // 用作最大寻路次数
    int nCurSteps = 0;
    // 临时保存临时的寻路结果，如果一个寻路尝试失败了，可以用它回退（因为Open集被局限在某个link的范围内，无法用于回退）
    std::list<std::list<ShPtr_LaneNode>> lstlsAllOpenLaneNodes = {};
    m_lstslOpenLaneNodes.push_back(ShPtr_LaneNode(splnStartNode));  // 置入起点,拷贝开辟一个节点，内外隔离
    // 如果Open集（根据link级别寻路结果，仅使用当前寻路节点所在link内的可到达车道）为空，则判断寻路失败
    while (!m_lstslOpenLaneNodes.empty()) {
        ++nCurSteps;
        ShPtr_LaneNode splnCurrent(new LaneNode());
        GetLeastFNode(splnCurrent);                    // 找到F值最小的车道
        m_lstslOpenLaneNodes.remove(splnCurrent);      // 从开启列表中删除
        m_lstslCloseLaneNodes.push_back(splnCurrent);  // 放到关闭列表
        // 当前link内的lane在清除前先存储
        if (0 != m_lstslOpenLaneNodes.size()) {
            lstlsAllOpenLaneNodes.push_back(m_lstslOpenLaneNodes);
        }
        // 1,找到当前车道可以到达的下一车道
        std::vector<ShPtr_LaneNode> vctspNexts = {};
        while (false == GetNextNodes(splnCurrent, vctleLinkPath, vctspNexts)) {
            if (true == m_lstslOpenLaneNodes.empty()) {
                // 如果临时车道集，则寻路失败
                if (false == lstlsAllOpenLaneNodes.empty()) {
                    // 找到了死胡同，回退
                    m_lstslOpenLaneNodes = lstlsAllOpenLaneNodes.back();
                    lstlsAllOpenLaneNodes.pop_back();
                } else if (true == m_lstslOpenLaneNodes.empty()) {
                    return false;
                }
            }
            GetLeastFNode(splnCurrent);                    // 找到F值最小的车道
            m_lstslOpenLaneNodes.remove(splnCurrent);      // 从开启列表中删除
            m_lstslCloseLaneNodes.push_back(splnCurrent);  // 放到关闭列表
            // 当前link内的lane在清除前先存储
            if (0 != m_lstslOpenLaneNodes.size()) {
                lstlsAllOpenLaneNodes.push_back(m_lstslOpenLaneNodes);
            }
        }
        // 当前link内的lane在清除
        m_lstslOpenLaneNodes.clear();
        for (auto &splnNode : vctspNexts) {
            // 2,对某一个车道，如果它不在开启列表中，加入到开启列表，设置当前车道为其父节点，计算F, G, H
            if (!IsInList(m_lstslOpenLaneNodes, splnNode)) {
                // 设置当前车道为其父节点
                splnNode->splnParent = splnCurrent;

                // 计算F, G, H
                splnNode->G = CalcG(splnCurrent, splnNode);
                splnNode->H = CalcH(splnNode, vctslEndNode[0]);
                splnNode->F = CalcF(splnNode);

                // jf_log_info("astar::findPath(), push lane: {0}, from: {1}, to: {2}", splnNode->hdlane.nId,
                m_lstslOpenLaneNodes.push_back(splnNode);
            }
                // 3，对某一个车道，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
            else {
                // 计算G值
                int nTempG = CalcG(splnCurrent, splnNode);
                // 如果比原来的小
                if (nTempG < splnNode->G) {
                    // 设置它的父节点为当前点,并更新G和F
                    splnNode->splnParent = splnCurrent;

                    splnNode->G = nTempG;
                    splnNode->F = CalcF(splnNode);
                }
            }

            // 检查当前结果
            for (ShPtr_LaneNode splnEndNode : vctslEndNode) {
                // 当前的Open集里面已经有了终点车道，寻路结束
                splnResult = IsInList(m_lstslOpenLaneNodes, splnEndNode);
                if (splnResult) {
                    leOutEndNode = splnEndNode->leNode;
                    return true;  // 返回列表里的节点指针，不要用原来传入的endNode指针，因为发生了深拷贝
                }
            }
        }

        // 如果元素太多，则认为寻路失败
        if ((m_lstslOpenLaneNodes.size() > nMaxSteps) || (nCurSteps >= nMaxSteps * 2)) {
            return false;
        }

        // // 如果元素太多，则删除最初的那些（否则会影响处理速度）
        // while (m_lstOpenLaneNodes.size() > 2000)
        // {
        //     m_lstOpenLaneNodes.sort(CompLaneFDsc);
        //     m_lstOpenLaneNodes.pop_front();
        // }
    }

    return false;
}


bool AStar::GetPath(const LinkEndpoint &leStartNode, const std::vector<LinkEndpoint> &vctleEndNode,
                    std::vector<LinkEndpoint> &vctleOutAsNodePath, LinkEndpoint &leOutEndNode,
                    int nMaxSteps /* = 65535 */, bool bHighPrecision /* = false*/) {
    // 清空临时开闭列表，防止重复执行GetPath导致结果异常
        m_lstslOpenLinkNodes.clear();
        m_lstslCloseLinkNodes.clear();

    // 寻找路径
    ShPtr_LinkNode splnStart(new LinkNode(leStartNode));
    ShPtr_LinkNode splnResult = {};
    if (true == bHighPrecision) {
        // 高精度寻路：将vctleEndNode里面的端点（按照所给点的距离做过排序）逐个寻路
        for (LinkEndpoint leEndNode : vctleEndNode) {
            // 将目前距离终点最近的lane加入寻路终点集
            std::vector<ShPtr_LinkNode> vctslEnd = {};
            ShPtr_LinkNode splnEnd(new LinkNode(leEndNode));
            vctslEnd.push_back(splnEnd);
            // 寻路：如果成功，则停止继续寻路
            if (true == FindLinkPath(splnStart, vctslEnd, splnResult, leOutEndNode, nMaxSteps)) {
                break;
            }
        }
    } else {
        // 低经度寻路：将vctleEndNode里面的端点都放入寻路结束里面，先找到哪一条就先使用哪一个结果
        std::vector<ShPtr_LinkNode> vctslEnd = {};
        // 将距离终点一定范围内的所有lane加入寻路终点集
        for (LinkEndpoint leEndNode : vctleEndNode) {
            ShPtr_LinkNode splnEnd(new LinkNode(leEndNode));
            vctslEnd.push_back(splnEnd);
        }
        if (true == FindLinkPath(splnStart, vctslEnd, splnResult, leOutEndNode, nMaxSteps)) {
            ;
        }
    }
    // 如果寻路失败，则返回错误
    if (nullptr == splnResult.get()) {
        return false;
    }

    // 返回路径, 生成返回数据
    while (splnResult.get() != nullptr) {
        vctleOutAsNodePath.push_back(splnResult->leNode);
        splnResult = splnResult->splnParent;
    }

    // 反转：由于保存在splnResult里面的数据从终点到起点的，所以需要反转
    std::reverse(vctleOutAsNodePath.begin(), vctleOutAsNodePath.end());
    for (size_t i = 0; i < vctleOutAsNodePath.size(); ++i) {
        // jf_log_info("AStar::GetPath(), lane {0}, id: {1}", i,
        // vctleOutAsNodePath[i].lLinkId.getLinkId());
    }

    // 如果生成失败或者车道数目过多，则返回失败
    if ((false == vctleOutAsNodePath.empty()) && (vctleOutAsNodePath.size() <= nMaxSteps)) {
        return true;
    } else {
        return false;
    }
}

bool AStar::GetPath(const LinkEndpoint &leStartNode, const LinkEndpoint &leEndNode, std::vector<LinkEndpoint> &vctleOutAsNodePath, int nMaxSteps /* = 65535 */) {
    std::vector<LinkEndpoint> vctleEndNode = {leEndNode};
    LinkEndpoint leOutEndNode = {};
    // 通用接口
    return GetPath(leStartNode, vctleEndNode, vctleOutAsNodePath, leOutEndNode, nMaxSteps);
}

bool AStar::GetPath(const LaneEndpoint &leStartNode, const std::vector<LaneEndpoint> &vctleEndNode, std::vector<LaneEndpoint> &vctleOutAsNodePath, LaneEndpoint &leOutEndNode, int nMaxSteps /* = 65535 */, bool bHighPrecision /* = false*/,
                    bool bLastLinkChangeLane /* = false*/) {

    // 清空临时开闭列表，防止重复执行GetPath导致结果异常RoadID

    m_lstslOpenLaneNodes.clear();
    m_lstslCloseLaneNodes.clear();

    // 1.先进行道路级别的寻路
    // 生成寻路终点
    std::vector<LinkEndpoint> vctleTempEndNodes = {};
    for (LaneEndpoint leEndNode : vctleEndNode) {
        vctleTempEndNodes.push_back(leEndNode);
    }
    // 道路级别的寻路
    std::vector<LinkEndpoint> vctleLinkPath = {};
    if (false == GetPath(leStartNode, vctleTempEndNodes, vctleLinkPath, leOutEndNode, nMaxSteps, bHighPrecision)) {
        return false;
    }

    // 2.再进行车道级别的寻路
    ShPtr_LaneNode splnStart(new LaneNode(leStartNode));
    ShPtr_LaneNode splnResult = {};
    // 如果需要尽量寻找所给终点附近的车道作为结束
    if (true == bHighPrecision) {
        // 按照距离所给终点距离由近至远开始寻路
        for (LaneEndpoint leEndNode : vctleEndNode) {
            std::vector<LaneEndpoint> vctleStartPre = {};
            // 为了允许起点或途径点所在的车道的车道变道，寻路会从起点前面的点开始，最后再将第一条车道删除
            if (true == HdLib::GetInstance()->GetPreEndpoints(leStartNode, vctleStartPre)) {
                std::vector<LinkEndpoint> vctlePreLinkPath = vctleLinkPath;
                vctlePreLinkPath.insert(vctlePreLinkPath.begin(), vctleStartPre[0]);
                // 将和起点车道相连的前面的车道集添加至寻路起点集
                ShPtr_LaneNode splnStartPre(new LaneNode(vctleStartPre[0]));
                // 寻路处理
                std::vector<ShPtr_LaneNode> vctslEnd = {};
                ShPtr_LaneNode splnEnd(new LaneNode(leEndNode));
                vctslEnd.push_back(splnEnd);
                if (true == FindLanePath(splnStartPre, vctslEnd, vctlePreLinkPath, splnResult, leOutEndNode, nMaxSteps)) {
                    break;
                }
            }
            // 当前已经是地图的起点，所以有向前寻找失败的情况
            else {
                // 将起点车道添加至寻路起点集
                ShPtr_LaneNode splnStart(new LaneNode(leStartNode));
                // 寻路处理
                std::vector<ShPtr_LaneNode> vctslEnd = {};
                ShPtr_LaneNode splnEnd(new LaneNode(leEndNode));
                vctslEnd.push_back(splnEnd);
                if (true == FindLanePath(splnStart, vctslEnd, vctleLinkPath, splnResult, leOutEndNode, nMaxSteps)) {
                    break;
                }
            }
        }
    }
    // 只需要寻路到终点附近的任意车道即可
    else {
        std::vector<ShPtr_LaneNode> vctslEnd = {};
        // 将终点附近的车道加入寻路结束集合
        for (LaneEndpoint leEndNode : vctleEndNode) {
            ShPtr_LaneNode splnEnd(new LaneNode(leEndNode));
            vctslEnd.push_back(splnEnd);
        }
        // 寻路处理
        if (true == FindLanePath(splnStart, vctslEnd, vctleLinkPath, splnResult, leOutEndNode, nMaxSteps)) {
            ;
        }
    }
    // 如果寻路失败
    if (nullptr == splnResult.get()) {
        return false;
    }

    // 返回路径, 将结果加入到vector数组中
    while (splnResult.get() != nullptr) {
        vctleOutAsNodePath.push_back(splnResult->leNode);
        splnResult = splnResult->splnParent;
    }

    // 去掉起点前的车道
    if (true == bHighPrecision) {
        if (vctleOutAsNodePath.at(vctleOutAsNodePath.size() - 1).lLinkId != leStartNode.lLinkId) {
            vctleOutAsNodePath.pop_back();
        }
    }

    // 反转, 由于结果是从结束到开始的顺序，所以需要反转
    std::reverse(vctleOutAsNodePath.begin(), vctleOutAsNodePath.end());

    // 如果寻路结果为空或者寻路结果太多，则返回失败
    if ((false == vctleOutAsNodePath.empty()) && (vctleOutAsNodePath.size() <= nMaxSteps)) {
        return true;
    } else {
        return false;
    }
}

bool AStar::GetPath(const LaneEndpoint &leStartNode, const LaneEndpoint &leEndNode, std::vector<LaneEndpoint> &vctleOutAsNodePath, int nMaxSteps /* = 65535 */) {
    // 将单条结束车道的需求转换，使用多条寻路结束车道的接口
    std::vector<LaneEndpoint> vctleEndNode = {leEndNode};
    LaneEndpoint leOutEndNode = {};
    return GetPath(leStartNode, vctleEndNode, vctleOutAsNodePath, leOutEndNode, nMaxSteps);
}

bool AStar::GetLeastFNode(ShPtr_LinkNode &splnNode) {
    // Open集不为空
    if (!m_lstslOpenLinkNodes.empty()) {
        // 获取第一个节点
        auto resNode = m_lstslOpenLinkNodes.front();
        // 遍历整个链表
        for (auto &lane : m_lstslOpenLinkNodes) {
            // 如果F值更小，则设置更小的节点为下一个比较节点
            if (lane->F < resNode->F) {
                resNode = lane;
            }
        }
        splnNode = resNode;

        return true;
    } else {
        return false;
    }
}

bool AStar::GetLeastFNode(ShPtr_LaneNode &splnNode) {
    // Open集不为空
    if (!m_lstslOpenLaneNodes.empty()) {
        // 获取第一个节点
        auto resNode = m_lstslOpenLaneNodes.front();
        // 遍历整个链表
        for (auto &lane : m_lstslOpenLaneNodes) {
            // 如果F值更小，则设置更小的节点为下一个比较节点
            if (lane->F < resNode->F) {
                resNode = lane;
            }
        }
        splnNode = resNode;

        return true;
    } else {
        return false;
    }
}

bool AStar::GetNextNodes(const ShPtr_LinkNode &splnCurrentNode, std::vector<ShPtr_LinkNode> &vctslOutNextNodes) {
    std::vector<LinkEndpoint> vctNextEndpoints = {};
    // 获取和当前link相连的下一个link的集合
    if (true == HdLib::GetInstance()->GetNextLinkEndpoints(splnCurrentNode->leNode, vctNextEndpoints)) {
        // 将这些link转换后放入返回数组
        for (LinkEndpoint leNextEp : vctNextEndpoints) {
            vctslOutNextNodes.push_back(ShPtr_LinkNode(new LinkNode(leNextEp)));
        }
        return true;
    } else {
        // jf_log_error("AStar::GetNextNodes(), no next vctleClosedList, current id {0}", splnCurrentNode->leNode.lLinkId.getLinkId());
        return false;
    }
}

bool AStar::GetNextNodes(const ShPtr_LaneNode &splnCurrentNode, const std::vector<LinkEndpoint> &vctleLinkPath, std::vector<ShPtr_LaneNode> &vctslOutNextNodes) {
    // 判断当前节点所在link级别寻路结果内的索引
    int nCurIndex = 0;
    for (size_t i = 0; i < vctleLinkPath.size(); i++) {
        if (splnCurrentNode->leNode.lLinkId == vctleLinkPath[i].lLinkId) {
            nCurIndex = i;
            break;
        }
    }

    // 寻找和当前车道相连的下一组车道
    std::vector<LaneEndpoint> vctNextEndpoints = {};
    if (true == HdLib::GetInstance()->GetNextLaneEndpoints(splnCurrentNode->leNode, vctNextEndpoints)) {
        for (LaneEndpoint leNextEp : vctNextEndpoints) {
            // 检查这些车道，只有link的ID属于link级别寻路nCurIndex的下一条link的车道才能被采用
            if (leNextEp.lLinkId == vctleLinkPath[nCurIndex + 1].lLinkId) {
                vctslOutNextNodes.push_back(ShPtr_LaneNode(new LaneNode(leNextEp)));
            }
        }
    }

    // 如果没有合适的车道，则返回false
    if (false == vctslOutNextNodes.empty()) {
        return true;
    } else {
        return false;
    }
}

ShPtr_LinkNode AStar::IsInList(const std::list<ShPtr_LinkNode> &lstslNode, const ShPtr_LinkNode &splnNode) {
    // 判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
    for (auto link : lstslNode) {
        if (link->leNode.lLinkId == splnNode->leNode.lLinkId) {
            return link;
        }
    }
    return NULL;
}

ShPtr_LaneNode AStar::IsInList(const std::list<ShPtr_LaneNode> &lstslNode, const ShPtr_LaneNode &splnNode) {
    // 判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
    for (auto lane : lstslNode) {
        if (lane->leNode.lLaneId == splnNode->leNode.lLaneId) {
            return lane;
        }
    }
    return NULL;
}

double AStar::CalcG(const ShPtr_LinkNode &splnStart, const ShPtr_LinkNode &splnNode) {
    // 计算G值，这里使用link长度
    int nExtraG = splnNode->leNode.dLength;
    int nParentG = splnNode->splnParent == NULL ? 0 : splnNode->splnParent->G;  //如果是初始节点，则其父节点是空
    return nParentG + nExtraG;
}

double AStar::CalcG(const ShPtr_LaneNode &splnStart, const ShPtr_LaneNode &splnNode) {
    // 计算G值，这里使用觉非公式
    int nExtraG = splnNode->leNode.dLength + 4000 / splnNode->leNode.dLength * pow(abs(splnNode->leNode.nStartLaneNum - splnNode->leNode.nEndLaneNum), 4);
    int nParentG = splnNode->splnParent == NULL ? 0 : splnNode->splnParent->G;  //如果是初始节点，则其父节点是空
    return nParentG + nExtraG;
}

double AStar::CalcH(const ShPtr_LinkNode &splnNode, const ShPtr_LinkNode &splnEnd) {
    // 此处取直线距离
    return DXYZ(P2XYZ(splnNode->leNode.ptEpNode), P2XYZ(splnEnd->leNode.ptEpNode));
}

double AStar::CalcH(const ShPtr_LaneNode &splnNode, const ShPtr_LaneNode &splnEnd) {
    // 此处取直线距离
    return DXYZ(P2XYZ(splnNode->leNode.ptEpNode), P2XYZ(splnEnd->leNode.ptEpNode));
}

double AStar::CalcF(const ShPtr_LinkNode &splnNode) {
    // 此处取G+H的值
    return splnNode->G + splnNode->H;
}

double AStar::CalcF(const ShPtr_LaneNode &splnNode) {
    // 此处取G+H的值
    return splnNode->G + splnNode->H;
}

bool CompLinkFDsc(const ShPtr_LinkNode &splnNode1, const ShPtr_LinkNode &spNode2) {
    // 比较两个节点的F值
    return splnNode1->F < spNode2->F;
}

bool CompLaneFDsc(const ShPtr_LaneNode &splnNode1, const ShPtr_LaneNode &spNode2) {
    // 比较两个节点的F值
    return splnNode1->F < spNode2->F;
}
