#ifndef LEMON_OPT2_TSP_H
#define LEMON_OPT2_TSP_H

#include <vector>
#include <lemon/full_graph.h>
#include <lemon/path.h>

namespace lemon {
  
  namespace opt2_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 Opt2Tsp {
    private:
      GRAPH_TYPEDEFS(FullGraph);

    public:
      typedef CM CostMap;
      typedef typename CM::Value Cost;
      
    
      Opt2Tsp(const FullGraph &gr, const CostMap &cost) : _gr(gr), _cost(cost), 
            tmppath(_gr.nodeNum()*2) {
            
        for (int i=1; i<_gr.nodeNum()-1; ++i) {
          tmppath[2*i] = i-1;
          tmppath[2*i+1] = i+1;
        }
        tmppath[0] = _gr.nodeNum()-1;
        tmppath[1] = 1;
        tmppath[2*(_gr.nodeNum()-1)] = _gr.nodeNum()-2;
        tmppath[2*(_gr.nodeNum()-1)+1] = 0;
      }
      
      Opt2Tsp(const FullGraph &gr, const CostMap &cost, const std::vector<Node> &path) : 
              _gr(gr), _cost(cost), tmppath(_gr.nodeNum()*2) {

        for (unsigned int i=1; i<path.size()-1; ++i) {
          tmppath[2*_gr.id(path[i])] = _gr.id(path[i-1]);
          tmppath[2*_gr.id(path[i])+1] = _gr.id(path[i+1]);
        }
        
        tmppath[2*_gr.id(path[0])] = _gr.id(path.back());
        tmppath[2*_gr.id(path[0])+1] = _gr.id(path[1]);
        tmppath[2*_gr.id(path.back())] = _gr.id(path[path.size()-2]);
        tmppath[2*_gr.id(path.back())+1] = _gr.id(path.front());
      }

    private:
      Cost c(int u, int v) {
        return _cost[_gr.edge(_gr.nodeFromId(u), _gr.nodeFromId(v))];
      }
      
      class It {
        public:
          It(const std::vector<int> &path, int i=0) : tmppath(path), act(i), last(tmppath[2*act]) {}
          It(const std::vector<int> &path, int i, int l) : tmppath(path), act(i), last(l) {}

          int next_index() const {
            return (tmppath[2*act]==last)? 2*act+1 : 2*act;
          }
          
          int prev_index() const {
            return (tmppath[2*act]==last)? 2*act : 2*act+1;
          }
          
          int next() const {
            return tmppath[next_index()];
          }

          int prev() const {
            return tmppath[prev_index()];
          }
          
          It& operator++() {
            int tmp = act;
            act = next();
            last = tmp;
            return *this;
          }
          
          operator int() const {
            return act;
          }
          
        private:
          const std::vector<int> &tmppath;
          int act;
          int last;
      };

      bool check(std::vector<int> &path, It i, It j) {
        if (c(i, i.next()) + c(j, j.next()) > 
            c(i, j) + c(j.next(), i.next())) {

            path[ It(path, i.next(), i).prev_index() ] = j.next();
            path[ It(path, j.next(), j).prev_index() ] = i.next();

            path[i.next_index()] = j;
            path[j.next_index()] = i;

            return true;
        }
        return false;
      }
      
    public:
      
      Cost run() {
        _path.clear();

        if (_gr.nodeNum()>3) {

opt2_tsp_label:
          It i(tmppath);
          It j(tmppath, i, i.prev());
          ++j; ++j;
          for (; j.next()!=0; ++j) {
            if (check(tmppath, i, j))
              goto opt2_tsp_label;
          }
          
          for (++i; i.next()!=0; ++i) {
            It j(tmppath, i, i.prev());
            if (++j==0)
              break;
            if (++j==0)
              break;
            
            for (; j!=0; ++j) {
              if (check(tmppath, i, j))
                goto opt2_tsp_label;
            }
          }
        }

        It k(tmppath);
        _path.push_back(_gr.nodeFromId(k));
        for (++k; k!=0; ++k)
          _path.push_back(_gr.nodeFromId(k));

        

        _sum = _cost[ _gr.edge(_path.back(), _path.front()) ];
        for (unsigned int i=0; i<_path.size()-1; ++i)
          _sum += _cost[ _gr.edge(_path[i], _path[i+1]) ];
        return _sum;
      }

      
      template <typename L>
      void tourNodes(L &container) {
        container(opt2_helper::vectorConvert<L>(_path));
      }
      
      template <template <typename> class L>
      L<Node> tourNodes() {
        return opt2_helper::vectorConvert<L<Node> >(_path);
      }

      const std::vector<Node>& tourNodes() {
        return _path;
      }
      
      Path<FullGraph> tour() {
        Path<FullGraph> p;
        if (_path.size()<2)
          return p;

        for (unsigned int i=0; i<_path.size()-1; ++i) {
          p.addBack(_gr.arc(_path[i], _path[i+1]));
        }
        p.addBack(_gr.arc(_path.back(), _path.front()));
        return p;
      }
      
      Cost tourCost() {
        return _sum;
      }
      

  private:
    const FullGraph &_gr;
    const CostMap &_cost;
    Cost _sum;
    std::vector<int> tmppath;
    std::vector<Node> _path;
  };


}; // namespace lemon

#endif