#include <algorithm>
#include <cassert>

#include "algo.h"
#include "graph.h"
#include "node.h"
#include "result.h"
#include "types.h"

namespace
{

class ClosestNodeFirst
{
public:
    bool operator()( const Node* l, const Node* r ) const
    {
        return l->GetDistance() > r->GetDistance();
    }
};

class UnvisitedQueue : public node_pointer_container
{
public:
    UnvisitedQueue( Graph& graph )
    : cmp()
    {
        // Could be faster
        for( Graph::iterator it = graph.begin(); it != graph.end(); ++it )
        {
            push_back( &( *it ) );
        }
        std::make_heap( begin(), end(), cmp );
    }

    void Add( Node& node )
    {
        push_back( &node );
        std::push_heap( begin(), end(), cmp );
    }

    /**
     * Call this after changing the distance to this node.
     */
    void Reorder()
    {
        std::make_heap( begin(), end(), cmp );
    }

    void Pop()
    {
        std::pop_heap( begin(), end(), cmp );
        pop_back();
    }

    Node& Top()
    {
        return *( front() );
    }

    //void Write( std::ostream& out )
    //{
    //    for( const_iterator it = begin(); it != end(); ++it )
    //    {
    //        out << *it << "," << (*it)->GetDistance() << " ";
    //    }
    //    out << std::endl;
    //}

private:
    const ClosestNodeFirst cmp;
};


const_node_pointer_container build_path( const Node* end )
{
    const_node_pointer_container ret;

    while( end )
    {
        ret.push_back( end );
        end = end->GetPrevious();
    }

    std::reverse( ret.begin(), ret.end() );
    return ret;
}

}

Result dijkstra( Graph& graph, Node& start, Node& end )
{
    // Only needed if this is not a new graph - maybe we should
    // have a method like "Reset()" on Graph?
    //for( Graph::iterator it = graph.begin(); it != graph.end(); ++it )
    //{
    //    it->SetDistance( Node::INFINITE_DISTANCE );
    //    it->SetVisited( false );
    //}

    start.SetDistance( 0 );

    UnvisitedQueue unvisited( graph );

    Node* current = &start;

    while( current != &end )
    {
        Node::NeighbourList neighbours = current->Neighbours();
        for( Node::NeighbourList::iterator it = neighbours.begin();
            it != neighbours.end(); ++it )
        {
            Node::NeighbourList::value_type& nd = *it;
            Node& neigh = *( nd.first );
            if( !neigh.IsVisited() )
            {
                distance_t new_dist = nd.second + current->GetDistance();
                if( new_dist < neigh.GetDistance() )
                {
                    neigh.SetDistance( new_dist );
                    neigh.SetPrevious( *current );
                    unvisited.Reorder();
                }
            }
        }
        current->SetVisited( true );

        unvisited.Pop(); // Remove current from the queue

        assert( !unvisited.empty() );

        current = &unvisited.Top();
    }

    assert( current == &end );

    Result res;
    res.distance = current->GetDistance();
    res.path = build_path( current );

    return res;
}