#ifndef LEMON_INSERTION_TSP_H

#define LEMON_INSERTION_TSP_H

#include <lemon/full_graph.h>

#include <lemon/path.h>

#include <lemon/maps.h>

#include <lemon/random.h>

#include <vector>

namespace lemon {

  namespace insertion_tsp_helper {

    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 InsertionTsp {

    private:

      GRAPH_TYPEDEFS(FullGraph);

    public:      

      typedef CM CostMap;

      typedef typename CM::Value Cost;

      InsertionTsp(const FullGraph &gr, const CostMap &cost) : 

            _gr(gr), _cost(cost) {}

      enum InsertionMethod {

        INSERT_NEAREST,

        INSERT_FARTHEST,

        INSERT_CHEAPEST,

        INSERT_RANDOM

      };     

      Cost run(InsertionMethod method = INSERT_FARTHEST) {

        switch (method) {

          case INSERT_NEAREST:

            start<InitTour<true>, NearestSelection, DefaultInsert>();

            break;

          case INSERT_FARTHEST:

            start<InitTour<false>, FarthestSelection, DefaultInsert>();

            break;

          case INSERT_CHEAPEST:

            start<InitTour<true>, CheapestSelection, CheapestInsert>();

            break;

          case INSERT_RANDOM:

            start<InitTour<true>, RandomSelection, DefaultInsert>();

            break;

        }

        return sum;

      }

      template <typename L>

      void tourNodes(L &container) {

        container(insertion_tsp_helper::vectorConvert<L>(nodesPath));

      }

      template <template <typename> class L>

      L<Node> tourNodes() {

        return insertion_tsp_helper::vectorConvert<L<Node> >(nodesPath);

      }

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

        return nodesPath;

      }

      Path<FullGraph> tour() {

        Path<FullGraph> p;

        if (nodesPath.size()<2)

          return p;

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

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

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

        return p;

      }

      Cost tourCost() {

        return sum;

      }

    private:

      template <bool MIN>

      class InitTour {

        public:

          InitTour(const FullGraph &gr, const CostMap &cost,

                   std::vector<Node> &used, std::vector<Node> &notused,

                   Cost &fullcost) :

              _gr(gr), _cost(cost), _used(used), _notused(notused),

              _fullcost(fullcost) {}

          std::vector<Node> init() const {

            Edge min = (MIN) ? mapMin(_gr, _cost) : mapMax(_gr, _cost);

            std::vector<Node> path;

            path.push_back(_gr.u(min));

            path.push_back(_gr.v(min));

            _used.clear();

            _notused.clear();

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

              if (n != _gr.u(min) && n != _gr.v(min)) {

                _notused.push_back(n);

              }

            }

            _used.push_back(_gr.u(min));

            _used.push_back(_gr.v(min));

            _fullcost = _cost[min]*2;

            return path;

          }

        private:

          const FullGraph &_gr;

          const CostMap &_cost;

          std::vector<Node> &_used;

          std::vector<Node> &_notused;

          Cost &_fullcost;

      };

      class NearestSelection {

        public:

          NearestSelection(const FullGraph &gr, const CostMap &cost,

                           std::vector<Node> &used, std::vector<Node> &notused) : 

              _gr(gr), _cost(cost), _used(used), _notused(notused) {}

          Node select() const {

            Cost insert_val =

              std::numeric_limits<Cost>::max();

            int insert_node = -1;

            for (unsigned int i=0; i<_notused.size(); ++i) {

              Cost min_val = _cost[_gr.edge(_notused[i], _used[0])];

              int min_node = 0;

              for (unsigned int j=1; j<_used.size(); ++j) {

                if (min_val > _cost[_gr.edge(_notused[i], _used[j])]) {

                    min_val = _cost[_gr.edge(_notused[i], _used[j])];

                    min_node = j;

                }

              }

              if (insert_val > min_val) {

                insert_val = min_val;

                insert_node = i;

              }

            }

            Node n = _notused[insert_node];

            _notused.erase(_notused.begin()+insert_node);

            return n;

          }

        private:

          const FullGraph &_gr;

          const CostMap &_cost;

          std::vector<Node> &_used;

          std::vector<Node> &_notused;

      };

      class FarthestSelection {

        public:

          FarthestSelection(const FullGraph &gr, const CostMap &cost,

                            std::vector<Node> &used, std::vector<Node> &notused) : 

              _gr(gr), _cost(cost), _used(used), _notused(notused) {}

          Node select() const {

            Cost insert_val =

              std::numeric_limits<Cost>::min();

            int insert_node = -1;

            for (unsigned int i=0; i<_notused.size(); ++i) {

              Cost min_val = _cost[_gr.edge(_notused[i], _used[0])];

              int min_node = 0;

              for (unsigned int j=1; j<_used.size(); ++j) {

                if (min_val > _cost[_gr.edge(_notused[i], _used[j])]) {

                  min_val = _cost[_gr.edge(_notused[i], _used[j])];

                  min_node = j;

                }

              }

              if (insert_val < min_val || insert_node == -1) {

                insert_val = min_val;

                insert_node = i;

              }

            }

            Node n = _notused[insert_node];

            _notused.erase(_notused.begin()+insert_node);

            return n;

          }

        private:

          const FullGraph &_gr;

          const CostMap &_cost;

          std::vector<Node> &_used;

          std::vector<Node> &_notused;

      };

      class CheapestSelection {

        private:

          Cost costDiff(Node u, Node v, Node w) const {

            return 

              _cost[_gr.edge(u, w)] +

              _cost[_gr.edge(v, w)] -

              _cost[_gr.edge(u, v)];

          }

        public:

          CheapestSelection(const FullGraph &gr, const CostMap &cost,

                            std::vector<Node> &used, std::vector<Node> &notused) : 

              _gr(gr), _cost(cost), _used(used), _notused(notused) {}

          Cost select() const {

            int insert_notused = -1;

            int best_insert_index = -1;

            Cost insert_val =

              std::numeric_limits<Cost>::max();

            for (unsigned int i=0; i<_notused.size(); ++i) {

              int min = i;

              int best_insert_tmp = 0;

              Cost min_val =

                costDiff(_used.front(), _used.back(), _notused[i]);

              for (unsigned int j=1; j<_used.size(); ++j) {

                Cost tmp =

                  costDiff(_used[j-1], _used[j], _notused[i]);

                if (min_val > tmp) {

                  min = i;

                  min_val = tmp;

                  best_insert_tmp = j;

                }

              }

              if (insert_val > min_val) {

                insert_notused = min;

                insert_val = min_val;

                best_insert_index = best_insert_tmp;

              }

            }

            _used.insert(_used.begin()+best_insert_index, _notused[insert_notused]);

            _notused.erase(_notused.begin()+insert_notused);

            return insert_val;

          }

        private:

          const FullGraph &_gr;

          const CostMap &_cost;

          std::vector<Node> &_used;

          std::vector<Node> &_notused;

      };

      class RandomSelection {

        public:

          RandomSelection(const FullGraph &, const CostMap &,

                            std::vector<Node> &, std::vector<Node> &notused) : 

                           _notused(notused) {

            rnd.seedFromTime();

          }

          Node select() const {

            const int index = rnd[_notused.size()];

            Node n = _notused[index];

            _notused.erase(_notused.begin()+index);

            return n;

          }

        private:

          std::vector<Node> &_notused;

      };

      class DefaultInsert {

        private:

          Cost costDiff(Node u, Node v, Node w) const {

            return 

              _cost[_gr.edge(u, w)] +

              _cost[_gr.edge(v, w)] -

              _cost[_gr.edge(u, v)];

          }

        public:

          DefaultInsert(const FullGraph &gr, const CostMap &cost,

                        std::vector<Node> &path, Cost &fullcost) : 

            _gr(gr), _cost(cost), _path(path), _fullcost(fullcost) {}

          void insert(Node n) const {

            int min = 0;

            Cost min_val =

              costDiff(_path.front(), _path.back(), n);

            for (unsigned int i=1; i<_path.size(); ++i) {

              Cost tmp = costDiff(_path[i-1], _path[i], n);

              if (tmp < min_val) {

                min = i;

                min_val = tmp;

              }

            }

            _path.insert(_path.begin()+min, n);

            _fullcost += min_val;

          }

        private:

          const FullGraph &_gr;

          const CostMap &_cost;

          std::vector<Node> &_path;

          Cost &_fullcost;

      };

      class CheapestInsert {

        TEMPLATE_GRAPH_TYPEDEFS(FullGraph);

        public:

          CheapestInsert(const FullGraph &, const CostMap &,

                         std::vector<Node> &, Cost &fullcost) : 

            _fullcost(fullcost) {}

          void insert(Cost diff) const {

            _fullcost += diff;

          }

        private:

          Cost &_fullcost;

      };  

      template <class InitFunctor, class SelectionFunctor, class InsertFunctor>

      void start() {

        InitFunctor init(_gr, _cost, nodesPath, notused, sum);

        SelectionFunctor selectNode(_gr, _cost, nodesPath, notused);

        InsertFunctor insertNode(_gr, _cost, nodesPath, sum);

        nodesPath = init.init();

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

          insertNode.insert(selectNode.select());

        }

        sum = _cost[ _gr.edge(nodesPath.front(), nodesPath.back()) ];

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

          sum += _cost[ _gr.edge(nodesPath[i], nodesPath[i+1]) ];

      }

      const FullGraph &_gr;

      const CostMap &_cost;

      std::vector<Node> notused;

      std::vector<Node> nodesPath;

      Cost sum;

  };

}; // namespace lemon

#endif