#ifndef LEMON_GREEDY_TSP_H

#define LEMON_GREEDY_TSP_H

#include <lemon/path.h>

#include <lemon/full_graph.h>

#include <lemon/unionfind.h>

#include <algorithm>

namespace lemon {

  namespace greedy_tsp_helper {

    template <typename CostMap>

    class KeyComp {

      typedef typename CostMap::Key Key;

      const CostMap &cost;

      public:

        KeyComp(const CostMap &_cost) : cost(_cost) {}

        bool operator() (const Key &a, const Key &b) const {

          return cost[a] < cost[b];

        }

    };

    template <class L>

    L vectorConvert(const std::vector<FullGraph::Node> &_path) {

      return L(_path.begin(), _path.end());

    }

    template <>

    std::vector<FullGraph::Node> vectorConvert(const std::vector<FullGraph::Node> &_path) {

      return _path;

    }

  }

  template <typename CM>

  class GreedyTsp {

    private:

      GRAPH_TYPEDEFS(FullGraph);

    public:

      typedef CM CostMap;

      typedef typename CM::Value Cost;

      GreedyTsp(const FullGraph &gr, const CostMap &cost) : _gr(gr), _cost(cost) {}  

      Cost run() {

        typedef UnionFind<FullGraph::NodeMap<int> > Union;

        _nodes.clear();

        std::vector<int> path;

        path.resize(_gr.nodeNum()*2, -1);

        std::vector<typename CostMap::Key> sorted_edges;

        sorted_edges.reserve(_gr.edgeNum());

        for (EdgeIt n(_gr); n != INVALID; ++n)

          sorted_edges.push_back(n);

        std::sort(sorted_edges.begin(), sorted_edges.end(), greedy_tsp_helper::KeyComp<CostMap>(_cost));

        FullGraph::NodeMap<int> nodemap(_gr);

        Union unionfind(nodemap);

        for (NodeIt n(_gr); n != INVALID; ++n)

          unionfind.insert(n);

        FullGraph::NodeMap<int> degree(_gr, 0);

        int nodesNum = 0, i = 0;

        while ( nodesNum != _gr.nodeNum()-1 ) {

          const Edge &e = sorted_edges[i];

          const Node u = _gr.u(e),

                     v = _gr.v(e);

          if (degree[u]<=1 && degree[v]<=1) {

            if (unionfind.join(u, v)) {

              ++degree[u];

              ++degree[v];

              ++nodesNum;

              const int uid = _gr.id(u),

                        vid = _gr.id(v);

              path[uid*2 + (path[uid*2]==-1 ? 0 : 1)] = vid;

              path[vid*2 + (path[vid*2]==-1 ? 0 : 1)] = uid;

            }

          }

          ++i;

        }

        for (int i=0, n=-1; i<_gr.nodeNum()*2; ++i) {

          if (path[i] == -1) {

            if (n==-1) {

              n = i;

            } else {

              path[n] = i/2;

              path[i] = n/2;

              break;

            }

          }

        }

        for (int i=0, j=0, last=-1; i!=_gr.nodeNum(); ++i) {

          _nodes.push_back(_gr.nodeFromId(j));

          if (path[2*j] != last) {

            last = j;

            j = path[2*j];

          } else {

            last = j;

            j = path[2*j+1];

          }

        }

        _sum = _cost[_gr.edge(_nodes.back(), _nodes.front())];

        for (unsigned int i=0; i<_nodes.size()-1; ++i)

          _sum += _cost[_gr.edge(_nodes[i], _nodes[i+1])];

        return _sum;

      }

      template <typename L>

      void tourNodes(L &container) {

        container(greedy_tsp_helper::vectorConvert<L>(_nodes));

      }

      template <template <typename> class L>

      L<Node> tourNodes() {

        return greedy_tsp_helper::vectorConvert<L<Node> >(_nodes);

      }

      const std::vector<Node>& tourNodes() {

        return _nodes;

      }

      Path<FullGraph> tour() {

        Path<FullGraph> p;

        if (_nodes.size()<2)

          return p;

        for (unsigned int i=0; i<_nodes.size()-1; ++i) {

          p.addBack(_gr.arc(_nodes[i], _nodes[i+1]));

        }

        p.addBack(_gr.arc(_nodes.back(), _nodes.front()));

        return p;

      }

      Cost tourCost() {

        return _sum;

      }

    private:

      const FullGraph &_gr;

      const CostMap &_cost;

      Cost _sum;

      std::vector<Node> _nodes;

  };

}; // namespace lemon

#endif