//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_PUSH_RELABEL_MAX_FLOW_HPP
#define BOOST_PUSH_RELABEL_MAX_FLOW_HPP

#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <vector>
#include <list>
#include <iosfwd>
#include <algorithm> // for std::min and std::max

#include <boost/pending/queue.hpp>
#include <boost/limits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/named_function_params.hpp>

namespace boost {

  namespace detail {

   // This implementation is based on Goldberg's
   // "On Implementing Push-Relabel Method for the Maximum Flow Problem"
   // by B.V. Cherkassky and A.V. Goldberg, IPCO '95, pp. 157--171
   // and on the h_prf.c and hi_pr.c code written by the above authors.

   // This implements the highest-label version of the push-relabel method
   // with the global relabeling and gap relabeling heuristics.

   // The terms "rank", "distance", "height" are synonyms in
   // Goldberg's implementation, paper and in the CLR.  A "layer" is a
   // group of vertices with the same distance. The vertices in each
   // layer are categorized as active or inactive.  An active vertex
   // has positive excess flow and its distance is less than n (it is
   // not blocked).

    template <class Vertex>
    struct preflow_layer {
      std::list<Vertex> active_vertices;
      std::list<Vertex> inactive_vertices;
    };

    template <class Graph,
              class EdgeCapacityMap,    // integer value type
              class ResidualCapacityEdgeMap,
              class ReverseEdgeMap,
              class VertexIndexMap,     // vertex_descriptor -> integer
              class FlowValue>
    class push_relabel
    {
    public:
      typedef graph_traits<Graph> Traits;
      typedef typename Traits::vertex_descriptor vertex_descriptor;
      typedef typename Traits::edge_descriptor edge_descriptor;
      typedef typename Traits::vertex_iterator vertex_iterator;
      typedef typename Traits::out_edge_iterator out_edge_iterator;
      typedef typename Traits::vertices_size_type vertices_size_type;
      typedef typename Traits::edges_size_type edges_size_type;

      typedef preflow_layer<vertex_descriptor> Layer;
      typedef std::vector< Layer > LayerArray;
      typedef typename LayerArray::iterator layer_iterator;
      typedef typename LayerArray::size_type distance_size_type;

      typedef color_traits<default_color_type> ColorTraits;

      //=======================================================================
      // Some helper predicates

      inline bool is_admissible(vertex_descriptor u, vertex_descriptor v) {
        return get(distance, u) == get(distance, v) + 1;
      }
      inline bool is_residual_edge(edge_descriptor a) {
        return 0 < get(residual_capacity, a);
      }
      inline bool is_saturated(edge_descriptor a) {
        return get(residual_capacity, a) == 0;
      }

      //=======================================================================
      // Layer List Management Functions

      typedef typename std::list<vertex_descriptor>::iterator list_iterator;

      void add_to_active_list(vertex_descriptor u, Layer& layer) {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        layer.active_vertices.push_front(u);
        max_active = max BOOST_PREVENT_MACRO_SUBSTITUTION(get(distance, u), max_active);
        min_active = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(distance, u), min_active);
        layer_list_ptr[u] = layer.active_vertices.begin();
      }
      void remove_from_active_list(vertex_descriptor u) {
        layers[get(distance, u)].active_vertices.erase(layer_list_ptr[u]);
      }

      void add_to_inactive_list(vertex_descriptor u, Layer& layer) {
        layer.inactive_vertices.push_front(u);
        layer_list_ptr[u] = layer.inactive_vertices.begin();
      }
      void remove_from_inactive_list(vertex_descriptor u) {
        layers[get(distance, u)].inactive_vertices.erase(layer_list_ptr[u]);
      }

      //=======================================================================
      // initialization
      push_relabel(Graph& g_,
                   EdgeCapacityMap cap,
                   ResidualCapacityEdgeMap res,
                   ReverseEdgeMap rev,
                   vertex_descriptor src_,
                   vertex_descriptor sink_,
                   VertexIndexMap idx)
        : g(g_), n(num_vertices(g_)), capacity(cap), src(src_), sink(sink_),
          index(idx),
          excess_flow_data(num_vertices(g_)),
          excess_flow(excess_flow_data.begin(), idx),
          current_data(num_vertices(g_), out_edges(*vertices(g_).first, g_)),
          current(current_data.begin(), idx),
          distance_data(num_vertices(g_)),
          distance(distance_data.begin(), idx),
          color_data(num_vertices(g_)),
          color(color_data.begin(), idx),
          reverse_edge(rev),
          residual_capacity(res),
          layers(num_vertices(g_)),
          layer_list_ptr_data(num_vertices(g_),
                              layers.front().inactive_vertices.end()),
          layer_list_ptr(layer_list_ptr_data.begin(), idx),
          push_count(0), update_count(0), relabel_count(0),
          gap_count(0), gap_node_count(0),
          work_since_last_update(0)
      {
        vertex_iterator u_iter, u_end;
        // Don't count the reverse edges
        edges_size_type m = num_edges(g) / 2;
        nm = alpha() * n + m;

        // Initialize flow to zero which means initializing
        // the residual capacity to equal the capacity.
        out_edge_iterator ei, e_end;
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
          for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei) {
            put(residual_capacity, *ei, get(capacity, *ei));
          }

        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          vertex_descriptor u = *u_iter;
          put(excess_flow, u, 0);
          current[u] = out_edges(u, g);
        }

        bool overflow_detected = false;
        FlowValue test_excess = 0;

        out_edge_iterator a_iter, a_end;
        for (boost::tie(a_iter, a_end) = out_edges(src, g); a_iter != a_end; ++a_iter)
          if (target(*a_iter, g) != src)
            test_excess += get(residual_capacity, *a_iter);
        if (test_excess > (std::numeric_limits<FlowValue>::max)())
          overflow_detected = true;

        if (overflow_detected)
          put(excess_flow, src, (std::numeric_limits<FlowValue>::max)());
        else {
          put(excess_flow, src, 0);
          for (boost::tie(a_iter, a_end) = out_edges(src, g);
               a_iter != a_end; ++a_iter) {
            edge_descriptor a = *a_iter;
            vertex_descriptor tgt = target(a, g);
            if (tgt != src) {
              ++push_count;
              FlowValue delta = get(residual_capacity, a);
              put(residual_capacity, a, get(residual_capacity, a) - delta);
              edge_descriptor rev = get(reverse_edge, a);
              put(residual_capacity, rev, get(residual_capacity, rev) + delta);
              put(excess_flow, tgt, get(excess_flow, tgt) + delta);
            }
          }
        }
        max_distance = num_vertices(g) - 1;
        max_active = 0;
        min_active = n;

        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          vertex_descriptor u = *u_iter;
          if (u == sink) {
            put(distance, u, 0);
            continue;
          } else if (u == src && !overflow_detected)
            put(distance, u, n);
          else
            put(distance, u, 1);

          if (get(excess_flow, u) > 0)
            add_to_active_list(u, layers[1]);
          else if (get(distance, u) < n)
            add_to_inactive_list(u, layers[1]);
        }

      } // push_relabel constructor

      //=======================================================================
      // This is a breadth-first search over the residual graph
      // (well, actually the reverse of the residual graph).
      // Would be cool to have a graph view adaptor for hiding certain
      // edges, like the saturated (non-residual) edges in this case.
      // Goldberg's implementation abused "distance" for the coloring.
      void global_distance_update()
      {
        BOOST_USING_STD_MAX();
        ++update_count;
        vertex_iterator u_iter, u_end;
        for (boost::tie(u_iter,u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          put(color, *u_iter, ColorTraits::white());
          put(distance, *u_iter, n);
        }
        put(color, sink, ColorTraits::gray());
        put(distance, sink, 0);

        for (distance_size_type l = 0; l <= max_distance; ++l) {
          layers[l].active_vertices.clear();
          layers[l].inactive_vertices.clear();
        }

        max_distance = max_active = 0;
        min_active = n;

        Q.push(sink);
        while (! Q.empty()) {
          vertex_descriptor u = Q.top();
          Q.pop();
          distance_size_type d_v = get(distance, u) + 1;

          out_edge_iterator ai, a_end;
          for (boost::tie(ai, a_end) = out_edges(u, g); ai != a_end; ++ai) {
            edge_descriptor a = *ai;
            vertex_descriptor v = target(a, g);
            if (get(color, v) == ColorTraits::white()
                && is_residual_edge(get(reverse_edge, a))) {
              put(distance, v, d_v);
              put(color, v, ColorTraits::gray());
              current[v] = out_edges(v, g);
              max_distance = max BOOST_PREVENT_MACRO_SUBSTITUTION(d_v, max_distance);

              if (get(excess_flow, v) > 0)
                add_to_active_list(v, layers[d_v]);
              else
                add_to_inactive_list(v, layers[d_v]);

              Q.push(v);
            }
          }
        }
      } // global_distance_update()

      //=======================================================================
      // This function is called "push" in Goldberg's h_prf implementation,
      // but it is called "discharge" in the paper and in hi_pr.c.
      void discharge(vertex_descriptor u)
      {
        BOOST_ASSERT(get(excess_flow, u) > 0);
        while (1) {
          out_edge_iterator ai, ai_end;
          for (boost::tie(ai, ai_end) = current[u]; ai != ai_end; ++ai) {
            edge_descriptor a = *ai;
            if (is_residual_edge(a)) {
              vertex_descriptor v = target(a, g);
              if (is_admissible(u, v)) {
                ++push_count;
                if (v != sink && get(excess_flow, v) == 0) {
                  remove_from_inactive_list(v);
                  add_to_active_list(v, layers[get(distance, v)]);
                }
                push_flow(a);
                if (get(excess_flow, u) == 0)
                  break;
              }
            }
          } // for out_edges of i starting from current

          Layer& layer = layers[get(distance, u)];
          distance_size_type du = get(distance, u);

          if (ai == ai_end) {   // i must be relabeled
            relabel_distance(u);
            if (layer.active_vertices.empty()
                && layer.inactive_vertices.empty())
              gap(du);
            if (get(distance, u) == n)
              break;
          } else {              // i is no longer active
            current[u].first = ai;
            add_to_inactive_list(u, layer);
            break;
          }
        } // while (1)
      } // discharge()

      //=======================================================================
      // This corresponds to the "push" update operation of the paper,
      // not the "push" function in Goldberg's h_prf.c implementation.
      // The idea is to push the excess flow from from vertex u to v.
      void push_flow(edge_descriptor u_v)
      {
        vertex_descriptor
          u = source(u_v, g),
          v = target(u_v, g);

        BOOST_USING_STD_MIN();
        FlowValue flow_delta
          = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(excess_flow, u), get(residual_capacity, u_v));

        put(residual_capacity, u_v, get(residual_capacity, u_v) - flow_delta);
        edge_descriptor rev = get(reverse_edge, u_v);
        put(residual_capacity, rev, get(residual_capacity, rev) + flow_delta);

        put(excess_flow, u, get(excess_flow, u) - flow_delta);
        put(excess_flow, v, get(excess_flow, v) + flow_delta);
      } // push_flow()

      //=======================================================================
      // The main purpose of this routine is to set distance[v]
      // to the smallest value allowed by the valid labeling constraints,
      // which are:
      // distance[t] = 0
      // distance[u] <= distance[v] + 1   for every residual edge (u,v)
      //
      distance_size_type relabel_distance(vertex_descriptor u)
      {
        BOOST_USING_STD_MAX();
        ++relabel_count;
        work_since_last_update += beta();

        distance_size_type min_distance = num_vertices(g);
        put(distance, u, min_distance);

        // Examine the residual out-edges of vertex i, choosing the
        // edge whose target vertex has the minimal distance.
        out_edge_iterator ai, a_end, min_edge_iter;
        for (boost::tie(ai, a_end) = out_edges(u, g); ai != a_end; ++ai) {
          ++work_since_last_update;
          edge_descriptor a = *ai;
          vertex_descriptor v = target(a, g);
          if (is_residual_edge(a) && get(distance, v) < min_distance) {
            min_distance = get(distance, v);
            min_edge_iter = ai;
          }
        }
        ++min_distance;
        if (min_distance < n) {
          put(distance, u, min_distance);     // this is the main action
          current[u].first = min_edge_iter;
          max_distance = max BOOST_PREVENT_MACRO_SUBSTITUTION(min_distance, max_distance);
        }
        return min_distance;
      } // relabel_distance()

      //=======================================================================
      // cleanup beyond the gap
      void gap(distance_size_type empty_distance)
      {
        ++gap_count;

        distance_size_type r; // distance of layer before the current layer
        r = empty_distance - 1;

        // Set the distance for the vertices beyond the gap to "infinity".
        for (layer_iterator l = layers.begin() + empty_distance + 1;
             l < layers.begin() + max_distance; ++l) {
          list_iterator i;
          for (i = l->inactive_vertices.begin();
               i != l->inactive_vertices.end(); ++i) {
            put(distance, *i, n);
            ++gap_node_count;
          }
          l->inactive_vertices.clear();
        }
        max_distance = r;
        max_active = r;
      }

      //=======================================================================
      // This is the core part of the algorithm, "phase one".
      FlowValue maximum_preflow()
      {
        work_since_last_update = 0;

        while (max_active >= min_active) { // "main" loop

          Layer& layer = layers[max_active];
          list_iterator u_iter = layer.active_vertices.begin();

          if (u_iter == layer.active_vertices.end())
            --max_active;
          else {
            vertex_descriptor u = *u_iter;
            remove_from_active_list(u);

            discharge(u);

            if (work_since_last_update * global_update_frequency() > nm) {
              global_distance_update();
              work_since_last_update = 0;
            }
          }
        } // while (max_active >= min_active)

        return get(excess_flow, sink);
      } // maximum_preflow()

      //=======================================================================
      // remove excess flow, the "second phase"
      // This does a DFS on the reverse flow graph of nodes with excess flow.
      // If a cycle is found, cancel it.
      // Return the nodes with excess flow in topological order.
      //
      // Unlike the prefl_to_flow() implementation, we use
      //   "color" instead of "distance" for the DFS labels
      //   "parent" instead of nl_prev for the DFS tree
      //   "topo_next" instead of nl_next for the topological ordering
      void convert_preflow_to_flow()
      {
        vertex_iterator u_iter, u_end;
        out_edge_iterator ai, a_end;

        vertex_descriptor r, restart, u;

        std::vector<vertex_descriptor> parent(n);
        std::vector<vertex_descriptor> topo_next(n);

        vertex_descriptor tos(parent[0]),
          bos(parent[0]); // bogus initialization, just to avoid warning
        bool bos_null = true;

        // handle self-loops
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
          for (boost::tie(ai, a_end) = out_edges(*u_iter, g); ai != a_end; ++ai)
            if (target(*ai, g) == *u_iter)
              put(residual_capacity, *ai, get(capacity, *ai));

        // initialize
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          u = *u_iter;
          put(color, u, ColorTraits::white());
          parent[get(index, u)] = u;
          current[u] = out_edges(u, g);
        }
        // eliminate flow cycles and topologically order the vertices
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          u = *u_iter;
          if (get(color, u) == ColorTraits::white()
              && get(excess_flow, u) > 0
              && u != src && u != sink ) {
            r = u;
            put(color, r, ColorTraits::gray());
            while (1) {
              for (; current[u].first != current[u].second; ++current[u].first) {
                edge_descriptor a = *current[u].first;
                if (get(capacity, a) == 0 && is_residual_edge(a)) {
                  vertex_descriptor v = target(a, g);
                  if (get(color, v) == ColorTraits::white()) {
                    put(color, v, ColorTraits::gray());
                    parent[get(index, v)] = u;
                    u = v;
                    break;
                  } else if (get(color, v) == ColorTraits::gray()) {
                    // find minimum flow on the cycle
                    FlowValue delta = get(residual_capacity, a);
                    while (1) {
                      BOOST_USING_STD_MIN();
                      delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(delta, get(residual_capacity, *current[v].first));
                      if (v == u)
                        break;
                      else
                        v = target(*current[v].first, g);
                    }
                    // remove delta flow units
                    v = u;
                    while (1) {
                      a = *current[v].first;
                      put(residual_capacity, a, get(residual_capacity, a) - delta);
                      edge_descriptor rev = get(reverse_edge, a);
                      put(residual_capacity, rev, get(residual_capacity, rev) + delta);
                      v = target(a, g);
                      if (v == u)
                        break;
                    }

                    // back-out of DFS to the first saturated edge
                    restart = u;
                    for (v = target(*current[u].first, g); v != u; v = target(a, g)){
                      a = *current[v].first;
                      if (get(color, v) == ColorTraits::white()
                          || is_saturated(a)) {
                        put(color, target(*current[v].first, g), ColorTraits::white());
                        if (get(color, v) != ColorTraits::white())
                          restart = v;
                      }
                    }
                    if (restart != u) {
                      u = restart;
                      ++current[u].first;
                      break;
                    }
                  } // else if (color[v] == ColorTraits::gray())
                } // if (get(capacity, a) == 0 ...
              } // for out_edges(u, g)  (though "u" changes during loop)

              if ( current[u].first == current[u].second ) {
                // scan of i is complete
                put(color, u, ColorTraits::black());
                if (u != src) {
                  if (bos_null) {
                    bos = u;
                    bos_null = false;
                    tos = u;
                  } else {
                    topo_next[get(index, u)] = tos;
                    tos = u;
                  }
                }
                if (u != r) {
                  u = parent[get(index, u)];
                  ++current[u].first;
                } else
                  break;
              }
            } // while (1)
          } // if (color[u] == white && excess_flow[u] > 0 & ...)
        } // for all vertices in g

        // return excess flows
        // note that the sink is not on the stack
        if (! bos_null) {
          for (u = tos; u != bos; u = topo_next[get(index, u)]) {
            boost::tie(ai, a_end) = out_edges(u, g);
            while (get(excess_flow, u) > 0 && ai != a_end) {
              if (get(capacity, *ai) == 0 && is_residual_edge(*ai))
                push_flow(*ai);
              ++ai;
            }
          }
          // do the bottom
          u = bos;
          boost::tie(ai, a_end) = out_edges(u, g);
          while (get(excess_flow, u) > 0 && ai != a_end) {
            if (get(capacity, *ai) == 0 && is_residual_edge(*ai))
              push_flow(*ai);
            ++ai;
          }
        }

      } // convert_preflow_to_flow()

      //=======================================================================
      inline bool is_flow()
      {
        vertex_iterator u_iter, u_end;
        out_edge_iterator ai, a_end;

        // check edge flow values
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          for (boost::tie(ai, a_end) = out_edges(*u_iter, g); ai != a_end; ++ai) {
            edge_descriptor a = *ai;
            if (get(capacity, a) > 0)
              if ((get(residual_capacity, a) + get(residual_capacity, get(reverse_edge, a))
                   != get(capacity, a) + get(capacity, get(reverse_edge, a)))
                  || (get(residual_capacity, a) < 0)
                  || (get(residual_capacity, get(reverse_edge, a)) < 0))
              return false;
          }
        }

        // check conservation
        FlowValue sum;
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
          vertex_descriptor u = *u_iter;
          if (u != src && u != sink) {
            if (get(excess_flow, u) != 0)
              return false;
            sum = 0;
            for (boost::tie(ai, a_end) = out_edges(u, g); ai != a_end; ++ai)
              if (get(capacity, *ai) > 0)
                sum -= get(capacity, *ai) - get(residual_capacity, *ai);
              else
                sum += get(residual_capacity, *ai);

            if (get(excess_flow, u) != sum)
              return false;
          }
        }

        return true;
      } // is_flow()

      bool is_optimal() {
        // check if mincut is saturated...
        global_distance_update();
        return get(distance, src) >= n;
      }

      void print_statistics(std::ostream& os) const {
        os << "pushes:     " << push_count << std::endl
           << "relabels:   " << relabel_count << std::endl
           << "updates:    " << update_count << std::endl
           << "gaps:       " << gap_count << std::endl
           << "gap nodes:  " << gap_node_count << std::endl
           << std::endl;
      }

      void print_flow_values(std::ostream& os) const {
        os << "flow values" << std::endl;
        vertex_iterator u_iter, u_end;
        out_edge_iterator ei, e_end;
        for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
          for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
            if (get(capacity, *ei) > 0)
              os << *u_iter << " " << target(*ei, g) << " "
                 << (get(capacity, *ei) - get(residual_capacity, *ei)) << std::endl;
        os << std::endl;
      }

      //=======================================================================

      Graph& g;
      vertices_size_type n;
      vertices_size_type nm;
      EdgeCapacityMap capacity;
      vertex_descriptor src;
      vertex_descriptor sink;
      VertexIndexMap index;

      // will need to use random_access_property_map with these
      std::vector< FlowValue > excess_flow_data;
      iterator_property_map<typename std::vector<FlowValue>::iterator, VertexIndexMap> excess_flow;
      std::vector< std::pair<out_edge_iterator, out_edge_iterator> > current_data;
      iterator_property_map<
        typename std::vector< std::pair<out_edge_iterator, out_edge_iterator> >::iterator,
        VertexIndexMap> current;
      std::vector< distance_size_type > distance_data;
      iterator_property_map<
        typename std::vector< distance_size_type >::iterator,
        VertexIndexMap> distance;
      std::vector< default_color_type > color_data;
      iterator_property_map<
        std::vector< default_color_type >::iterator,
        VertexIndexMap> color;

      // Edge Property Maps that must be interior to the graph
      ReverseEdgeMap reverse_edge;
      ResidualCapacityEdgeMap residual_capacity;

      LayerArray layers;
      std::vector< list_iterator > layer_list_ptr_data;
      iterator_property_map<typename std::vector< list_iterator >::iterator, VertexIndexMap> layer_list_ptr;
      distance_size_type max_distance;  // maximal distance
      distance_size_type max_active;    // maximal distance with active node
      distance_size_type min_active;    // minimal distance with active node
      boost::queue<vertex_descriptor> Q;

      // Statistics counters
      long push_count;
      long update_count;
      long relabel_count;
      long gap_count;
      long gap_node_count;

      inline double global_update_frequency() { return 0.5; }
      inline vertices_size_type alpha() { return 6; }
      inline long beta() { return 12; }

      long work_since_last_update;
    };

  } // namespace detail

  template <class Graph,
            class CapacityEdgeMap, class ResidualCapacityEdgeMap,
            class ReverseEdgeMap, class VertexIndexMap>
  typename property_traits<CapacityEdgeMap>::value_type
  push_relabel_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     CapacityEdgeMap cap, ResidualCapacityEdgeMap res,
     ReverseEdgeMap rev, VertexIndexMap index_map)
  {
    typedef typename property_traits<CapacityEdgeMap>::value_type FlowValue;

    detail::push_relabel<Graph, CapacityEdgeMap, ResidualCapacityEdgeMap,
      ReverseEdgeMap, VertexIndexMap, FlowValue>
      algo(g, cap, res, rev, src, sink, index_map);

    FlowValue flow = algo.maximum_preflow();

    algo.convert_preflow_to_flow();

    BOOST_ASSERT(algo.is_flow());
    BOOST_ASSERT(algo.is_optimal());

    return flow;
  } // push_relabel_max_flow()

  template <class Graph, class P, class T, class R>
  typename detail::edge_capacity_value<Graph, P, T, R>::type
  push_relabel_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink,
     const bgl_named_params<P, T, R>& params)
  {
    return push_relabel_max_flow
      (g, src, sink,
       choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
       choose_pmap(get_param(params, edge_residual_capacity),
                   g, edge_residual_capacity),
       choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index)
       );
  }

  template <class Graph>
  typename property_traits<
    typename property_map<Graph, edge_capacity_t>::const_type
  >::value_type
  push_relabel_max_flow
    (Graph& g,
     typename graph_traits<Graph>::vertex_descriptor src,
     typename graph_traits<Graph>::vertex_descriptor sink)
  {
    bgl_named_params<int, buffer_param_t> params(0); // bogus empty param
    return push_relabel_max_flow(g, src, sink, params);
  }

} // namespace boost

#endif // BOOST_PUSH_RELABEL_MAX_FLOW_HPP


/******************************************************************************

  Various energy minimization algorithms for MRF models.

  Copyright (c) 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef OPTIMIZATION_HPP_40C9F0DC_7E18_4316_A594_63DAFD793CCA_
#define OPTIMIZATION_HPP_40C9F0DC_7E18_4316_A594_63DAFD793CCA_

#include <cstddef>
#include <cmath>
#include <ctime>
#include <limits>
#include <functional>
#include <boost/noncopyable.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/random.hpp>

#include "bo/mrf/mrf_2d.hpp"
#include "bo/mrf/type_values.hpp"

namespace bo {
namespace mrf {

// A basic class for MRF minimization algorithms. Takes (and captures sole ownership
// over it) an istance of TypePossibleValues<NodeType> to let its descendants sample
// possible values of type NodeType during optimization.
template <typename NodeType, typename DataType, typename RealType>
class MRF2DOptimizer: boost::noncopyable
{
public:
    typedef TypeValues<NodeType> NodePossibleLabels;
    typedef boost::scoped_ptr<NodePossibleLabels> NodePossibleLabelsPtr;

    MRF2DOptimizer(NodePossibleLabels* possible_values): values_(possible_values)
    { }

    // Performs the maximum-likelihood estimation (MLE) method for the given MRF.
    // This method is usually used to obtain initial configuration of the MRF.
    virtual void mle(MRF2D<NodeType, DataType, RealType>& mrf)
    {
        for (std::size_t col = 0; col < mrf.width(); ++col) {
            for (std::size_t row = 0; row < mrf.height(); ++row) {

                RealType min_energy = std::numeric_limits<RealType>::max();
                NodeType min_value = values_->next();

                values_->reset();
                std::size_t count = values_->count();

                while (count--)
                {
                    NodeType value = values_->next();
                    RealType energy = mrf.compute_local_likelihood(value, col, row);

                    if (energy < min_energy)
                    {
                        min_energy = energy;
                        min_value = value;
                    }
                }

                mrf(col, row) = min_value;
        }   }
    }

    virtual void next_iteration(MRF2D<NodeType, DataType, RealType>&) = 0;

    virtual ~MRF2DOptimizer()
    { }

protected:
    NodePossibleLabelsPtr values_;
};

// A class impementing iterated conditional modes minimization algorithm. As its
// parent, captures sole ownership over an istance of TypePossibleValues<NodeType>.
template <typename NodeType, typename DataType, typename RealType>
class ICM2D: public MRF2DOptimizer<NodeType, DataType, RealType>
{
public:
    typedef MRF2DOptimizer<NodeType, DataType, RealType> BaseType;

    typedef typename BaseType::NodePossibleLabels NodePossibleLabels;

    ICM2D(NodePossibleLabels* possible_values): BaseType(possible_values)
    { }

    void next_iteration(MRF2D<NodeType, DataType, RealType>& mrf)
    {
        for (std::size_t col = 0; col < mrf.width(); ++col) {
            for (std::size_t row = 0; row < mrf.height(); ++row) {
                RealType min_energy = std::numeric_limits<RealType>::max();
                NodeType min_value = this->values_->next();

                // Reset the possible labels so we can iterate through the whole
                // variety only once.
                this->values_->reset();
                std::size_t count = this->values_->count();

                while (count--)
                {
                    NodeType value = this->values_->next();
                    RealType energy = mrf.compute_local_energy(value, col, row);

                    if (energy < min_energy)
                    {
                        min_energy = energy;
                        min_value = value;
                    }
                }

                // Apply the "best" value.
                mrf(col, row) = min_value;
        }   }
    }
};

// A class implementing Metropolis dynamics algorithm minimization algorithm. As its
// parent, captures sole ownership over an istance of TypePossibleValues<NodeType>.
// Additionally takes initial temperature and decreasing multiplier for it, and
// precomputed probability for MMD modification if this modification was requested.
template <typename NodeType, typename DataType, typename RealType>
class MD2D: public MRF2DOptimizer<NodeType, DataType, RealType>
{
public:
    typedef MRF2DOptimizer<NodeType, DataType, RealType> BaseType;

    typedef typename BaseType::NodePossibleLabels NodePossibleLabels;
    typedef boost::uniform_real<RealType> RealDistribution;
    typedef boost::variate_generator<boost::mt19937, RealDistribution> Generator;

    MD2D(NodePossibleLabels* possible_values, RealType temp, RealType temp_delta,
         bool is_modified, RealType mmd_probability)
        : BaseType(possible_values), t_(temp), t_delta_(temp_delta),
        is_modified_(is_modified), mmd_log_probab_(std::log(mmd_probability)),
        rng_(boost::mt19937(static_cast<boost::uint32_t>(std::time(NULL))), RealDistribution(0, 1))
    { }

    void next_iteration(MRF2D<NodeType, DataType, RealType>& mrf)
    {
        // If a modified version of MD algo (MMD) is used, then the decision probability
        // is fixed and obtained as a parameter. Otherwise, it is generated every time.
        RealType decision_probab = mmd_log_probab_;

        // Update current temperature. See algorithm description for details.
        t_ *= t_delta_;

        // Do an inner iteration pixelwise.
        for (std::size_t col = 0; col < mrf.width(); ++col) {
            for (std::size_t row = 0; row < mrf.height(); ++row) {
                // Randomly generate a new state for the current pixel.
                NodeType old_value = mrf(col, row);
                NodeType new_value = this->values_->random();

                // if classical version is used, generate the decision probability.
                if (!is_modified_)
                    decision_probab = std::log(rng_());

                // Compute local energy and accept it or reject.
                if (decision_probab <=
                    (mrf.compute_local_energy(old_value, col, row) -
                     mrf.compute_local_energy(new_value, col, row)) / t_)
                {   // Accept new state.
                    mrf(col, row) = new_value;
                }
        }   }
    }

private:
    RealType t_;
    RealType t_delta_;
    bool is_modified_;
    RealType mmd_log_probab_;
    Generator rng_;
};

} // namespace mrf
} // namespace bo

#endif // OPTIMIZATION_HPP_40C9F0DC_7E18_4316_A594_63DAFD793CCA_


/**  \file ublas_extra.hpp \brief Extra functions for Ublas library */
/*
-----------------------------------------------------------------------------
   Copyright (C) 2011 Ruben Martinez-Cantin <rmcantin@unizar.es>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
-----------------------------------------------------------------------------
*/

#ifndef __UBLAS_EXTRA_HPP__
#define __UBLAS_EXTRA_HPP__

#include <typeinfo>
#include <boost/numeric/ublas/vector.hpp>

namespace bayesopt
{
  /// Extra utils: math functions, ublas helpers, etc.
  namespace utils
  {
    template<class V, class D>
    void append(V& vect, D element)
    {
      typedef typename V::value_type VD;
      assert(typeid(VD) == typeid(D));

      // This method is super inefficient but there seems to be the uBlas style.
      const size_t size = vect.size();
      vect.resize(size+1,true);
      vect(size) = element;
    };

    template<class V, class I>
    void erase(V& vect, I begin)
    {
      typedef typename V::iterator VI;
      assert(typeid(VI) == typeid(I));

      for(VI it = begin; it != vect.end()-1; ++it)
	{
	  *it = *(it+1);
	}
      vect.resize(vect.size()-1);
    };

    template<class M>
    void erase_column(M& mat, size_t pos)
    {
      for(size_t i = pos; i < mat.size2()-1; ++i)
	{
	  column(mat,i) = column(mat,i+1);
	}
      mat.resize(mat.size1(),mat.size2()-1);
    };

    template<class M, class V>
    void add_to_diagonal(M& mat, const V& vec)
    {
      assert(mat.size1()==mat.size2());
      assert(mat.size1()==vec.size());
      const size_t ll = vec.size();
      for(size_t ii = 0; ii < ll; ++ii)
	{
	  mat(ii,ii) += vec(ii);
	}
    };

    boost::numeric::ublas::vector<double> array2vector(const double array[],
						       const size_t n);

  } //  namespace utils
} //namespace bayesopt

#endif


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "edge_labeler.h"

#include <Eigen/Dense>

#include "g2o/stuff/unscented.h"

namespace g2o {

using namespace std;
using namespace Eigen;

typedef SigmaPoint<VectorXd> MySigmaPoint;

EdgeLabeler::EdgeLabeler(SparseOptimizer* optimizer) { _optimizer = optimizer; }

int EdgeLabeler::labelEdges(std::set<OptimizableGraph::Edge*>& edges) {
  // assume the system is "solved"
  // compute the sparse pattern of the inverse
  std::set<std::pair<int, int> > pattern;
  for (std::set<OptimizableGraph::Edge*>::iterator it = edges.begin();
       it != edges.end(); ++it) {
    augmentSparsePattern(pattern, *it);
  }

  SparseBlockMatrix<MatrixXd> spInv;

  bool result = computePartialInverse(spInv, pattern);
  // cerr << "partial inverse computed = " << result << endl;
  // cerr << "non zero blocks" << spInv.nonZeroBlocks() << endl;

  if (!result) {
    return -1;
  }
  int count = 0;
  for (std::set<OptimizableGraph::Edge*>::iterator it = edges.begin();
       it != edges.end(); ++it) {
    count += labelEdge(spInv, *it) ? 1 : 0;
  }
  return count;
}

void EdgeLabeler::augmentSparsePattern(std::set<std::pair<int, int> >& pattern,
                                       OptimizableGraph::Edge* e) {
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* v =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = v->hessianIndex();
    if (ti == -1) continue;
    for (size_t j = i; j < e->vertices().size(); j++) {
      const OptimizableGraph::Vertex* v =
          (const OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = v->hessianIndex();
      if (tj == -1) continue;
      if (tj < ti) swap(ti, tj);
      pattern.insert(std::make_pair(ti, tj));
    }
  }
}

bool EdgeLabeler::computePartialInverse(
    SparseBlockMatrix<MatrixXd>& spinv,
    const std::set<std::pair<int, int> >& pattern) {
  std::vector<std::pair<int, int> > blockIndices(pattern.size());
  // Why this does not work???
  // std::copy(pattern.begin(),pattern.end(),blockIndices.begin());

  int k = 0;
  for (std::set<std::pair<int, int> >::const_iterator it = pattern.begin();
       it != pattern.end(); ++it) {
    blockIndices[k++] = *it;
  }

  // cerr << "sparse pattern contains " << blockIndices.size() << " blocks" <<
  // endl;
  return _optimizer->computeMarginals(spinv, blockIndices);
}

bool EdgeLabeler::labelEdge(const SparseBlockMatrix<MatrixXd>& spinv,
                            OptimizableGraph::Edge* e) {
  Eigen::Map<MatrixXd> info(e->informationData(), e->dimension(),
                            e->dimension());
  // cerr << "original information matrix" << endl;
  // cerr << info << endl;

  int maxDim = 0;
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* v =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = v->hessianIndex();
    if (ti == -1) continue;
    maxDim += v->minimalEstimateDimension();
  }

  // cerr << "maxDim= " << maxDim << endl;
  MatrixXd cov(maxDim, maxDim);
  int cumRow = 0;
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* vr =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = vr->hessianIndex();
    if (ti > -1) {
      int cumCol = 0;
      for (size_t j = 0; j < e->vertices().size(); j++) {
        const OptimizableGraph::Vertex* vc =
            (const OptimizableGraph::Vertex*)e->vertices()[j];
        int tj = vc->hessianIndex();
        if (tj > -1) {
          // cerr << "ti=" << ti << " tj=" << tj
          //    << " cumRow=" << cumRow << " cumCol=" << cumCol << endl;
          if (ti <= tj) {
            assert(spinv.block(ti, tj));
            // cerr << "cblock_ptr" << spinv.block(ti, tj) << endl;
            // cerr << "cblock.size=" << spinv.block(ti, tj)->rows() << "," <<
            // spinv.block(ti, tj)->cols() << endl; cerr << "cblock" << endl;
            // cerr << *spinv.block(ti, tj) << endl;
            cov.block(cumRow, cumCol, vr->minimalEstimateDimension(),
                      vc->minimalEstimateDimension()) = *spinv.block(ti, tj);
          } else {
            assert(spinv.block(tj, ti));
            // cerr << "cblock.size=" << spinv.block(tj, ti)->cols() << "," <<
            // spinv.block(tj, ti)->rows() << endl; cerr << "cblock" << endl;
            // cerr << spinv.block(tj, ti)->transpose() << endl;
            cov.block(cumRow, cumCol, vr->minimalEstimateDimension(),
                      vc->minimalEstimateDimension()) =
                spinv.block(tj, ti)->transpose();
          }
          cumCol += vc->minimalEstimateDimension();
        }
      }
      cumRow += vr->minimalEstimateDimension();
    }
  }

  // cerr << "covariance assembled" << endl;
  // cerr << cov << endl;
  // now cov contains the aggregate marginals of the state variables in the edge
  VectorXd incMean(maxDim);
  incMean.fill(0);
  std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> >
      incrementPoints;
  if (!sampleUnscented(incrementPoints, incMean, cov)) {
    cerr << "sampleUnscented fail" << endl;
    return false;
  }
  // now determine the zero-error measure by applying the error function of the
  // edge with a zero measurement
  // TODO!!!
  bool smss = e->setMeasurementFromState();
  if (!smss) {
    cerr << "FATAL: Edge::setMeasurementFromState() not implemented" << endl;
  }
  assert(smss && "Edge::setMeasurementFromState() not implemented");

  // std::vector<MySigmaPoint> globalPoints(incrementPoints.size());
  std::vector<MySigmaPoint, Eigen::aligned_allocator<MySigmaPoint> >
      errorPoints(incrementPoints.size());

  // for each sigma point, project it to the global space, by considering those
  // variables that are involved
  // cerr << "sigma points are extracted, remapping to measurement space" <<
  // endl;
  for (size_t i = 0; i < incrementPoints.size(); i++) {
    int cumPos = 0;
    // VectorXd globalPoint(maxDim);

    // push all the "active" state variables
    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->push();
    }

    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->oplus(&incrementPoints[i]._sample[cumPos]);
      // assert(vr->getMinimalEstimateData(&globalPoint[cumPos]) &&
      // "Vertex::getMinimalEstimateData(...) not implemented");
      cumPos += vr->minimalEstimateDimension();
    }

    // construct the sigma point in the global space
    // globalPoints[i]._sample=globalPoint;
    // globalPoints[i]._wi=incrementPoints[i]._wi;
    // globalPoints[i]._wp=incrementPoints[i]._wp;

    // construct the sigma point in the error space
    e->computeError();
    Map<VectorXd> errorPoint(e->errorData(), e->dimension());

    errorPoints[i]._sample = errorPoint;
    errorPoints[i]._wi = incrementPoints[i]._wi;
    errorPoints[i]._wp = incrementPoints[i]._wp;

    // pop all the "active" state variables
    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->pop();
    }
  }

  // reconstruct the covariance of the error by the sigma points
  MatrixXd errorCov(e->dimension(), e->dimension());
  VectorXd errorMean(e->dimension());
  reconstructGaussian(errorMean, errorCov, errorPoints);

  // info=errorCov.inverse();
  //  cerr << "remapped information matrix" << endl;
  //  cerr << info << endl;
  return true;
}

}  // namespace g2o


#include <Core/Geometry/Approximation/VariationalShapeApproximation.hpp>

#include <Core/Geometry/TriangleOperation.hpp>
#include <Core/Utils/Log.hpp>
#include <Eigen/Eigenvalues>
#include <random>
#include <set>

namespace Ra {
namespace Core {
namespace Geometry {

//////////////////////////////////////////////////////////////////////////////
// CONSTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline VariationalShapeApproximationBase<K_Region>::VariationalShapeApproximationBase(
    const Mesh& mesh ) :
    m_mesh( mesh ),
    m_queue(),
    m_region(),
    m_proxy(),
    m_init( false ) {}

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline VariationalShapeApproximationBase<K_Region>::~VariationalShapeApproximationBase() {}

//////////////////////////////////////////////////////////////////////////////
// INIT
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::init() {
    this->compute_data();
    this->compute_seed();
    this->m_init = true;
}

template <uint K_Region>
inline bool VariationalShapeApproximationBase<K_Region>::initialized() const {
    return m_init;
}

//////////////////////////////////////////////////////////////////////////////
// EXECUTION
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::exec( const uint iteration ) {
    if ( !this->initialized() )
    {
        CORE_WARN_IF( false, "V.S.A. NOT INITIALIZED" );
        return;
    }
    LOG( Utils::logDEBUG ) << "Computing V.S.A. ...";
    for ( uint i = 0; i < iteration; ++i )
    {
        this->proxy_fitting();
        this->geometry_partitioning();
    }
    LOG( Utils::logDEBUG ) << "V.S.A. completed.";
}

template <uint K_Region>
template <uint Iteration>
inline void VariationalShapeApproximationBase<K_Region>::exec() {
    if ( !this->initialized() )
    {
        CORE_WARN_IF( false, "V.S.A. NOT INITIALIZED" );
        return;
    }
    LOG( Utils::logDEBUG ) << "Computing V.S.A. ...";
    for ( uint i = 0; i < Iteration; ++i )
    {
        this->proxy_fitting();
        this->geometry_partitioning();
    }
    LOG( Utils::logDEBUG ) << "V.S.A. completed.";
}

//////////////////////////////////////////////////////////////////////////////
// REGION
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline const typename VariationalShapeApproximationBase<K_Region>::Region&
VariationalShapeApproximationBase<K_Region>::region( const uint i ) const {
    return m_region.at( i );
}

//////////////////////////////////////////////////////////////////////////////
// PROXY
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline const typename VariationalShapeApproximationBase<K_Region>::Proxy&
VariationalShapeApproximationBase<K_Region>::proxy( const uint i ) const {
    return m_proxy.at( i );
}

//////////////////////////////////////////////////////////////////////////////
// COLOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::create_region_color() {
    for ( uint i = 0; i < K; ++i )
    {
        const Scalar q = Scalar( i ) / static_cast<Scalar>( K - 1 );
        for ( const auto& T : this->m_region[i] )
        {
            this->m_fvalue[T] = q;
        }
    }
}

template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::create_energy_color() {
    for ( uint i = 0; i < K; ++i )
    {
        for ( const auto& T : this->m_region[i] )
        {
            this->m_fvalue[T] = this->E( T, this->m_proxy[i] );
        }
    }
}

template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::shuffle_regions() {
    // This function was made in order to have a chance for neighboring regions
    // to have distant IDs, hence having a different color.
    std::array<uint, K> id;
    RegionList tmp_region;
    ProxyList tmp_proxy;
    for ( uint i = 0; i < K; ++i )
    {
        id[i] = i;
    }
    std::random_shuffle( id.begin(), id.end() );
    for ( uint i = 0; i < K; ++i )
    {
        tmp_region[i] = this->m_region[id[i]];
        tmp_proxy[i] = this->m_proxy[id[i]];
    }
    std::swap( tmp_region, this->m_region );
    std::swap( tmp_proxy, this->m_proxy );
    for ( uint i = 0; i < K; ++i )
    {
        for ( const auto& T : this->m_region[i] )
        {
            this->m_fregion[T] = i;
        }
    }
}

//////////////////////////////////////////////////////////////////////////////
// DATA
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::compute_data() {
    const uint size = this->m_mesh.m_triangles.size();
    this->m_fbary.resize( size );
    this->m_fnormal.resize( size );
    this->m_farea.resize( size );
    this->m_fregion.resize( size, -1 );
    this->m_fvisited.resize( size, false );
    this->m_fvalue.resize( 0 );
    this->m_fadj.resize( size );

    std::vector<std::vector<TriangleIdx>> vadj( this->m_mesh.m_vertices.size() );
    for ( uint t = 0; t < size; ++t )
    {
        const Triangle& T = this->m_mesh.m_triangles[t];
        const uint i = T[0];
        const uint j = T[1];
        const uint k = T[2];
        const Math::Vector3 v[3] = {this->m_mesh.m_vertices[i], this->m_mesh.m_vertices[j],
                              this->m_mesh.m_vertices[k]};
        this->m_fbary[t] = triangleBarycenter( v[0], v[1], v[2] );
        this->m_fnormal[t] = triangleNormal( v[0], v[1], v[2] );
        this->m_farea[t] = triangleArea( v[0], v[1], v[2] );
        vadj[i].push_back( t );
        vadj[j].push_back( t );
        vadj[k].push_back( t );
    }

    for ( const auto& list : vadj )
    {
        for ( uint i = 0; i < list.size(); ++i )
        {
            for ( uint j = i + 1; j < list.size(); ++j )
            {
                this->m_fadj[list[i]].insert( list[j] );
                this->m_fadj[list[j]].insert( list[i] );
            }
        }
    }
}

//////////////////////////////////////////////////////////////////////////////
// SEED
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::compute_seed() {
    std::set<TriangleIdx> t;
    std::default_random_engine g( time( 0 ) );
    std::uniform_int_distribution<TriangleIdx> rnd( 0, this->m_mesh.m_triangles.size() - 1 );
    while ( t.size() != K )
    {
        t.insert( rnd( g ) );
    }
    for ( uint i = 0; i < K; ++i )
    {
        m_region[i].push_back( *t.begin() );
        t.erase( t.begin() );
    }
}

//////////////////////////////////////////////////////////////////////////////
// GEOMETRY PARTITIONING
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximationBase<K_Region>::geometry_partitioning() {
    // Reset the visited flag
    for ( TriangleIdx T = 0; T < this->m_mesh.m_triangles.size(); ++T )
    {
        this->m_fvisited[T] = false;
    }

    // Force the queue to be empty
    this->m_queue = PriorityQueue();

    // Choose best triangle in each region
    for ( uint i = 0; i < K; ++i )
    {
        assert( !this->m_region[i].empty() );
        Energy min_e = std::numeric_limits<Energy>::max();
        TriangleIdx T;
        for ( const auto& t : this->m_region[i] )
        {
            const Energy e = this->E( t, this->m_proxy[i] );
            if ( e < min_e )
            {
                min_e = e;
                T = t;
            }
        }
        this->add_neighbors_to_queue( T, i );

        // Clear the region
        this->m_region[i].clear();
        this->m_region[i].push_back( T );
        this->m_fregion[T] = i;
        this->m_fvisited[T] = true;
    }

    // While the queue is not empty
    while ( !this->m_queue.empty() )
    {
        const auto q = this->m_queue.top();
        this->m_queue.pop();
        const TriangleIdx t = q.second.first;
        if ( this->m_fvisited[t] )
        {
            continue;
        }
        const uint R = q.second.second;
        this->m_region[R].push_back( t );
        this->m_fregion[t] = R;
        this->m_fvisited[t] = true;

        this->add_neighbors_to_queue( t, R );
    }
}

template <uint K_Region>
inline void
VariationalShapeApproximationBase<K_Region>::add_neighbors_to_queue( const TriangleIdx& T,
                                                                     const uint proxy_id ) {
    for ( const auto& r : this->m_fadj[T] )
    {
        this->m_queue.push(
            QueueEntry( this->E( r, this->m_proxy[proxy_id] ), Pair( r, proxy_id ) ) );
    }
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region, MetricType Type>
VariationalShapeApproximation<K_Region, Type>::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region, MetricType Type>
inline void VariationalShapeApproximation<K_Region, Type>::proxy_fitting() {
    static const Scalar c = 2.0 / 72.0;
    Matrix3 m;
    m << 10.0, 7.0, 0.0, 7.0, 10.0, 0.0, 0.0, 0.0, 0.0;
    for ( uint i = 0; i < this->K; ++i )
    {

        Scalar area_sum = 0;
        this->m_proxy[i].first = Vector3::Zero();
        Matrix3 Q = Matrix3::Zero();

        for ( const auto& T : this->m_region[i] )
        {
            const Math::Vector3 v[3] = {this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][0]],
                                  this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][1]],
                                  this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][2]]};
            const Math::Vector3& g = this->m_fbary[T];

            Matrix3 M;
            M.row( 0 ) = v[1] - v[0];
            M.row( 1 ) = v[2] - v[0];
            M.row( 2 ) = Vector3::Zero();

            Q += ( c * this->m_farea[T] * M * m * M.transpose() ) +
                 ( this->m_farea[T] * g * g.transpose() );

            this->m_proxy[i].first += this->m_farea[T] * g;
            area_sum += this->m_farea[T];
        }
        this->m_proxy[i].first /= area_sum;
        Q -= area_sum * this->m_proxy[i].first * this->m_proxy[i].first.transpose();
        this->m_proxy[i].second =
            Eigen::SelfAdjointEigenSolver<Matrix3>( Q ).eigenvectors().col( 0 ).normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region, MetricType Type>
inline Scalar VariationalShapeApproximation<K_Region, Type>::E( const TriangleIdx& T,
                                                                const Proxy& P ) const {
    static const Scalar c = 1.0 / 6.0;
    const Math::Vector3 v[3] = {this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][0]],
                          this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][1]],
                          this->m_mesh.m_vertices[this->m_mesh.m_triangles[T][2]]};
    const Math::Vector3 p = P.first;
    const Math::Vector3 n = P.second;
    Scalar d[3];
    d[0] = std::abs( n.dot( p - v[0] ) );
    d[1] = std::abs( n.dot( p - v[1] ) );
    d[2] = std::abs( n.dot( p - v[2] ) );
    return ( c *
             ( ( d[0] * d[0] ) + ( d[1] * d[1] ) + ( d[2] * d[2] ) + ( d[0] * d[1] ) +
               ( d[0] * d[2] ) + ( d[1] * d[2] ) ) *
             this->m_farea[T] );
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
VariationalShapeApproximation<K_Region, MetricType::L21>::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximation<K_Region, MetricType::L21>::proxy_fitting() {
    for ( uint i = 0; i < this->K; ++i )
    {
        Scalar area_sum = 0;
        this->m_proxy[i].first = Vector3::Zero();
        this->m_proxy[i].second = Vector3::Zero();
        for ( const auto& T : this->m_region[i] )
        {
            this->m_proxy[i].first += this->m_farea[T] * this->m_fbary[T];
            this->m_proxy[i].second += this->m_farea[T] * this->m_fnormal[T];
            area_sum += this->m_farea[T];
        }
        this->m_proxy[i].first /= area_sum;
        this->m_proxy[i].second = this->m_proxy[i].second.normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline Scalar VariationalShapeApproximation<K_Region, MetricType::L21>::E( const TriangleIdx& T,
                                                                           const Proxy& P ) const {
    const Scalar a = this->m_farea[T];
    const Math::Vector3 n = this->m_fnormal[T];
    const Math::Vector3 d = n - P.second;
    const Scalar result = a * d.squaredNorm();
    return result;
}

//============================================================================
//============================================================================
//============================================================================

//////////////////////////////////////////////////////////////////////////////
// DESTRUCTOR
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
VariationalShapeApproximation<K_Region, MetricType::LLOYD>::~VariationalShapeApproximation() {}

//////////////////////////////////////////////////////////////////////////////
// PROXY FITTING
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline void VariationalShapeApproximation<K_Region, MetricType::LLOYD>::proxy_fitting() {
    for ( uint i = 0; i < this->K; ++i )
    {
        this->m_proxy[i].first = Vector3::Zero();
        this->m_proxy[i].second = Vector3::Zero();
        for ( const auto& T : this->m_region[i] )
        {
            this->m_proxy[i].first += this->m_fbary[T];
            this->m_proxy[i].second += this->m_fnormal[T];
        }
        this->m_proxy[i].first /= this->m_region[i].size();
        this->m_proxy[i].second = this->m_proxy[i].second.normalized();
    }
}

//////////////////////////////////////////////////////////////////////////////
// ENERGY
//////////////////////////////////////////////////////////////////////////////
template <uint K_Region>
inline Scalar
VariationalShapeApproximation<K_Region, MetricType::LLOYD>::E( const TriangleIdx& T,
                                                               const Proxy& P ) const {
    const Math::Vector3& b = this->m_fbary[T];
    const Scalar result = ( b - P.first ).norm();
    return result;
}

} // namespace Geometry
} // namespace Core
} // namespace Ra


/*!
 * @file traits.cpp
 *
 * @author Andrzej Ciarkowski <mailto:andrzej.ciarkowski@gmail.com>
 */

#include <dsp++/config.h>
#include <dsp++/fftw/plan_unavailable.h>

// emit vtable and type_info for the exception in this translation unit
dsp::dft::fftw::plan_unavailable::~plan_unavailable() throw() {}

#if !DSP_FFTW_DISABLED

#include <dsp++/export.h>
#include <dsp++/fft.h> // for dft::sign::forward/dft::sign::backward
#include <dsp++/fftw/allocator.h>
#include <dsp++/fftw/dft.h>
#include <boost/static_assert.hpp>

#include <fftw3.h> // Visual Studio build: place fftw3 distribution in directory pointed by %FFTW3_ROOT% environment variable and x64 version of thereof in %FFTW3_ROOT%\x64

#if defined(_MSC_VER) && !defined(DSPXX_STATIC)
#if DSP_FFTW_HAVE_FLOAT
#pragma comment(lib, "libfftw3f-3.lib")
#endif // DSP_FFTW_HAVE_FLOAT
#if DSP_FFTW_HAVE_DOUBLE
#pragma comment(lib, "libfftw3-3.lib")
#endif // DSP_FFTW_HAVE_DOUBLE
#if DSP_FFTW_HAVE_LONG_DOUBLE
#pragma comment(lib, "libfftw3l-3.lib")
#endif // DSP_FFTW_HAVE_LONG_DOUBLE
#if DSP_FFTW_HAVE_QUAD
#pragma comment(lib, "libfftw3q-3.lib")
#endif // DSP_FFTW_HAVE_QUAD
#endif // _MSC_VER && !DSPXX_STATIC

using namespace dsp::dft::fftw;

void dsp::dft::fftw::detail::verify_plan_available(void* plan)
{
	if (NULL == plan)
		throw plan_unavailable();
}

namespace {
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::R2HC) == FFTW_R2HC);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::HC2R) == FFTW_HC2R);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::DHT) == FFTW_DHT);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::REDFT00) == FFTW_REDFT00);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::REDFT01) == FFTW_REDFT01);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::REDFT10) == FFTW_REDFT10);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::REDFT11) == FFTW_REDFT11);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::RODFT00) == FFTW_RODFT00);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::RODFT01) == FFTW_RODFT01);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::RODFT10) == FFTW_RODFT10);
	BOOST_STATIC_ASSERT(static_cast<int>(r2r::RODFT11) == FFTW_RODFT11);

	BOOST_STATIC_ASSERT(static_cast<int>(flag::measure) == FFTW_MEASURE);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::destroy_input) == FFTW_DESTROY_INPUT);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::unaligned) == FFTW_UNALIGNED);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::conserve_memory) == FFTW_CONSERVE_MEMORY);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::exhaustive) == FFTW_EXHAUSTIVE);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::preserve_input) == FFTW_PRESERVE_INPUT);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::patient) == FFTW_PATIENT);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::estimate) == FFTW_ESTIMATE);
	BOOST_STATIC_ASSERT(static_cast<int>(flag::wisdom_only) == FFTW_WISDOM_ONLY);

	BOOST_STATIC_ASSERT(static_cast<int>(dsp::dft::sign::forward) == FFTW_FORWARD);
	BOOST_STATIC_ASSERT(static_cast<int>(dsp::dft::sign::backward) == FFTW_BACKWARD);
	BOOST_STATIC_ASSERT(sizeof(int) == sizeof(unsigned));	// unsigned is (unsigned int), so it must be the same size and it should be fine to cast unsigned/signed pointers too
}

#define MANGLE(prefix, name) prefix ## name
#define COMPLEX_CAST(prefix, param) reinterpret_cast<MANGLE(prefix, complex)*>(param)
#define KIND_CAST(prefix, param) static_cast<MANGLE(prefix, r2r_kind)>(param)
#define PKIND_CAST(prefix, param) reinterpret_cast<const MANGLE(prefix, r2r_kind)*>(param)
#define INT(param) static_cast<int>(param)
#define CPINT(param) reinterpret_cast<const int*>(param)

#define DEFINE_TRAITS(prefix, type) \
BOOST_STATIC_ASSERT(sizeof(dsp::dft::fftw::r2r::kind) == sizeof(MANGLE(prefix, r2r_kind)));\
\
void traits<type>::execute(const plan_type p) {MANGLE(prefix, execute)(p);}\
\
traits<type>::plan_type traits<type>::plan_dft(size_t rank, const unsigned* n,\
		    complex_type* in, complex_type* out, dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_dft)(INT(rank), CPINT(n), COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_1d(size_t n, complex_type* in, complex_type* out, dsp::dft::sign::spec sign,\
		       unsigned flags)\
{return MANGLE(prefix, plan_dft_1d)(INT(n), COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_2d(size_t n0, size_t n1,\
		       complex_type* in, complex_type* out, dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_dft_2d)(INT(n0), INT(n1), COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_3d(size_t n0, size_t n1, size_t n2,\
		       complex_type* in, complex_type* out, dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_dft_3d)(INT(n0), INT(n1), INT(n2), COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_many_dft(size_t rank, const unsigned* n,\
                         size_t howmany,\
                         complex_type* in, const int* inembed,\
                         int istride, int idist,\
                         complex_type* out, const int* onembed,\
                         int ostride, int odist,\
                         dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_many_dft)(INT(rank), CPINT(n), INT(howmany), COMPLEX_CAST(prefix, in), inembed, istride, idist, COMPLEX_CAST(prefix, out), onembed, ostride, odist, static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_dft(size_t rank, const fftw_iodim* dims,\
			 size_t howmany_rank,\
			 const fftw_iodim* howmany_dims,\
			 complex_type* in, complex_type* out,\
			 dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_guru_dft)(INT(rank), dims, INT(howmany_rank), howmany_dims, COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_split_dft(size_t rank, const fftw_iodim* dims,\
			 size_t howmany_rank,\
			 const fftw_iodim* howmany_dims,\
			 real_type* ri, real_type* ii, real_type* ro, real_type* io,\
			 unsigned flags)\
{return MANGLE(prefix, plan_guru_split_dft)(INT(rank), dims, INT(howmany_rank), howmany_dims, ri, ii, ro, io, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_dft(size_t rank,\
                         const fftw_iodim64* dims,\
			 size_t howmany_rank,\
			 const fftw_iodim64* howmany_dims,\
			 complex_type* in, complex_type* out,\
			 dsp::dft::sign::spec sign, unsigned flags)\
{return MANGLE(prefix, plan_guru64_dft)(INT(rank), dims, INT(howmany_rank), howmany_dims, COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out), static_cast<int>(sign), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_split_dft(size_t rank,\
                         const fftw_iodim64* dims,\
			 size_t howmany_rank,\
			 const fftw_iodim64* howmany_dims,\
			 real_type* ri, real_type* ii, real_type* ro, real_type* io,\
			 unsigned flags)\
{return MANGLE(prefix, plan_guru64_split_dft)(INT(rank), dims, INT(howmany_rank), howmany_dims, ri, ii, ro, io, flags);}\
\
void traits<type>::execute_dft(const plan_type p, complex_type* in, complex_type* out)\
{MANGLE(prefix, execute_dft)(p, COMPLEX_CAST(prefix, in), COMPLEX_CAST(prefix, out));}\
\
void traits<type>::execute_split_dft(const plan_type p, real_type* ri, real_type* ii,\
                                      real_type* ro, real_type* io)\
{MANGLE(prefix, execute_split_dft)(p, ri, ii, ro, io);}\
\
traits<type>::plan_type traits<type>::plan_many_dft_r2c(size_t rank, const unsigned* n,\
                             size_t howmany,\
                             real_type* in, const int* inembed,\
                             int istride, int idist,\
                             complex_type* out, const int* onembed,\
                             int ostride, int odist,\
                             unsigned flags)\
{return MANGLE(prefix, plan_many_dft_r2c)(INT(rank), CPINT(n), INT(howmany), in, inembed, istride, idist, COMPLEX_CAST(prefix, out), onembed, ostride, odist, flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_r2c(size_t rank, const unsigned* n,\
                        real_type* in, complex_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_r2c)(INT(rank), CPINT(n), in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_r2c_1d(size_t n,real_type* in,complex_type* out,unsigned flags)\
{return MANGLE(prefix, plan_dft_r2c_1d)(INT(n), in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_r2c_2d(size_t n0, size_t n1,\
			   real_type* in, complex_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_r2c_2d)(INT(n0), INT(n1), in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_r2c_3d(size_t n0, size_t n1, size_t n2,\
			   real_type* in, complex_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_r2c_3d)(INT(n0), INT(n1), INT(n2), in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_many_dft_c2r(size_t rank, const unsigned* n,\
			     size_t howmany,\
			     complex_type* in, const int* inembed,\
			     int istride, int idist,\
			     real_type* out, const int* onembed,\
			     int ostride, int odist,\
			     unsigned flags)\
{return MANGLE(prefix, plan_many_dft_c2r)(INT(rank), CPINT(n), INT(howmany), COMPLEX_CAST(prefix, in), inembed, istride, idist, out, onembed, ostride, odist, flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_c2r(size_t rank, const unsigned* n,\
                        complex_type* in, real_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_c2r)(INT(rank), CPINT(n), COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_c2r_1d(size_t n, complex_type* in, real_type* out,unsigned flags)\
{return MANGLE(prefix, plan_dft_c2r_1d)(INT(n), COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_c2r_2d(size_t n0, size_t n1,\
			   complex_type* in, real_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_c2r_2d)(INT(n0), INT(n1), COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_dft_c2r_3d(size_t n0, size_t n1, size_t n2,\
			   complex_type* in, real_type* out, unsigned flags)\
{return MANGLE(prefix, plan_dft_c2r_3d)(INT(n0), INT(n1), INT(n2), COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_dft_r2c(size_t rank, const fftw_iodim* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim* howmany_dims,\
			     real_type* in, complex_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru_dft_r2c)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_dft_c2r(size_t rank, const fftw_iodim* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim* howmany_dims,\
			     complex_type* in, real_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru_dft_c2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_split_dft_r2c(\
                             size_t rank, const fftw_iodim* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim* howmany_dims,\
			     real_type* in, real_type* ro, real_type* io,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru_split_dft_r2c)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, ro, io, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_split_dft_c2r(\
                             size_t rank, const fftw_iodim* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim* howmany_dims,\
			     real_type* ri, real_type* ii, real_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru_split_dft_c2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, ri, ii, out, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_dft_r2c(size_t rank,\
                             const fftw_iodim64* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim64* howmany_dims,\
			     real_type* in, complex_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru64_dft_r2c)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, COMPLEX_CAST(prefix, out), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_dft_c2r(size_t rank,\
                             const fftw_iodim64* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim64* howmany_dims,\
			     complex_type* in, real_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru64_dft_c2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, COMPLEX_CAST(prefix, in), out, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_split_dft_r2c(\
                             size_t rank, const fftw_iodim64* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim64* howmany_dims,\
			     real_type* in, real_type* ro, real_type* io,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru64_split_dft_r2c)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, ro, io, flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_split_dft_c2r(\
                             size_t rank, const fftw_iodim64* dims,\
			     size_t howmany_rank,\
			     const fftw_iodim64* howmany_dims,\
			     real_type* ri, real_type* ii, real_type* out,\
			     unsigned flags)\
{return MANGLE(prefix, plan_guru64_split_dft_c2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, ri, ii, out, flags);}\
\
void traits<type>::execute_dft_r2c(const plan_type p, real_type* in, complex_type* out)\
{MANGLE(prefix, execute_dft_r2c)(p, in, COMPLEX_CAST(prefix, out));}\
\
void traits<type>::execute_dft_c2r(const plan_type p, complex_type* in, real_type* out)\
{MANGLE(prefix, execute_dft_c2r)(p, COMPLEX_CAST(prefix, in), out);}\
\
void traits<type>::execute_split_dft_r2c(const plan_type p,\
                                          real_type* in, real_type* ro, real_type* io)\
{MANGLE(prefix, execute_split_dft_r2c)(p, in, ro, io);}\
\
void traits<type>::execute_split_dft_c2r(const plan_type p,\
                                          real_type* ri, real_type* ii, real_type* out)\
{MANGLE(prefix, execute_split_dft_c2r)(p, ri, ii, out);}\
\
traits<type>::plan_type traits<type>::plan_many_r2r(size_t rank, const unsigned* n,\
                         size_t howmany,\
                         real_type* in, const int* inembed,\
                         int istride, int idist,\
                         real_type* out, const int* onembed,\
                         int ostride, int odist,\
                         const r2r::kind* kind, unsigned flags)\
{return MANGLE(prefix, plan_many_r2r)(INT(rank), CPINT(n), INT(howmany), in, inembed, istride, idist, out, onembed, ostride, odist, PKIND_CAST(prefix, kind), flags);}\
\
traits<type>::plan_type traits<type>::plan_r2r(size_t rank, const unsigned* n, real_type* in, real_type* out,\
                    const r2r::kind* kind, unsigned flags)\
{return MANGLE(prefix, plan_r2r)(INT(rank), CPINT(n), in, out, PKIND_CAST(prefix, kind), flags);}\
\
traits<type>::plan_type traits<type>::plan_r2r_1d(size_t n, real_type* in, real_type* out,\
                       r2r::kind kind, unsigned flags)\
{return MANGLE(prefix, plan_r2r_1d)(INT(n), in, out, KIND_CAST(prefix, kind), flags);}\
\
traits<type>::plan_type traits<type>::plan_r2r_2d(size_t n0, size_t n1, real_type* in, real_type* out,\
                       r2r::kind kind0, r2r::kind kind1,\
                       unsigned flags)\
{return MANGLE(prefix, plan_r2r_2d)(INT(n0), INT(n1), in, out, KIND_CAST(prefix, kind0), KIND_CAST(prefix, kind1), flags);}\
\
traits<type>::plan_type traits<type>::plan_r2r_3d(size_t n0, size_t n1, size_t n2,\
                       real_type* in, real_type* out, r2r::kind kind0,\
                       r2r::kind kind1, r2r::kind kind2,\
                       unsigned flags)\
{return MANGLE(prefix, plan_r2r_3d)(INT(n0), INT(n1), INT(n2), in, out, KIND_CAST(prefix, kind0), KIND_CAST(prefix, kind1), KIND_CAST(prefix, kind2), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru_r2r(size_t rank, const fftw_iodim* dims,\
                         size_t howmany_rank,\
                         const fftw_iodim* howmany_dims,\
                         real_type* in, real_type* out,\
                         const r2r::kind* kind, unsigned flags)\
{return MANGLE(prefix, plan_guru_r2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, out, PKIND_CAST(prefix, kind), flags);}\
\
traits<type>::plan_type traits<type>::plan_guru64_r2r(size_t rank, const fftw_iodim64* dims,\
                         size_t howmany_rank,\
                         const fftw_iodim64* howmany_dims,\
                         real_type* in, real_type* out,\
                         const r2r::kind* kind, unsigned flags)\
{return MANGLE(prefix, plan_guru64_r2r)(INT(rank), dims, INT(howmany_rank), howmany_dims, in, out, PKIND_CAST(prefix, kind), flags);}\
\
void traits<type>::execute_r2r(const plan_type p, real_type* in, real_type* out)\
{MANGLE(prefix, execute_r2r)(p, in, out);}\
\
void traits<type>::destroy_plan(plan_type p)\
{MANGLE(prefix, destroy_plan)(p);}\
\
void traits<type>::forget_wisdom(void)\
{MANGLE(prefix, forget_wisdom)();}\
\
void traits<type>::cleanup(void)\
{MANGLE(prefix, cleanup)();}\
\
void traits<type>::set_timelimit(double t)\
{MANGLE(prefix, set_timelimit)(t);}\
\
void traits<type>::plan_with_nthreads(unsigned nthreads)\
{MANGLE(prefix, plan_with_nthreads)(INT(nthreads));}\
\
int traits<type>::init_threads(void)\
{return MANGLE(prefix, init_threads)();}\
\
void traits<type>::cleanup_threads(void)\
{MANGLE(prefix, cleanup_threads)();}\
\
int traits<type>::export_wisdom_to_filename(const char* filename)\
{return MANGLE(prefix, export_wisdom_to_filename)(filename);}\
\
void traits<type>::export_wisdom_to_file(FILE* output_file)\
{MANGLE(prefix, export_wisdom_to_file)(output_file);}\
\
char* traits<type>::export_wisdom_to_string(void)\
{return MANGLE(prefix, export_wisdom_to_string)();}\
\
void traits<type>::export_wisdom(fftw_write_char_func write_char,\
                                  void* data)\
{MANGLE(prefix, export_wisdom)(write_char, data);}\
\
int traits<type>::import_system_wisdom(void)\
{return MANGLE(prefix, import_system_wisdom)();}\
\
int traits<type>::import_wisdom_from_filename(const char* filename)\
{return MANGLE(prefix, import_wisdom_from_filename)(filename);}\
\
int traits<type>::import_wisdom_from_file(FILE* input_file)\
{return MANGLE(prefix, import_wisdom_from_file)(input_file);}\
\
int traits<type>::import_wisdom_from_string(const char* input_string)\
{return MANGLE(prefix, import_wisdom_from_string)(input_string);}\
\
int traits<type>::import_wisdom(fftw_read_char_func read_char, void* data)\
{return MANGLE(prefix, import_wisdom)(read_char, data);}\
\
void traits<type>::fprint_plan(const plan_type p, FILE* output_file)\
{MANGLE(prefix, fprint_plan)(p, output_file);}\
\
void traits<type>::print_plan(const plan_type p)\
{MANGLE(prefix, print_plan)(p);}\
\
void* traits<type>::malloc(size_t n)\
{return MANGLE(prefix, malloc)(n);}\
\
traits<type>::real_type* traits<type>::alloc_real(size_t n)\
{return MANGLE(prefix, alloc_real)(n);}\
\
traits<type>::complex_type* traits<type>::alloc_complex(size_t n)\
{return reinterpret_cast<complex_type*>(MANGLE(prefix, alloc_complex)(n));}\
\
void traits<type>::free(void* p)\
{MANGLE(prefix, free)(p);}\
\
void traits<type>::flops(const plan_type p,\
                          double* add, double* mul, double* fmas)\
{MANGLE(prefix, flops)(p, add, mul, fmas);}\
\
double traits<type>::estimate_cost(const plan_type p)\
{return MANGLE(prefix, estimate_cost)(p);}\
\
double traits<type>::cost(const plan_type p)\
{return MANGLE(prefix, cost)(p);} \
\
template class DSPXX_API dsp::dft::fftw::dft<type, type>; \
template class DSPXX_API dsp::dft::fftw::dft<type, std::complex<type> >; \
template class DSPXX_API dsp::dft::fftw::dft<std::complex<type>, type>; \
template class DSPXX_API dsp::dft::fftw::dft<std::complex<type>, std::complex<type> >;

#if DSP_FFTW_HAVE_FLOAT
DEFINE_TRAITS(fftwf_, float)
#endif // DSP_FFTW_HAVE_FLOAT

#if DSP_FFTW_HAVE_DOUBLE
DEFINE_TRAITS(fftw_, double)
#endif // DSP_FFTW_HAVE_FLOAT

#if DSP_FFTW_HAVE_LONG_DOUBLE
DEFINE_TRAITS(fftwl_, long double)
#endif // DSP_FFTW_HAVE_FLOAT

#if DSP_FFTW_HAVE_QUAD
DEFINE_TRAITS(fftwq_, quad)
#endif // DSP_FFTW_HAVE_FLOAT

void* allocator_base::allocate(size_t n)
{
	void* p =
#if DSP_FFTW_HAVE_FLOAT
			traits<float>::malloc(n);
#elif DSP_FFTW_HAVE_DOUBLE
			traits<double>::malloc(n);
#elif DSP_FFTW_HAVE_LONG_DOUBLE
			traits<long double>::malloc(n);
#elif DSP_FFTW_HAVE_QUAD
			traits<quad>::malloc(n);
#else
			malloc(n);
#endif
	if (NULL == p)
		throw std::bad_alloc();
	return p;
}

void allocator_base::deallocate(void* p)
{
#if DSP_FFTW_HAVE_FLOAT
			traits<float>::free(p);
#elif DSP_FFTW_HAVE_DOUBLE
			traits<double>::free(p);
#elif DSP_FFTW_HAVE_LONG_DOUBLE
			traits<long double>::free(p);
#elif DSP_FFTW_HAVE_QUAD
			traits<quad>::free(p);
#else
			free(p);
#endif
}

#endif // !DSP_FFTW3_DISABLED


//  Copyright (c) 2006, Giovanni P. Deretta
//
//  This code may be used under either of the following two licences:
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
//  THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE. OF SUCH DAMAGE.
//
//  Or:
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <cstdlib>
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/bind.hpp>
#include <boost/coroutine/coroutine.hpp>

/*
 * Play the iterated prisoner dilemma between two prisoners.
 * Each prisoner is impersonated by a coroutine using the actor model.
 * Each coroutine can implement a different strategy.
 * On paper this looked as a great coroutine demonstrator, because
 * each player might use a complex algorithm better implemented in a coroutine.
 * Unfortunately the best known solution (tit-for-tat) is so trivially
 * simple that doesn't need state!.
 */
namespace coroutines = boost::coroutines;
using coroutines::coroutine;

enum option {cooperate, defect};

typedef coroutine<option(option)> coroutine_type;

typedef boost::tuple<int, int> score_type;

void add_score(score_type& x, option a, option b) {
  if(a == cooperate && b == defect) {
    x.get<0>() += 0;
    x.get<1>() += 5;
  } else if(a == defect && b == defect) {
    x.get<0>() += 1;
    x.get<1>() += 1;
  } else if(a == defect && b == cooperate) {
    x.get<0>() += 5;
    x.get<1>() += 0;
  } else  /* if(a == cooperate && b == cooperate) */ {
    x.get<0>() += 3;
    x.get<1>() += 3;
  }
}

score_type play(coroutine_type plr1,
		coroutine_type plr2,
		size_t iterations) {
  score_type score(0,0);
  option a = cooperate;
  option b = cooperate;
  while(iterations--) {
    option new_a = plr1(b);
    option new_b = plr2(a);
    a = new_a;
    b = new_b;
    add_score(score, a, b);
  }
  return score;
}

option always_cooperate(coroutine_type::self& self, option) {
  while(true)
    self.yield(cooperate);
  return cooperate;
}

option always_defect(coroutine_type::self& self, option) {
  while(true)
    self.yield(defect);
  return defect;
}

option random_result(coroutine_type::self& self, option) {
  while(true)
    self.yield(std::rand()%2? defect: cooperate);
  return defect;
}

option tit_for_tat(coroutine_type::self& self, option a) {
  while(true)
    a = self.yield(a);
  return cooperate;
}

option tit_for_tat_forgiving(coroutine_type::self& self, option a, int forgiveness) {
  while(true)
    a = self.yield((std::rand() %100) < forgiveness? cooperate: a);
  return cooperate;
}

int main() {
  coroutine_type list[] = {
    coroutine_type(always_cooperate),
    coroutine_type(always_defect),
    coroutine_type(random_result),
    coroutine_type(tit_for_tat),
    coroutine_type(boost::bind(tit_for_tat_forgiving, _1, _2, 15)),
    coroutine_type(boost::bind(tit_for_tat_forgiving, _1, _2, 10))
  };

  int scores [sizeof(list)] = {};
  (void)scores;
  const int rounds = 100;
  std::cout << play(coroutine_type(always_cooperate),
		    coroutine_type(always_cooperate),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(always_cooperate),
		    coroutine_type(always_defect),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(always_defect),
		    coroutine_type(always_defect),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(always_cooperate),
		    coroutine_type(random_result),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(always_defect),
		    coroutine_type(random_result),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(tit_for_tat),
		    coroutine_type(always_cooperate),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(tit_for_tat),
		    coroutine_type(always_defect),
		    rounds)<<"\n";
  std::cout << play(coroutine_type(tit_for_tat),
		    coroutine_type(random_result),
		    rounds)<<"\n";
}