#pragma once
#include <QObject>

#include <functional>
#include <QPointF>

namespace OptimisationTools {
    /*!
     * \fn bool dijkstraAlgorithm(int startIndex, int endIndex, const QList<T> elements, QList<T> &elementPath, double(*distance)(const T &t1, const T &t2))
     * Calculates the shortest path between the elements stored in \a elements.
     * The \l {Dijkstra Algorithm} is used to find the shorest path.
     * Stores the result inside \a elementPath when sucessfull.
     * The function handle \a distance is used to calculate the distance between two elements. The distance must be positive.
     * Returns \c true if successful, \c false else.
     *
     * \sa QList
     */
    template <class T>
    bool dijkstraAlgorithm(const QList<T> &elements, int startIndex, int endIndex, QList<T> &elementPath, std::function<double(const T &, const T &)> distance)
    {
        if (    elements.isEmpty() || startIndex < 0
             || startIndex >= elements.size() || endIndex < 0
             || endIndex >= elements.size()) {
            return false;
        }
        //qWarning("optitools");
        // Each element of type T gets stuff into a Node
        /// @param distance is the distance between the Node and it's predecessor
        struct Node{
            T element;
            double distance = std::numeric_limits<qreal>::infinity();
            Node* predecessorNode = nullptr;
        };

        // The list with all Nodes (elements)
        QList<Node> nodeList;
        // This list will be initalized with (pointer to) all elements of nodeList.
        // Elements will be successively remove during the execution of the Dijkstra Algorithm.
        QList<Node*> workingSet;

        //append elements to node list
        for (int i = 0; i < elements.size(); i++) {
            Node node;
            node.element = elements[i];
            nodeList.append(node);
            workingSet.append(&nodeList[i]);
        }

        nodeList[startIndex].distance = 0;

        // 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 = 0;
            for (int i = 0; i < workingSet.size(); i++) {
                Node* node = workingSet.value(i);
                double dist = node->distance;
                if (dist < minDist) {
                    minDist = dist;
                    minDistIndex = i;
                }
            }
            Node* u = workingSet.takeAt(minDistIndex);

            //update distance
            for (int i = 0; i < workingSet.size(); i++) {
                Node* v = workingSet[i];
                double dist = distance(u->element, v->element);
                // is ther a alternative path which is shorter?
                double alternative = u->distance + dist;
                if (alternative < v->distance)  {
                    v->distance         = alternative;
                    v->predecessorNode  = u;
                }
            }

        }
        // end Djikstra Algorithm


        // reverse assemble path
        Node* node = &nodeList[endIndex];
        while (1) {
            if (node == nullptr) {
                if (elementPath[0] == elements[startIndex])// check if starting point was reached
                    break;
                return false;
            }
            elementPath.prepend(node->element);

            //Update Node
            node = node->predecessorNode;

        }
        return true;
    }
}