Bellman_Ford.H

/* Aleph-w

     / \  | | ___ _ __ | |__      __      __
    / _ \ | |/ _ \ '_ \| '_ \ ____\ \ /\ / / Data structures & Algorithms
   / ___ \| |  __/ |_) | | | |_____\ V  V /  version 1.9b
  /_/   \_\_|\___| .__/|_| |_|      \_/\_/   https://github.com/lrleon/Aleph-w
                 |_|         

  This file is part of Aleph-w library

  Copyright (c) 2002-2018 Leandro Rabindranath Leon & Alejandro Mujica

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
# ifndef BELLMAN_FORD_H
# define BELLMAN_FORD_H

# include <tpl_dynListQueue.H>
# include <tpl_dynSetTree.H>
# include <tpl_graph_utils.H>
# include <Tarjan.H>

namespace Aleph {

template <class GT, class Distance> struct Bellman_Ford_Node_Info
{
  int                              idx;
  typename Distance::Distance_Type acum;
};

    template <class GT, 
              class Distance                     = Dft_Dist<GT>, 
              template <class, class> class Ait  = Arc_Iterator,
              template <class, class> class NAit = Out_Iterator,
              class SA                           = Dft_Show_Arc<GT>> 
class Bellman_Ford
{
  typedef typename Distance::Distance_Type Distance_Type;

  using Node = typename GT::Node;
  using Arc = typename GT::Arc;

  struct Sni
  {
    Distance_Type acum;
  };

  struct Ni : public Sni
  {
    int idx; // índice en los arreglos de predecesores
  };

  Distance_Type & acum(Node * p) noexcept
  {
    return ((Sni*) NODE_COOKIE(p))->acum;
  }

  int & idx(Node * p) noexcept
  {
    return ((Ni*) NODE_COOKIE(p))->idx;
  }

  DynArray<typename GT::Arc*> arcs;
  const GT & g;
  const Distance_Type Inf;
  bool painted = false;
  Node * s = nullptr;
  SA sa;
  Distance dist;

  // Inicializa sin usar los arreglos
  void init_simple(Node * start)
  {
    typename GT::Node_Iterator it(g);
    for (int i = 0; it.has_curr(); ++i, it.next_ne())
      {
        auto p = it.get_curr_ne();
        g.reset_bit(p, Aleph::Spanning_Tree); // colocar bit en cero
        auto ptr = new Sni;
        ptr->acum = Inf;
        NODE_BITS(p).set_bit(Spanning_Tree, false); 
        NODE_COOKIE(p) = ptr;
    }
    s = start;
    acum(s) = 0;
    g.reset_arcs();
  }

  void init_with_indexes(Node * start)
  {
    arcs.reserve(g.get_num_nodes());
    typename GT::Node_Iterator it(g);
    for (int i = 0; it.has_curr(); ++i, it.next_ne())
      {
        arcs(i) = nullptr;
        auto p = it.get_curr_ne();
        g.reset_bit(p, Aleph::Spanning_Tree); // colocar bit en cero
        auto ptr = new Ni;
        ptr->acum = Inf;
        ptr->idx = i;
        NODE_BITS(p).set_bit(Spanning_Tree, false);
        NODE_BITS(p).set_bit(Depth_First, false); // indicates if it is in queue
        NODE_COOKIE(p) = ptr;
    }

    s = start;
    acum(s) = 0;

    g.reset_arcs();
  }

  // libera la memoria asociada a los cookies
  template <class Info_Type>
  void uninit() noexcept
  {
    for (typename GT::Node_Iterator it(g); it.has_curr(); it.next_ne())
      {
        auto p = it.get_curr_ne();
        delete (Info_Type*) NODE_COOKIE(p);
        NODE_COOKIE(p) = nullptr;
      }
  }

  bool check_painted_arcs() noexcept
  {
    auto num_arcs = 0;
    for (Ait<GT, SA> it(g, sa); it.has_curr(); it.next_ne())
      if (IS_ARC_VISITED(it.get_curr_ne(), Aleph::Spanning_Tree))
        ++num_arcs;

    return num_arcs == g.get_num_nodes() - 1 or num_arcs == g.get_num_nodes();
  }

public:

  Bellman_Ford(const GT & __g, Distance d = Distance(), SA __sa = SA())
    : g(const_cast<GT&>(__g)), Inf(std::numeric_limits<Distance_Type>::max()), 
      painted(false), sa(__sa), dist(d)
  {
    // empty
  }

private:
  
  void relax_arcs() noexcept
  {
    const size_t & n = g.vsize();
    for (int i = 0; i < n - 1; ++i)
      for (Ait<GT, SA> it(g, sa); it.has_curr(); it.next_ne()) 
        {
          auto arc = it.get_curr_ne();
          auto src  = g.get_src_node(arc);
          const auto & acum_src = acum(src);
          if (acum_src == Inf)
            continue;
          
          auto tgt = it.get_tgt_node_ne();
          auto w = dist(arc);
          auto sum = acum_src + w;
          auto & acum_tgt = acum(tgt);
          if (sum < acum_tgt) // relajar arco
            {
              const auto & index = idx(tgt);
              arcs(index)        = arc;
              acum_tgt           = sum;
            }
        }
  }

  static void 
  put_in_queue(DynListQueue<typename GT::Node*> & q, typename GT::Node * p)
  {
    if (IS_NODE_VISITED(p, Depth_First)) // is already inside the queue?
      return;
    NODE_BITS(p).set_bit(Depth_First, true);
    q.put(p);
  }

  static typename GT::Node * get_from_queue(DynListQueue<typename GT::Node*>& q)
  {
    auto ret = q.get();
    assert(IS_NODE_VISITED(ret, Depth_First));
    NODE_BITS(ret).set_bit(Depth_First, false);
    return ret;
  }

  void relax_arcs(typename GT::Node * src, DynListQueue<typename GT::Node*> & q)
  {
    for (NAit<GT, SA> it(src, sa); it.has_curr(); it.next_ne()) 
        {
          auto arc = it.get_curr_ne();
          auto src  = g.get_src_node(arc);
          const auto & acum_src = acum(src);
          if (acum_src == Inf)
            continue;

          auto tgt = g.get_tgt_node(arc);
          auto w = dist(arc);
          auto sum = acum_src + w;
          auto & acum_tgt = acum(tgt);
          if (sum < acum_tgt) // relajar arco
            {
              const auto & index = idx(tgt);
              arcs(index)        = arc;
              acum_tgt           = sum;
              put_in_queue(q, tgt);
            }
        }
  }

  void paint_tree() noexcept
  {    // pinta los nodos y arcos involucrados
    const auto & n = g.vsize();
    for (int i = 0; i < n; ++i)
      {
        auto arc = arcs(i);
        if (arc == nullptr)
          continue;
        
        ARC_BITS(arc).set_bit(Aleph::Spanning_Tree, true);
        auto src = (Node*) arc->src_node;
        auto tgt = (Node*) arc->tgt_node;
        NODE_BITS(src).set_bit(Aleph::Spanning_Tree, true);
        NODE_BITS(tgt).set_bit(Aleph::Spanning_Tree, true);
      }
    NODE_BITS(s).set_bit(Aleph::Spanning_Tree, true);

    assert(check_painted_arcs());

    painted = true;
  }

  bool last_relax_and_prepare_check_negative_cycle() noexcept
  {
    bool negative_cycle = false;
    for (Ait<GT, SA> it(g, sa); it.has_curr(); it.next_ne())
      {
        auto arc = it.get_curr_ne();
        auto src = g.get_src_node(arc);
        auto & acum_src = acum(src);
        if (acum_src == Inf)
          continue;

        auto tgt        = g.get_tgt_node(arc);
        auto d          = dist(arc);
        auto & acum_tgt = acum(tgt);
        auto sum        = acum_src + d;
        if (sum < acum_tgt)
          {
            negative_cycle = true; 
            const auto & index = idx(tgt);
            arcs(index)       = arc;
            acum_tgt          = sum;
          }
      }
    return negative_cycle;
  }

  bool last_relax_and_test_negative_cycle() noexcept
  {
    for (Ait<GT, SA> it(g, sa); it.has_curr(); it.next_ne())
      {
        auto arc = it.get_curr_ne();
        auto src  = g.get_src_node(arc);
        auto & acum_src = acum(src);
        if (acum_src == Inf)
          continue;

        auto tgt = g.get_tgt_node(arc);
        auto d = dist(arc);
        auto & acum_tgt = acum(tgt);
        auto sum = acum_src + d;
        if (sum < acum_tgt)
          return true;
      }
    return false;
  }

  void link_cookies_and_free(typename GT::Node * s) noexcept
  {
    uninit<Ni>();

    // construye los caminos invertidos hacia el nodo origen s
    const auto & n = g.vsize();
    for (int i = 0; i < n; ++i)
      {
        auto arc = arcs(i);
        if (arc == nullptr)
          continue;

        auto tgt = g.get_tgt_node(arc);
        NODE_COOKIE(tgt) = g.get_src_node(arc);
      }

    NODE_COOKIE(s) = nullptr; // por si acaso hay ciclo negativo 

    arcs.cut_ne();
  }

public:

  bool paint_spanning_tree(Node * start)
  {
    init_with_indexes(start); 
    
    relax_arcs();
    bool negative_cycle = last_relax_and_prepare_check_negative_cycle();
    paint_tree();
    link_cookies_and_free(s);

    return negative_cycle;
  }

  bool faster_paint_spanning_tree(Node * start) 
  {
    init_with_indexes(start);
    
    const auto & n = g.get_num_nodes();

    DynListQueue<typename GT::Node*> q;

    Node __sentinel;
    Node * sentinel = &__sentinel;

    put_in_queue(q, s);
    put_in_queue(q, sentinel);

    for (size_t i = 0; not q.is_empty(); )
      {
        auto src = get_from_queue(q);
        if (src == sentinel) // ¿se saca el centinela?
          {
            if (i++ > n) 
              break;

            put_in_queue(q, sentinel);
          }
        else
          relax_arcs(src, q);
      }

    bool negative_cycle = last_relax_and_prepare_check_negative_cycle();
    paint_tree();
    link_cookies_and_free(s);

    return negative_cycle;
  }

private:

  Node * create_dummy_node()
  {
    s = const_cast<GT&>(g).insert_node(typename GT::Node_Type());
    for (typename GT::Node_Iterator it(g); it.has_curr(); it.next_ne())
      {
        auto p = it.get_curr_ne();
        if (p == s)
          continue;
        auto a = const_cast<GT&>(g).insert_arc(s, p);
        Distance::set_zero(a);
      }
    return s;
  }

  template <class Info_Type>  
  void remove_dummy_node(Node * p) 
  {
    delete (Info_Type*) NODE_COOKIE(p);
    const_cast<GT&>(g).remove_node(p);
  }

public:

  bool has_negative_cycle(Node * start)
  {
    init_with_indexes(start);
    s = start;
    acum(s) = 0;
    
    relax_arcs();
    bool negative_cycle = last_relax_and_test_negative_cycle();
    uninit<Ni>();

    return negative_cycle;
  }

  bool has_negative_cycle()
  {
    create_dummy_node();
    auto ret = has_negative_cycle(s);
    remove_dummy_node<Ni>(s);

    return ret;
  }

private:

  Path<GT> search_negative_cycle_on_partial_graph()
  {
     GT aux = build_spanning_tree<GT>(arcs);

     // we map because Tarjan algorithm modifies cookies
     DynMapTree<Node*, Node*> table; 
     for (typename GT::Node_Iterator it(aux); it.has_curr(); it.next_ne())
       {
         auto p = it.get_curr_ne();
         table.insert(p, (Node*) NODE_COOKIE(p));
       }

     Path<GT> path(aux);
     if (Tarjan_Connected_Components<GT, NAit, SA>(sa).compute_cycle(aux, path))
       {
         Path<GT> ret(g);
         for (typename Path<GT>::Iterator it(path); it.has_current_node(); 
              it.next_ne())
           ret.append_directed((Node*) table.find(it.get_current_node_ne()));
         return ret;
       }
     
     return Path<GT>(g);
  }

public:

  Path<GT> test_negative_cycle(Node * start)
  {
    init_with_indexes(start);

    relax_arcs();
    bool negative_cycle = last_relax_and_prepare_check_negative_cycle();
    if (not negative_cycle)
      {
        link_cookies_and_free(s);
        return Path<GT>(g);
      }

    Path<GT> ret = search_negative_cycle_on_partial_graph();
    if (ret.is_empty())
      WARNING("Serious inconsistency. Bellman-Ford algorithm has detected\n"
              "a negative cycle, but Tarjan algorithm executed on partial\n"
              "graph has not found such cycle\n\n"
              "Be very careful, this is provably a bug");

    link_cookies_and_free(s);
    return ret;
  }

  Path<GT> test_negative_cycle()
  {
    auto start = create_dummy_node();
    auto ret_val = test_negative_cycle(start);
    remove_dummy_node<Ni>(start);
    return ret_val;
  }

       tuple<Path<GT>, size_t>
  search_negative_cycle(Node * start, double it_factor, const size_t step)
  {
    init_with_indexes(start);
    
    const auto & n = g.get_num_nodes();
    DynListQueue<typename GT::Node*> q;
    Node __sentinel;
    Node * sentinel = &__sentinel;
    put_in_queue(q, s); 
    put_in_queue(q, sentinel); 

    double threshold = it_factor*n;
    Path<GT> ret(g);

    size_t i = 0;
    while (not q.is_empty())
      {
        auto src = get_from_queue(q);
        if (src == sentinel)
          {
            if (i++ > n)
              break;

            put_in_queue(q, sentinel);
            if (i >= threshold) // must I search negative cycles?
              {
                ret = search_negative_cycle_on_partial_graph();
                if (not ret.is_empty()) // negative cycle found?
                  {
                    link_cookies_and_free(s);
                    return make_tuple(std::forward<Path<GT>>(ret), i);
                  }
                threshold += step;
              }
          }
        else
          relax_arcs(src, q);
      }

    bool negative_cycle = last_relax_and_prepare_check_negative_cycle();
    if (negative_cycle)
      {
        ret = search_negative_cycle_on_partial_graph();
        if (ret.is_empty())
          WARNING("Serious inconsistency. Bellman-Ford algorithm has detected\n"
                  "a negative cycle, but Tarjan algorithm executed on partial\n"
                  "graph has not found such cycle\n\n"
                  "Be very careful, this provably is a bug");
      }

    link_cookies_and_free(s);
    return make_tuple(std::forward<Path<GT>>(ret), i);
  }

  Path<GT> search_negative_cycle(Node * start)
  {
    init_with_indexes(start);
    
    const auto & n = g.get_num_nodes();
    DynListQueue<typename GT::Node*> q;
    Node __sentinel;
    Node * sentinel = &__sentinel;
    put_in_queue(q, s); 
    put_in_queue(q, sentinel); 

    Path<GT> ret(g);
    for (size_t i = 0; not q.is_empty(); /* nothing */)
      {
        auto src = get_from_queue(q);
        if (src == sentinel)
          {
            if (i++ > n)
              break;

            put_in_queue(q, sentinel);
          }
        else
          relax_arcs(src, q);
      }

    bool negative_cycle = last_relax_and_prepare_check_negative_cycle();
    if (negative_cycle)
      {
        ret = search_negative_cycle_on_partial_graph();
        if (ret.is_empty())
          WARNING("Serious inconsistency. Bellman-Ford algorithm has detected\n"
                  "a negative cycle, but Tarjan algorithm executed on partial\n"
                  "graph has not found such cycle\n\n"
                  "Be very careful, this provably is a bug");
      }

    link_cookies_and_free(s);

    return ret;
  }

  tuple<Path<GT>, size_t> search_negative_cycle(double it_factor,
                                                const size_t step)
  {
    auto start = create_dummy_node();
    auto ret_val = search_negative_cycle(start, it_factor, step);
    remove_dummy_node<Ni>(start);
    return ret_val;
  }

  Path<GT> search_negative_cycle()
  {
    auto start = create_dummy_node();
    auto ret_val = search_negative_cycle(start);
    remove_dummy_node<Ni>(start);
    return ret_val;
  }
  

  DynArray<Arc*> extract_mim_spanning_tree()
  {
    return arcs;
  }

  void build_tree(GT & tree, bool with_map = true)
  { 
    if (not painted and with_map)
      throw std::domain_error("Spanning tree has not been painted");

    clear_graph(tree);

    DynMapTree<Node*, Node*> table;
    for (typename GT::Node_Iterator it(g); it.has_curr(); it.next_ne())
      {
        auto gp = it.get_curr_ne();
        auto tp = tree.insert_node(gp->get_info());
        table.insert(gp, tp);
      }

    for (typename GT::Node_Iterator it(g); it.has_curr(); it.next_ne())
      {
        auto gtgt = it.get_curr_ne();
        auto gsrc = (Node*) NODE_COOKIE(gtgt);
        if (gsrc == nullptr)
          continue; // Se trata del nodo origen del árbol abarcador
        
        auto garc_ptr = g.arcs(gsrc).find_ptr([gsrc, gtgt] (Arc * a)
                {
                  return a->src_node == gsrc and a->tgt_node == gtgt;
                });
        assert(garc_ptr and IS_ARC_VISITED(*garc_ptr, Aleph::Spanning_Tree));
        auto garc = *garc_ptr;
        assert(garc->src_node == gsrc and garc->tgt_node == gtgt);
        auto tsrc_ptr = table.search(gsrc);
        auto ttgt_ptr = table.search(gtgt);
        assert(tsrc_ptr and ttgt_ptr);
        auto tarc = tree.insert_arc(tsrc_ptr->second, ttgt_ptr->second, 
                                    garc->get_info());
        if (with_map)
          GT::map_arcs(garc, tarc);
      }

    if (with_map)
      table.for_each([] (const auto & p) { GT::map_nodes(p.first, p.second); });
  }
   
  bool test_negative_cycle(typename GT::Node * s, Path<GT> & cycle)
  {
    cycle = test_negative_cycle(s);
    return not cycle.is_empty();
  }

  bool test_negative_cycle(Path<GT> & cycle)
  {
    cycle = test_negative_cycle();
    return not cycle.is_empty();
  }

  Distance_Type get_min_path(typename GT::Node * end, Path<GT> & path)
  {
    if (not painted)
      throw std::domain_error("Graph has not previously painted");
      
    return Aleph::get_min_path<GT, Distance>(s, end, path);
  }

  DynMapTree<Node*, Distance_Type> compute_nodes_weights()
  {
    create_dummy_node();

    init_with_indexes(s);
    
    const auto & n = g.get_num_nodes();

    DynListQueue<typename GT::Node*> q;

    Node __sentinel;
    Node * sentinel = &__sentinel;

    put_in_queue(q, s);
    put_in_queue(q, sentinel);

    bool negative_cycle = false;
    for (int i = 0; not q.is_empty() and not negative_cycle; /* nada */)
      {
        auto src = get_from_queue(q); 
        if (src == sentinel) // ¿se saca el centinela?
          {
            if (i++ > n) 
              break;
            else
              put_in_queue(q, sentinel);
          }
        else
          relax_arcs(src, q);
      }

    negative_cycle = last_relax_and_prepare_check_negative_cycle();
    remove_dummy_node<Ni>(s);

    DynMapTree<Node*, long> ret;

    // build mapping if there are no negative cycles
    if (not negative_cycle)
      for (auto it = g.get_node_it(); it.has_curr(); it.next_ne())
        {
          auto p = it.get_curr_ne();
          ret.insert(p, acum(p));
        }

    uninit<Ni>();
    arcs.cut();

    if (negative_cycle)
      throw domain_error("negative cycles detected");

    return ret;
  }
};

 template <class GT, 
           class Distance                     = Dft_Dist<GT>, 
           template <class, class> class Ait  = Arc_Iterator,
           template <class, class> class NAit = Node_Arc_Iterator,
           class SA                           = Dft_Show_Arc<GT>> 
struct Bellman_Ford_Negative_Cycle
{ 
  bool operator () (const GT & g, Path<GT> & path, 
                    Distance & d, SA & sa) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
      test_negative_cycle(path);
  }

  bool operator () (const GT & g, Path<GT> & path, 
                    Distance && d = Distance(), SA && sa = SA()) const
  {
    return Bellman_Ford<GT,Distance, Ait, NAit, SA>(g, d, sa).
      test_negative_cycle(path);
  }  

  bool operator () (const GT & g, typename GT::Node * s,
                    Path<GT> & path, Distance & d, SA & sa) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
      test_negative_cycle(s, path);
  }

  bool operator () (const GT & g, typename GT::Node * s,
                    Path<GT> & path, Distance && d = Distance(), 
                    SA && sa = SA()) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
      test_negative_cycle(s, path);
  }

  Path<GT> operator () (const GT & g, typename GT::Node * s, 
                        Distance & d, SA & sa, double it_factor = 0.7) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
      search_negative_cycle(s, it_factor);
  }

  Path<GT> operator () (const GT & g, typename GT::Node * s, 
                        Distance && d = Distance(), SA && sa = SA(), 
                        double it_factor = 0.7) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
      search_negative_cycle(s, it_factor);
  }

  Path<GT> operator () (const GT & g, Distance & d, SA & sa, 
                        double it_factor = 0.7) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
    search_negative_cycle(it_factor);
  }

  Path<GT> 
  operator () (const GT & g, Distance && d = Distance(), SA && sa = SA(), 
               double it_factor = 0.7) const
  {
    return Bellman_Ford<GT, Distance, Ait, NAit, SA>(g, d, sa).
    search_negative_cycle(it_factor);
  }
};


# undef NI
# undef IDX
# undef ACU
} // end namespace Aleph

# endif // BELLMAN_FORD_H