#include "OptimisationTools.h"

namespace OptimisationTools {
    namespace  {

    } // end anonymous namespace

    bool dijkstraAlgorithm(const int numElements, int startIndex, int endIndex, QVector<int> &elementPath, std::function<double (const int, const int)> distanceDij)
    {
        if (    numElements < 0
             || startIndex < 0
             || endIndex < 0
             || startIndex >= numElements
             || endIndex >= numElements
             || endIndex == startIndex) {
            return false;
        }
        // Node struct
        // predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex])
        // distance is the distance between the node and the start node
        // node number is stored by the position in nodeList
        struct Node{
            int predecessorIndex = -1;
            double distance = std::numeric_limits<qreal>::infinity();
        };

        // The list with all Nodes (elements)
        QVector<Node> nodeList(numElements);
        // This list will be initalized with indices referring to the elements of nodeList.
        // Elements will be successively remove during the execution of the Dijkstra Algorithm.
        QVector<int> workingSet(numElements);

        //append elements to node list
        for (int i = 0; i < numElements; ++i) workingSet[i] = i;


        nodeList[startIndex].distance = 0;

//        qDebug() << "nodeList" ;
//        for (auto node : nodeList) {
//            qDebug() << "predecessor: " << node.predecessorIndex;
//            qDebug() << "distance: " << node.distance;
//        }
//        qDebug() << "workingSet";
//        for (auto node : workingSet) {
//            qDebug() << "index: " << node;
//        }

        // Dijkstra Algorithm
        // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus
        while (workingSet.size() > 0) {
            // serach Node with minimal distance
            double minDist = std::numeric_limits<qreal>::infinity();
            int minDistIndex_WS = -1; // WS = workinSet
            for (int i = 0; i < workingSet.size(); ++i) {
                const int nodeIndex = workingSet.at(i);
                const double dist = nodeList.at(nodeIndex).distance;
                if (dist < minDist) {
                    minDist = dist;
                    minDistIndex_WS = i;
                }
            }
            if (minDistIndex_WS == -1)
                    return false;

            int indexU_NL = workingSet.takeAt(minDistIndex_WS); // NL = nodeList
            if (indexU_NL == endIndex) // shortest path found
                break;

            const double distanceU = nodeList.at(indexU_NL).distance;
            //update distance
            for (int i = 0; i < workingSet.size(); ++i) {
                int indexV_NL = workingSet[i]; // NL = nodeList
                Node* v = &nodeList[indexV_NL];
                double dist = distanceDij(indexU_NL, indexV_NL);
                // is ther an alternative path which is shorter?
                double alternative = distanceU + dist;
                if (alternative < v->distance)  {
                    v->distance         = alternative;
                    v->predecessorIndex = indexU_NL;
                }
            }

        }
        // end Djikstra Algorithm


        // reverse assemble path
        int e = endIndex;
        while (1) {
            if (e == -1) {
                if (elementPath[0] == startIndex)// check if starting point was reached
                    break;
                return false;
            }
            elementPath.prepend(e);

            //Update Node
            e = nodeList[e].predecessorIndex;

        }
        return true;
    }

} // end OptimisationTools namespace