//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#ifndef BOOST_SELF_AVOIDING_WALK_HPP
#define BOOST_SELF_AVOIDING_WALK_HPP

/*
  This file defines necessary components for SAW.

  mesh language: (defined by myself to clearify what is what)
  A triangle in mesh is called an triangle.
  An edge in mesh is called an line.
  A vertex in mesh is called a point.

  A triangular mesh corresponds to a graph in which a vertex is a
  triangle and an edge(u, v) stands for triangle u and triangle v
  share an line.

  After this point, a vertex always refers to vertex in graph,
  therefore it is a traingle in mesh.

 */

#include <utility>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>

#define SAW_SENTINAL -1

namespace boost {

  template <class T1, class T2, class T3>
  struct triple {
    T1 first;
    T2 second;
    T3 third;
    triple(const T1& a, const T2& b, const T3& c) : first(a), second(b), third(c) {}
    triple() : first(SAW_SENTINAL), second(SAW_SENTINAL), third(SAW_SENTINAL) {}
  };

  typedef triple<int, int, int> Triple;

  /* Define a vertex property which has a triangle inside. Triangle is
    represented by a triple.  */
  struct triangle_tag { enum { num = 100 }; };
  typedef property<triangle_tag,Triple> triangle_property;

  /* Define an edge property with a line. A line is represented by a
    pair.  This is not required for SAW though.
  */
  struct line_tag { enum { num = 101 }; };
  template <class T> struct line_property
    : public property<line_tag, std::pair<T,T> > { };

  /*Precondition: Points in a Triangle are in order */
  template <class Triangle, class Line>
  inline void get_sharing(const Triangle& a, const Triangle& b, Line& l)
  {
    l.first = SAW_SENTINAL;
    l.second = SAW_SENTINAL;

    if ( a.first == b.first ) {
      l.first = a.first;
      if ( a.second == b.second || a.second == b.third )
        l.second = a.second;
      else if ( a.third == b.second || a.third == b.third )
        l.second = a.third;

    }  else if ( a.first == b.second ) {
      l.first = a.first;
      if ( a.second == b.third )
        l.second = a.second;
      else if ( a.third == b.third )
        l.second = a.third;

    } else if ( a.first == b.third ) {
      l.first = a.first;

    } else if ( a.second == b.first ) {
      l.first = a.second;
      if ( a.third == b.second || a.third == b.third )
        l.second = a.third;

    } else if ( a.second == b.second ) {
      l.first = a.second;
      if ( a.third == b.third )
        l.second = a.third;

    } else if ( a.second == b.third ) {
      l.first = a.second;

    } else if ( a.third == b.first
                || a.third == b.second
                || a.third == b.third )
      l.first = a.third;

    /*Make it in order*/
    if ( l.first > l.second ) {
      typename Line::first_type i = l.first;
      l.first = l.second;
      l.second = i;
    }

  }

  template <class TriangleDecorator, class Vertex, class Line>
  struct get_vertex_sharing {
    typedef std::pair<Vertex, Line> Pair;
    get_vertex_sharing(const TriangleDecorator& _td) : td(_td) {}
    inline Line operator()(const Vertex& u, const Vertex& v) const {
      Line l;
      get_sharing(td[u], td[v], l);
      return l;
    }
    inline Line operator()(const Pair& u, const Vertex& v) const {
      Line l;
      get_sharing(td[u.first], td[v], l);
      return l;
    }
    inline Line operator()(const Pair& u, const Pair& v) const {
      Line l;
      get_sharing(td[u.first], td[v.first], l);
      return l;
    }
    TriangleDecorator td;
  };

  /* HList has to be a handle of data holder so that pass-by-value is
   * in right logic.
   *
   * The element of HList is a pair of vertex and line. (remember a
   * line is a pair of two ints.). That indicates the walk w from
   * current vertex is across line. (If the first of line is -1, it is
   * a point though.
   */
  template < class TriangleDecorator, class HList, class IteratorD>
  class SAW_visitor
    : public bfs_visitor<>, public dfs_visitor<>
  {
    typedef typename boost::property_traits<IteratorD>::value_type iter;
    /*use boost shared_ptr*/
    typedef typename HList::element_type::value_type::second_type     Line;
  public:

    typedef tree_edge_tag category;

    inline SAW_visitor(TriangleDecorator _td, HList _hlist, IteratorD ia)
      : td(_td), hlist(_hlist), iter_d(ia) {}

    template <class Vertex, class Graph>
    inline void start_vertex(Vertex v, Graph&) {
      Line l1;
      l1.first = SAW_SENTINAL;
      l1.second = SAW_SENTINAL;
      hlist->push_front(std::make_pair(v, l1));
      iter_d[v] = hlist->begin();
    }

    /*Several symbols:
      w(i): i-th triangle in walk w
      w(i) |- w(i+1): w enter w(i+1) from w(i) over a line
      w(i) ~> w(i+1): w enter w(i+1) from w(i) over a point
      w(i) -> w(i+1): w enter w(i+1) from w(i)
      w(i) ^ w(i+1): the line or point w go over from w(i) to w(i+1)
    */
    template <class Edge, class Graph>
    bool tree_edge(Edge e, Graph& G) {
          using std::make_pair;
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      Vertex tau = target(e, G);
      Vertex i   = source(e, G);

      get_vertex_sharing<TriangleDecorator, Vertex, Line> get_sharing_line(td);

      Line tau_i = get_sharing_line(tau, i);

      iter w_end = hlist->end();

      iter w_i = iter_d[i];

      iter w_i_m_1 = w_i;
      iter w_i_p_1 = w_i;

      /*----------------------------------------------------------
       *             true             false
       *==========================================================
       *a       w(i-1) |- w(i)    w(i-1) ~> w(i) or w(i-1) is null
       *----------------------------------------------------------
       *b       w(i) |- w(i+1)    w(i) ~> w(i+1) or no w(i+1) yet
       *----------------------------------------------------------
       */

      bool a = false, b = false;

      --w_i_m_1;
      ++w_i_p_1;
      b = ( w_i->second.first != SAW_SENTINAL );

      if ( w_i_m_1 != w_end ) {
        a = ( w_i_m_1->second.first != SAW_SENTINAL );
      }

      if ( a ) {

        if ( b ) {
          /*Case 1:

            w(i-1) |- w(i) |- w(i+1)
          */
          Line l1 = get_sharing_line(*w_i_m_1, tau);

          iter w_i_m_2 = w_i_m_1;
          --w_i_m_2;

          bool c = true;

          if ( w_i_m_2 != w_end ) {
            c = w_i_m_2->second != l1;
          }

          if ( c ) {  /* w(i-1) ^ tau != w(i-2) ^ w(i-1)  */
            /*extension: w(i-1) -> tau |- w(i) */
            w_i_m_1->second = l1;
            /*insert(pos, const T&) is to insert before pos*/
            iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));

          } else {  /* w(i-1) ^ tau == w(i-2) ^ w(i-1)  */
            /*must be w(i-2) ~> w(i-1) */

            bool d = true;
            //need to handle the case when w_i_p_1 is null
            Line l3 = get_sharing_line(*w_i_p_1, tau);
            if ( w_i_p_1 != w_end )
              d = w_i_p_1->second != l3;
            if ( d ) { /* w(i+1) ^ tau != w(i+1) ^ w(i+2) */
              /*extension: w(i) |- tau -> w(i+1) */
              w_i->second = tau_i;
              iter_d[tau] = hlist->insert(w_i_p_1, make_pair(tau, l3));
            } else { /* w(i+1) ^ tau == w(i+1) ^ w(i+2) */
              /*must be w(1+1) ~> w(i+2) */
              Line l5 = get_sharing_line(*w_i_m_1, *w_i_p_1);
              if ( l5 != w_i_p_1->second ) { /* w(i-1) ^ w(i+1) != w(i+1) ^ w(i+2) */
                /*extension: w(i-2) -> tau |- w(i) |- w(i-1) -> w(i+1) */
                w_i_m_2->second = get_sharing_line(*w_i_m_2, tau);
                iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));
                w_i->second = w_i_m_1->second;
                w_i_m_1->second = l5;
                iter_d[w_i_m_1->first] = hlist->insert(w_i_p_1, *w_i_m_1);
                hlist->erase(w_i_m_1);
              } else {
                /*mesh is tetrahedral*/
                // dont know what that means.
                ;
              }
            }

          }
        } else {
          /*Case 2:

            w(i-1) |- w(i) ~> w(1+1)
          */

          if ( w_i->second.second == tau_i.first
               || w_i->second.second == tau_i.second ) {  /*w(i) ^ w(i+1) < w(i) ^ tau*/
            /*extension: w(i) |- tau -> w(i+1) */
            w_i->second = tau_i;
            Line l1 = get_sharing_line(*w_i_p_1, tau);
            iter_d[tau] = hlist->insert(w_i_p_1, make_pair(tau, l1));
          } else { /*w(i) ^ w(i+1) !< w(i) ^ tau*/
            Line l1 = get_sharing_line(*w_i_m_1, tau);
            bool c = true;
            iter w_i_m_2 = w_i_m_1;
            --w_i_m_2;
            if ( w_i_m_2 != w_end )
              c =  l1 != w_i_m_2->second;
            if (c) { /*w(i-1) ^ tau != w(i-2) ^ w(i-1)*/
              /*extension: w(i-1) -> tau |- w(i)*/
              w_i_m_1->second = l1;
              iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));
            } else { /*w(i-1) ^ tau == w(i-2) ^ w(i-1)*/
              /*must be w(i-2)~>w(i-1)*/
              /*extension: w(i-2) -> tau |- w(i) |- w(i-1) -> w(i+1)*/
              w_i_m_2->second = get_sharing_line(*w_i_m_2, tau);
              iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));
              w_i->second = w_i_m_1->second;
              w_i_m_1->second = get_sharing_line(*w_i_m_1, *w_i_p_1);
              iter_d[w_i_m_1->first] = hlist->insert(w_i_p_1, *w_i_m_1);
              hlist->erase(w_i_m_1);
            }

          }

        }

      } else {

        if ( b ) {
          /*Case 3:

            w(i-1) ~> w(i) |- w(i+1)
          */
          bool c = false;
          if ( w_i_m_1 != w_end )
            c = ( w_i_m_1->second.second == tau_i.first)
              || ( w_i_m_1->second.second == tau_i.second);

          if ( c ) {  /*w(i-1) ^ w(i) < w(i) ^ tau*/
            /* extension: w(i-1) -> tau |- w(i) */
            if ( w_i_m_1 != w_end )
              w_i_m_1->second = get_sharing_line(*w_i_m_1, tau);
            iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));
          } else {
            bool d = true;
            Line l1;
            l1.first = SAW_SENTINAL;
            l1.second = SAW_SENTINAL;
            if ( w_i_p_1 != w_end ) {
              l1 = get_sharing_line(*w_i_p_1, tau);
              d = l1 != w_i_p_1->second;
            }
            if (d) { /*w(i+1) ^ tau != w(i+1) ^ w(i+2)*/
              /*extension: w(i) |- tau -> w(i+1) */
              w_i->second = tau_i;
              iter_d[tau] = hlist->insert(w_i_p_1, make_pair(tau, l1));
            } else {
              /*must be w(i+1) ~> w(i+2)*/
              /*extension: w(i-1) -> w(i+1) |- w(i) |- tau -> w(i+2) */
              iter w_i_p_2 = w_i_p_1;
              ++w_i_p_2;

              w_i_p_1->second = w_i->second;
              iter_d[i] = hlist->insert(w_i_p_2, make_pair(i, tau_i));
              hlist->erase(w_i);
              Line l2 = get_sharing_line(*w_i_p_2, tau);
              iter_d[tau] = hlist->insert(w_i_p_2, make_pair(tau, l2));
            }
          }

        } else {
          /*Case 4:

            w(i-1) ~> w(i) ~> w(i+1)

          */
          bool c = false;
          if ( w_i_m_1 != w_end ) {
            c = (w_i_m_1->second.second == tau_i.first)
              || (w_i_m_1->second.second == tau_i.second);
          }
          if ( c ) {   /*w(i-1) ^ w(i) < w(i) ^ tau */
            /*extension: w(i-1) -> tau |- w(i) */
            if ( w_i_m_1 != w_end )
              w_i_m_1->second = get_sharing_line(*w_i_m_1, tau);
            iter_d[tau] = hlist->insert(w_i, make_pair(tau, tau_i));
          } else {
            /*extension: w(i) |- tau -> w(i+1) */
            w_i->second = tau_i;
            Line l1;
            l1.first = SAW_SENTINAL;
            l1.second = SAW_SENTINAL;
            if ( w_i_p_1 != w_end )
              l1 = get_sharing_line(*w_i_p_1, tau);
            iter_d[tau] = hlist->insert(w_i_p_1, make_pair(tau, l1));
          }
        }

      }

      return true;
    }

  protected:
    TriangleDecorator td; /*a decorator for vertex*/
    HList             hlist;
    /*This must be a handle of list to record the SAW
      The element type of the list is pair<Vertex, Line>
     */

    IteratorD         iter_d;
    /*Problem statement: Need a fast access to w for triangle i.
     *Possible solution: mantain an array to record.
     iter_d[i] will return an iterator
     which points to w(i), where i is a vertex
     representing triangle i.
    */
  };

  template <class Triangle, class HList, class Iterator>
  inline
  SAW_visitor<Triangle, HList, Iterator>
  visit_SAW(Triangle t, HList hl, Iterator i) {
    return SAW_visitor<Triangle, HList, Iterator>(t, hl, i);
  }

  template <class Tri, class HList, class Iter>
  inline
  SAW_visitor< random_access_iterator_property_map<Tri*,Tri,Tri&>,
    HList, random_access_iterator_property_map<Iter*,Iter,Iter&> >
  visit_SAW_ptr(Tri* t, HList hl, Iter* i) {
    typedef random_access_iterator_property_map<Tri*,Tri,Tri&> TriD;
    typedef random_access_iterator_property_map<Iter*,Iter,Iter&> IterD;
    return SAW_visitor<TriD, HList, IterD>(t, hl, i);
  }

  // should also have combo's of pointers, and also const :(

}

#endif /*BOOST_SAW_H*/


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */
#ifndef GRAPHLAB_DISTRIBUTED_INGRESS_EDGE_DECISION_HPP
#define GRAPHLAB_DISTRIBUTED_INGRESS_EDGE_DECISION_HPP

#include <graphlab/graph/distributed_graph.hpp>
#include <graphlab/graph/graph_basic_types.hpp>
#include <graphlab/graph/graph_hash.hpp>
#include <graphlab/rpc/distributed_event_log.hpp>
#include <graphlab/util/dense_bitset.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace graphlab {
  template<typename VertexData, typename EdgeData>
  class distributed_graph;

 template<typename VertexData, typename EdgeData>
 class ingress_edge_decision {

    public:
      typedef graphlab::vertex_id_type vertex_id_type;
      typedef distributed_graph<VertexData, EdgeData> graph_type;
      typedef fixed_dense_bitset<RPC_MAX_N_PROCS> bin_counts_type;

    public:
      /** \brief A decision object for computing the edge assingment. */
      ingress_edge_decision(distributed_control& dc) { }

      /** Random assign (source, target) to a machine p in {0, ... numprocs-1} */
      procid_t edge_to_proc_random (const vertex_id_type source,
          const vertex_id_type target,
          size_t numprocs) {
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target),
            std::max(source, target));
        return graph_hash::hash_edge(edge_pair) % (numprocs);
      };

      /** Random assign (source, target) to a machine p in a list of candidates */
      procid_t edge_to_proc_random (const vertex_id_type source,
          const vertex_id_type target,
          const std::vector<procid_t> & candidates) {
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target),
            std::max(source, target));

        return candidates[graph_hash::hash_edge(edge_pair) % (candidates.size())];
      };

      /** Greedy assign (source, target) to a machine using:
       *  bitset<MAX_MACHINE> src_degree : the degree presence of source over machines
       *  bitset<MAX_MACHINE> dst_degree : the degree presence of target over machines
       *  vector<size_t>      proc_num_edges : the edge counts over machines
       * */
      procid_t edge_to_proc_greedy (const vertex_id_type source,
          const vertex_id_type target,
          bin_counts_type& src_degree,
          bin_counts_type& dst_degree,
          std::vector<size_t>& proc_num_edges,
          bool usehash = false,
          bool userecent = false) {
        size_t numprocs = proc_num_edges.size();

        // Compute the score of each proc.
        procid_t best_proc = -1;
        double maxscore = 0.0;
        double epsilon = 1.0;
        std::vector<double> proc_score(numprocs);
        size_t minedges = *std::min_element(proc_num_edges.begin(), proc_num_edges.end());
        size_t maxedges = *std::max_element(proc_num_edges.begin(), proc_num_edges.end());

        for (size_t i = 0; i < numprocs; ++i) {
          size_t sd = src_degree.get(i) + (usehash && (source % numprocs == i));
          size_t td = dst_degree.get(i) + (usehash && (target % numprocs == i));
          double bal = (maxedges - proc_num_edges[i])/(epsilon + maxedges - minedges);
          proc_score[i] = bal + ((sd > 0) + (td > 0));
        }
        maxscore = *std::max_element(proc_score.begin(), proc_score.end());

        std::vector<procid_t> top_procs;
        for (size_t i = 0; i < numprocs; ++i)
          if (std::fabs(proc_score[i] - maxscore) < 1e-5)
            top_procs.push_back(i);

        // Hash the edge to one of the best procs.
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target),
            std::max(source, target));
        best_proc = top_procs[graph_hash::hash_edge(edge_pair) % top_procs.size()];

        ASSERT_LT(best_proc, numprocs);
        if (userecent) {
          src_degree.clear();
          dst_degree.clear();
        }
        src_degree.set_bit(best_proc);
        dst_degree.set_bit(best_proc);
        ++proc_num_edges[best_proc];
        return best_proc;
      };

      procid_t edge_to_proc_rdma (const vertex_id_type source,
          const vertex_id_type target,
          bin_counts_type& src_degree,
          bin_counts_type& dst_degree,
          std::vector<size_t>& proc_num_edges,
          size_t start_proc,
          size_t end_proc,
          bool usehash = false,
          bool userecent = false) {
        size_t numprocs = proc_num_edges.size();

        // Compute the score of each proc.
        procid_t best_proc = -1;
        double maxscore = 0.0;
        double epsilon = 1.0;
        std::vector<double> proc_score(numprocs);
        size_t minedges = *std::min_element(proc_num_edges.begin()+start_proc, proc_num_edges.begin()+end_proc+1);
        size_t maxedges = *std::max_element(proc_num_edges.begin()+start_proc, proc_num_edges.begin()+end_proc+1);

        for (size_t i = start_proc; i <= end_proc; ++i) {
          size_t sd = src_degree.get(i) + (usehash && (source % numprocs == i));
          size_t td = dst_degree.get(i) + (usehash && (target % numprocs == i));
          double bal = (maxedges - proc_num_edges[i])/(epsilon + maxedges - minedges);
          proc_score[i] = bal + ((sd > 0) + (td > 0));
        }
        maxscore = *std::max_element(proc_score.begin()+start_proc, proc_score.begin()+end_proc+1);

        std::vector<procid_t> top_procs;
        for (size_t i = start_proc; i <= end_proc; ++i) {
          if (std::fabs(proc_score[i] - maxscore) < 1e-5)
            top_procs.push_back(i);
        }

        // Hash the edge to one of the best procs.
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target),
            std::max(source, target));
        best_proc = top_procs[graph_hash::hash_edge(edge_pair) % top_procs.size()];

        ASSERT_LT(best_proc, numprocs);
        if (userecent) {
          src_degree.clear();
          dst_degree.clear();
        }
        src_degree.set_bit(best_proc);
        dst_degree.set_bit(best_proc);
        ++proc_num_edges[best_proc];
        return best_proc;
      };

      /** Greedy assign (source, target) to a machine using:
       *  bitset<MAX_MACHINE> src_degree : the degree presence of source over machines
       *  bitset<MAX_MACHINE> dst_degree : the degree presence of target over machines
       *  vector<size_t>      proc_num_edges : the edge counts over machines
       * */
      procid_t edge_to_proc_greedy (const vertex_id_type source,
          const vertex_id_type target,
          bin_counts_type& src_degree,
          bin_counts_type& dst_degree,
          std::vector<procid_t>& candidates,
          std::vector<size_t>& proc_num_edges,
          bool usehash = false,
          bool userecent = false
          ) {
        size_t numprocs = proc_num_edges.size();

        // Compute the score of each proc.
        procid_t best_proc = -1;
        double maxscore = 0.0;
        double epsilon = 1.0;
        std::vector<double> proc_score(candidates.size());
        size_t minedges = *std::min_element(proc_num_edges.begin(), proc_num_edges.end());
        size_t maxedges = *std::max_element(proc_num_edges.begin(), proc_num_edges.end());

        for (size_t j = 0; j < candidates.size(); ++j) {
          size_t i = candidates[j];
          size_t sd = src_degree.get(i) + (usehash && (source % numprocs == i));
          size_t td = dst_degree.get(i) + (usehash && (target % numprocs == i));
          double bal = (maxedges - proc_num_edges[i])/(epsilon + maxedges - minedges);
          proc_score[j] = bal + ((sd > 0) + (td > 0));
        }
        maxscore = *std::max_element(proc_score.begin(), proc_score.end());

        std::vector<procid_t> top_procs;
        for (size_t j = 0; j < candidates.size(); ++j)
          if (std::fabs(proc_score[j] - maxscore) < 1e-5)
            top_procs.push_back(candidates[j]);

        // Hash the edge to one of the best procs.
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target),
            std::max(source, target));
        best_proc = top_procs[graph_hash::hash_edge(edge_pair) % top_procs.size()];

        ASSERT_LT(best_proc, numprocs);
        if (userecent) {
          src_degree.clear();
          dst_degree.clear();
        }
        src_degree.set_bit(best_proc);
        dst_degree.set_bit(best_proc);
        ++proc_num_edges[best_proc];
        return best_proc;
      };

     /** HDRF greedy assign (source, target) to a machine using:
      *  bitset<MAX_MACHINE> src_degree : the degree presence of source over machines
      *  bitset<MAX_MACHINE> dst_degree : the degree presence of target over machines
      *  size_t              src_true_degree : the degree of source vertex over machines
      *  size_t              dst_true_degree : the degree of target vertex over machines
      *  vector<size_t>      proc_num_edges : the edge counts over machines
      *
      *  author : Fabio Petroni [www.fabiopetroni.com]
	  *           Giorgio Iacoboni [g.iacoboni@gmail.com]
      *
      *  Based on the publication:
      *  F. Petroni, L. Querzoni, K. Daudjee, S. Kamali and G. Iacoboni:
      *  "HDRF: Stream-Based Partitioning for Power-Law Graphs".
      *  CIKM, 2015.
      * */
     procid_t edge_to_proc_hdrf (const vertex_id_type source,
          const vertex_id_type target,
          bin_counts_type& src_degree,
          bin_counts_type& dst_degree,
          size_t& src_true_degree,
          size_t& dst_true_degree,
          std::vector<size_t>& proc_num_edges,
          bool usehash = false,
          bool userecent = false) {

        size_t numprocs = proc_num_edges.size();

        size_t degree_u = src_true_degree;
        degree_u = degree_u +1;
        size_t degree_v = dst_true_degree;
        degree_v = degree_v +1;
        size_t SUM = degree_u + degree_v;
        double fu = degree_u;
        fu /= SUM;
        double fv = degree_v;
        fv /= SUM;

        // Compute the score of each proc.
        procid_t best_proc = -1;
        double maxscore = 0.0;
        double epsilon = 1.0;
        std::vector<double> proc_score(numprocs);
        size_t minedges = *std::min_element(proc_num_edges.begin(), proc_num_edges.end());
        size_t maxedges = *std::max_element(proc_num_edges.begin(), proc_num_edges.end());

        for (size_t i = 0; i < numprocs; ++i) {
		  double new_sd = 0;
		  double new_td = 0;
		  size_t sd = src_degree.get(i) + (usehash && (source % numprocs == i));
		  size_t td = dst_degree.get(i) + (usehash && (target % numprocs == i));
		  if (sd > 0){
		    new_sd = 1+(1-fu);
		  }
		  if (td > 0){
		    new_td = 1+(1-fv);
		  }
         double bal = (maxedges - proc_num_edges[i])/(epsilon + maxedges - minedges);

         proc_score[i] = bal + new_sd + new_td;
        }

        maxscore = *std::max_element(proc_score.begin(), proc_score.end());

        std::vector<procid_t> top_procs;
        for (size_t i = 0; i < numprocs; ++i)
          if (std::fabs(proc_score[i] - maxscore) < 1e-5)
            top_procs.push_back(i);

        // Hash the edge to one of the best procs.
        typedef std::pair<vertex_id_type, vertex_id_type> edge_pair_type;
        const edge_pair_type edge_pair(std::min(source, target), std::max(source, target));
        best_proc = top_procs[graph_hash::hash_edge(edge_pair) % top_procs.size()];

        ASSERT_LT(best_proc, numprocs);
        if (userecent) {
          src_degree.clear();
          dst_degree.clear();
        }
        src_degree.set_bit(best_proc);
        dst_degree.set_bit(best_proc);
        ++proc_num_edges[best_proc];
        ++src_true_degree;
        ++dst_true_degree;
        return best_proc;
     };
  };// end of ingress_edge_decision
}

#endif


/*
Copyright 2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef MATH_GENERALISE_TYPE_HPP_INCLUDED
#define MATH_GENERALISE_TYPE_HPP_INCLUDED

#include <type_traits>

#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>

#include "meta/vector.hpp"

#include "magma.hpp"

namespace math {

namespace detail {

    template <class Operations, class Magma> struct generalise_type_once;

    template <class Magma> struct generalise_type_once <meta::vector<>, Magma>
    { typedef Magma type; };

    template <class FirstOperation, class ... Operations, class Magma>
        struct generalise_type_once <
            meta::vector <FirstOperation, Operations ...>, Magma>
    {
        typedef typename std::result_of <FirstOperation (Magma, Magma)>::type
            operation_result_type;
        typedef typename merge_magma::apply <Magma, operation_result_type>::type
            next_type;
        typedef typename generalise_type_once <
            meta::vector <Operations ...>, next_type>::type type;
    };

} // namespace detail

/**
Convert a magma type to a type in the same magma that can contain the result of
any combination of a number of binary operations applied to it.

For example, a \ref single_sequence under \ref plus will generalise to
\ref optional_sequence; under \ref times (or under both) it will generalise to a
\ref sequence.

\tparam Operations
    A meta::vector of binary operations, usually from namespace callable.
\tparam Magma
    The magma type to be generalised.
*/
template <class Operations, class Magma> struct generalise_type {
    typedef typename std::decay <Magma>::type magma_type;
    typedef typename detail::generalise_type_once <Operations, magma_type>::type
        next_type;

    typedef typename boost::mpl::eval_if <
            std::is_same <Magma, next_type>,
            boost::mpl::identity <Magma>,
            generalise_type <Operations, next_type>
        >::type type;
};

} // namespace math

#endif  // MATH_GENERALISE_TYPE_HPP_INCLUDED


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef PARTIALINDEXSET_HH
#define PARTIALINDEXSET_HH

#include <cassert>
#include <vector>

#include <boost/mpl/range_c.hpp>

#include <boost/fusion/algorithm.hpp>

#include "dune/grid/common/indexidset.hh"
#include "dune/grid/common/referenceelements.hh"

#include "fem/fetransfer.hh"

/**
 *\todo recompute internal representation whenever grid changes.
 *
 * Part has to provide a method bool contains(typename Grid::Traits::template Codim<0>::Entity const&) const;
 */
template<class Grid, class IndexSet, class Part>
class PartialIndexSet {
  typedef PartialIndexSet<Grid,IndexSet,Part> Self;
  typedef typename Grid::Traits::template Codim<0>::Entity Cell;

public:
  template <int cd>
  struct Codim
  {
    template <Dune::PartitionIteratorType pitype>
    struct Partition
    {
      class Iterator
      {
        typedef typename IndexSet::template Codim<cd>::template Partition<pitype>::Iterator Iter;

      public:
        typedef typename Grid::template Codim<0>::Entity Entity;

        Iterator(Iter const& cur_, Iter const& end_, Part const& part_):
          cur(cur_), end(end_), part(part_) {
          ahead();
        }

        Iterator& operator++()
        {
          if (cur!=end)
            ++cur;
          ahead();
          return *this;
        }

        Entity& operator*() const { return *cur; }
        Entity* operator->() const { return &*cur; }

        bool operator==(Iterator const& i) const { return cur==i.cur; }
        bool operator!=(Iterator const& i) const { return !(*this == i); }

        operator typename Grid::Traits::template Codim<0>::EntityPointer() const
        {
          return cur;
        }

      private:
        void ahead()
        {
          while (cur!=end && !part.contains(*cur))
            ++cur;
        }

        Iter cur, end;
        Part const& part;
      };
    };
  };

  PartialIndexSet(GridSignals& signals, IndexSet const& indexSet_, Part const& part_):
    indexSet(indexSet_), part(part_)
  {
    typedef typename IndexSet::template Codim<0>::
        template Partition<Dune::All_Partition>::Iterator CellIterator;
    CellIterator end = indexSet.template end<0,Dune::All_Partition>();
    for (CellIterator ci=indexSet.template begin<0,Dune::All_Partition>(); ci!=end; ++ci)
      if (part.contains(*ci)) {
        insert(ci->type(),indexSet.index(*ci));
        boost::fusion::for_each(boost::mpl::range_c<int,1,Grid::dimension+1>(),
                                Update(*this,*ci));
      }
  }

  template <int cc>
  int index (typename Grid::Traits::template Codim<cc>::Entity const& e) const
  {
    typename Map::const_iterator i = idx.find(e.type());
    if (i==idx.end()) return -1;
    else              return i->second.first[indexSet.index(e)];
  }

  template<class EntityType>
  int index (const EntityType& e) const
  {
    return index<EntityType::codimension>(e);
  }

  template<int cc>
  int subIndex (Cell const& e, int i) const
  {
    // unfortunately the following is not implemented in every grid interface...
    // return index(*e.template entity<cc>(i));

    Dune::GeometryType subentityType =
      Dune::ReferenceElements<typename Grid::ctype,Grid::dimension>::general(e.type()).type(i,cc);
    typename Map::const_iterator it = idx.find(subentityType);
    if (it==idx.end()) return -1;
    else               return it->second.first[indexSet.template subIndex<cc>(e,i)];
  }

  const std::vector<Dune::GeometryType>& geomTypes (int codim) const
  {
    return indexSet.geomTypes(codim);
  }

  int size (Dune::GeometryType type) const
  {
    typename Map::const_iterator i= idx.find(type);
    if (i==idx.end()) return 0;
    else              return i->second.second;
  }

  int size (int codim) const
  {
    int count = 0;
    for (typename Map::const_iterator i=idx.begin(); i!=idx.end(); ++i)
      if (i->first.dim() == Grid::dimension-codim)
        count += i->second.second;
    return count;
  }

  template<class EntityType>
  bool contains (const EntityType& e) const
  {
    return indexSet.contains(e) && index(e)>=0;
  }

  template<int cd, Dune::PartitionIteratorType pitype>
  typename Codim<cd>::template Partition<pitype>::Iterator begin () const
  {
    return typename Codim<cd>::template Partition<pitype>::Iterator(indexSet.template begin<cd,pitype>(),
                                                                    indexSet.template end<cd,pitype>(),
                                                                    part);
  }

  template<int cd, Dune::PartitionIteratorType pitype>
  typename Codim<cd>::template Partition<pitype>::Iterator end () const
  {
    return typename Codim<cd>::template Partition<pitype>::Iterator(indexSet.template end<cd,pitype>(),
                                                                    indexSet.template end<cd,pitype>(),
                                                                    part);
  }

private:
  IndexSet const& indexSet;
  Part const& part;

  // For each geometry type, separate contiguous indices are
  // maintained, which are stored according to the underlying index
  // set. A negative value denotes entities which are not contained in
  // the partial index set. The second part in the data type is a
  // running counter used for generating contiguous indices.
  typedef std::map<Dune::GeometryType,std::pair<std::vector<int>,int> > Map;
  Map idx;

  void insert(Dune::GeometryType gt, int index)
  {
    std::pair<std::vector<int>,int>& gtIdx = idx[gt];

    if (gtIdx.first.empty()) {
      gtIdx.first.resize(indexSet.size(gt),-1);
      gtIdx.second = 0;
    }

    if (gtIdx.first[index]<0)
      gtIdx.first[index] = gtIdx.second++;
  }

  // A MPL/fusion functor which inserts all the cell's subentities of
  // given codimension into the index map.
  struct Update
  {
    Update(Self& pis_, Cell const& cell_): pis(pis_), cell(cell_) {}

    template <class Integer>
    void operator()(Integer const /* codim */) const
    {
      int const codim = Integer::value;
      int const count = cell.template count<codim>();

      for (int i=0; i<count; ++i)
        pis.insert(Dune::ReferenceElements<typename Grid::ctype,Grid::dimension>::general(cell.type()).type(i,codim),
                   pis.indexSet.template subIndex<codim>(cell,i));
    }

  private:
    Self& pis;
    Cell const& cell;
  };
};

#endif


#include "script_reader.h"
#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <boost/qvm/vec.hpp>
#include <boost/algorithm/string.hpp>
#include "Body.h"

// load data from file path provided by command line arg
initial_state load_input_data(char* input_file_path)
{
    std::ifstream input_file(input_file_path, std::ios_base::in);

    if (!input_file.is_open())
    {
        std::cout << "error: could not open" << input_file_path << std::endl;
        exit(EXIT_FAILURE);
    }

    initial_state input_state;
    double m;
    boost::qvm::vec<double, 3> r, v;
    std::string line;
    std::vector<std::string> parsed_line;

    // add conditionals to replicated Julia code
    while (getline(input_file, line))
    {
        boost::algorithm::split(parsed_line, line, boost::is_any_of(" "));

        if (parsed_line[0] == "time")
        {
            input_state.t = atof(parsed_line[1].c_str());
        }
        else if (parsed_line[0] == "step_size")
        {
            input_state.dt = atof(parsed_line[1].c_str());
        }
        else if (parsed_line[0] == "save_interval")
        {
            input_state.save_interval = atof(parsed_line[1].c_str());
        }
        else if (parsed_line[0] == "body")
        {
            m =   atof(parsed_line[1].c_str());
            r = { atof(parsed_line[2].c_str()),
                  atof(parsed_line[3].c_str()),
                  atof(parsed_line[4].c_str()) };
            v = { atof(parsed_line[5].c_str()),
                  atof(parsed_line[6].c_str()),
                  atof(parsed_line[7].c_str()) };

            input_state.system.push_back(Body(m, r, v));
        }
        else
        {
            std::cout << "error: incorrect syntax: " << line << std::endl;
            exit(EXIT_FAILURE);
        }
    }

    return input_state;
}


//   Copyright 2019 <Huawei Technologies Co., Ltd>
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.

#include <pybind11/complex.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/pytypes.h>
#include <pybind11/stl.h>

#include <complex>
#include <iostream>
#include <vector>
#if defined(_OPENMP)
#     include <omp.h>
#endif
#include <boost/container/vector.hpp>

#include "simulator-mpi/SimulatorMPI.hpp"

namespace pybind11
{
namespace detail
{
     template <typename Type, typename Alloc>
     struct type_caster<boost::container::vector<Type, Alloc>>
         : list_caster<boost::container::vector<Type, Alloc>, Type>
     {};
}  // namespace detail
}  // namespace pybind11

namespace py = pybind11;

using c_type = std::complex<double>;
using ArrayType = std::vector<c_type, aligned_allocator<c_type, 64>>;
using MatrixType = std::vector<ArrayType>;
using QuRegs = std::vector<std::vector<unsigned>>;

template <class QR>
void emulate_math_wrapper(SimulatorMPI& sim, py::function const& pyfunc,
                          QR const& qr, Fusion::IndexVector const& ctrls)
{
     auto f = [&](std::vector<int>& x) {
          pybind11::gil_scoped_acquire acquire;
          x = std::move(pyfunc(x).cast<std::vector<int>>());
     };
     pybind11::gil_scoped_release release;
     sim.emulate_math(f, qr, ctrls);
}

PYBIND11_MODULE(_cppsim_mpi, m)
{
     py::class_<SimulatorMPI>(m, "SimulatorMPI")
         .def(py::init<uint64_t, int, int>())
         .def("get_qubits_ids", &SimulatorMPI::GetQubitsPermutation)
         .def("get_local_qubits_ids", &SimulatorMPI::GetLocalQubitsPermutation)
         .def("get_global_qubits_ids",
              &SimulatorMPI::GetGlobalQubitsPermutation)
         .def("set_qubits_perm", &SimulatorMPI::SetQubitsPermutation)
         .def("swap_qubits", &SimulatorMPI::SwapQubitsWrapper)
         .def("allocate_qureg", &SimulatorMPI::AllocateQureg)
         .def("allocate_qubit", &SimulatorMPI::AllocateQubit)
         .def("deallocate_qubit", &SimulatorMPI::DeallocateQubit)
         .def("measure_qubits", &SimulatorMPI::MeasureQubits)
         .def("apply_controlled_gate", &SimulatorMPI::ApplyGate)
         .def("emulate_math", &emulate_math_wrapper<QuRegs>)
         .def("get_amplitude", &SimulatorMPI::GetAmplitude)
         .def("get_probability", &SimulatorMPI::GetProbability)
         .def("run", &SimulatorMPI::Run)
         .def("entropy", &SimulatorMPI::Entropy)
         .def("cheat_local", &SimulatorMPI::cheat_local)
         .def("collapse_wavefunction", &SimulatorMPI::collapseWaveFunction);
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2014 - 2019 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibamr/GeneralizedIBMethod.h"
#include "ibamr/IBHierarchyIntegrator.h"
#include "ibamr/IBKirchhoffRodForceGen.h"
#include "ibamr/IBMethod.h"
#include "ibamr/namespaces.h" // IWYU pragma: keep

#include "ibtk/HierarchyGhostCellInterpolation.h"
#include "ibtk/HierarchyMathOps.h"
#include "ibtk/IBTK_CHKERRQ.h"
#include "ibtk/LData.h"
#include "ibtk/LDataManager.h"
#include "ibtk/LInitStrategy.h"
#include "ibtk/LSiloDataWriter.h"
#include "ibtk/ibtk_utilities.h"

#include "BasePatchHierarchy.h"
#include "BasePatchLevel.h"
#include "CellVariable.h"
#include "CoarsenSchedule.h"
#include "GriddingAlgorithm.h"
#include "HierarchyDataOpsReal.h"
#include "IntVector.h"
#include "MultiblockDataTranslator.h"
#include "PatchHierarchy.h"
#include "PatchLevel.h"
#include "RefineAlgorithm.h"
#include "RefineOperator.h"
#include "RefineSchedule.h"
#include "SideVariable.h"
#include "Variable.h"
#include "VariableContext.h"
#include "tbox/Database.h"
#include "tbox/MathUtilities.h"
#include "tbox/Pointer.h"
#include "tbox/RestartManager.h"
#include "tbox/Utilities.h"

#include "petscvec.h"

#include "Eigen/src/Core/GeneralProduct.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <boost/multi_array.hpp>
IBTK_ENABLE_EXTRA_WARNINGS

#include <algorithm>
#include <cmath>
#include <limits>
#include <ostream>
#include <string>
#include <utility>
#include <vector>

namespace IBTK
{
class RobinPhysBdryPatchStrategy;
} // namespace IBTK

/////////////////////////////// NAMESPACE ////////////////////////////////////

namespace IBAMR
{
/////////////////////////////// STATIC ///////////////////////////////////////

namespace
{
// Version of GeneralizedIBMethod restart file data.
static const int GENERALIZED_IB_METHOD_VERSION = 1;
} // namespace

/////////////////////////////// PUBLIC ///////////////////////////////////////

GeneralizedIBMethod::GeneralizedIBMethod(std::string object_name, Pointer<Database> input_db, bool register_for_restart)
    : IBMethod(std::move(object_name), input_db, register_for_restart)
{
    // NOTE: Parent class constructor registers class with the restart manager,
    // sets object name.

    // Initialize object with data read from the input and restart databases.
    bool from_restart = RestartManager::getManager()->isFromRestart();
    if (from_restart) getFromRestart();
    if (input_db) getFromInput(input_db, from_restart);

    return;
} // GeneralizedIBMethod

void
GeneralizedIBMethod::registerIBKirchhoffRodForceGen(Pointer<IBKirchhoffRodForceGen> ib_force_and_torque_fcn)
{
#if !defined(NDEBUG)
    TBOX_ASSERT(ib_force_and_torque_fcn);
#endif
    d_ib_force_and_torque_fcn = ib_force_and_torque_fcn;
    return;
} // registerIBKirchhoffRodForceGen

void
GeneralizedIBMethod::registerEulerianVariables()
{
    IBMethod::registerEulerianVariables();

    const IntVector<NDIM> ib_ghosts = getMinimumGhostCellWidth();
    const IntVector<NDIM> no_ghosts = 0;

    Pointer<Variable<NDIM> > u_var = d_ib_solver->getVelocityVariable();
    Pointer<CellVariable<NDIM, double> > u_cc_var = u_var;
    Pointer<SideVariable<NDIM, double> > u_sc_var = u_var;
    if (u_cc_var)
    {
        d_f_var = new CellVariable<NDIM, double>(d_object_name + "::f", NDIM);
        d_w_var = new CellVariable<NDIM, double>(d_object_name + "::w", NDIM);
        d_n_var = new CellVariable<NDIM, double>(d_object_name + "::n", NDIM);
    }
    else if (u_sc_var)
    {
        d_f_var = new SideVariable<NDIM, double>(d_object_name + "::f");
        d_w_var = new SideVariable<NDIM, double>(d_object_name + "::w");
        d_n_var = new SideVariable<NDIM, double>(d_object_name + "::n");
    }
    else
    {
        TBOX_ERROR(d_object_name << "::registerEulerianVariables():\n"
                                 << "  unsupported velocity data centering" << std::endl);
    }
    registerVariable(d_f_idx, d_f_var, no_ghosts, d_ib_solver->getScratchContext());
    registerVariable(d_w_idx, d_w_var, ib_ghosts, d_ib_solver->getScratchContext());
    registerVariable(d_n_idx, d_n_var, ib_ghosts, d_ib_solver->getScratchContext());
    return;
} // registerEulerianVariables

void
GeneralizedIBMethod::registerEulerianCommunicationAlgorithms()
{
    IBMethod::registerEulerianCommunicationAlgorithms();

    Pointer<RefineAlgorithm<NDIM> > refine_alg;
    Pointer<RefineOperator<NDIM> > refine_op;

    refine_alg = new RefineAlgorithm<NDIM>();
    refine_op = nullptr;
    refine_alg->registerRefine(d_w_idx, d_w_idx, d_w_idx, refine_op);
    registerGhostfillRefineAlgorithm(d_object_name + "::w", refine_alg);

    refine_alg = new RefineAlgorithm<NDIM>();
    refine_op = nullptr;
    refine_alg->registerRefine(d_n_idx, d_n_idx, d_n_idx, refine_op);
    registerGhostfillRefineAlgorithm(d_object_name + "::n", refine_alg);
    return;
} // registerEulerianCommunicationAlgorithms

void
GeneralizedIBMethod::preprocessIntegrateData(double current_time, double new_time, int num_cycles)
{
    d_ib_force_and_torque_fcn_needs_init = d_ib_force_fcn_needs_init || d_ib_force_and_torque_fcn_needs_init;
    IBMethod::preprocessIntegrateData(current_time, new_time, num_cycles);

    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();
    const double start_time = d_ib_solver->getStartTime();

    if (d_ib_force_and_torque_fcn)
    {
        if (d_ib_force_and_torque_fcn_needs_init)
        {
            const bool initial_time = MathUtilities<double>::equalEps(current_time, start_time);
            resetLagrangianForceAndTorqueFunction(current_time, initial_time);
            d_ib_force_and_torque_fcn_needs_init = false;
        }
    }

    // Look-up or allocate Lagangian data.
    d_D_current_data.resize(finest_ln + 1);
    d_D_new_data.resize(finest_ln + 1);
    d_N_current_data.resize(finest_ln + 1);
    d_N_new_data.resize(finest_ln + 1);
    d_W_current_data.resize(finest_ln + 1);
    d_W_new_data.resize(finest_ln + 1);
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        d_D_current_data[ln] = d_l_data_manager->getLData("D", ln);
        d_D_new_data[ln] = d_l_data_manager->createLData("D_new", ln, NDIM * NDIM);
        d_N_current_data[ln] = d_l_data_manager->createLData("N", ln, NDIM);
        d_N_new_data[ln] = d_l_data_manager->createLData("N_new", ln, NDIM);
        d_W_current_data[ln] = d_l_data_manager->getLData("W", ln);
        d_W_new_data[ln] = d_l_data_manager->createLData("W_new", ln, NDIM);

        // Initialize D^{n+1} to equal D^{n}, and initialize W^{n+1} to equal
        // W^{n}.
        int ierr;
        ierr = VecCopy(d_D_current_data[ln]->getVec(), d_D_new_data[ln]->getVec());
        IBTK_CHKERRQ(ierr);
        ierr = VecCopy(d_W_current_data[ln]->getVec(), d_W_new_data[ln]->getVec());
        IBTK_CHKERRQ(ierr);
    }
    return;
} // preprocessIntegrateData

void
GeneralizedIBMethod::postprocessIntegrateData(double current_time, double new_time, int num_cycles)
{
    IBMethod::postprocessIntegrateData(current_time, new_time, num_cycles);

    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();

    // Reset time-dependent Lagrangian data.
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        int ierr;
        ierr = VecSwap(d_D_current_data[ln]->getVec(), d_D_new_data[ln]->getVec());
        IBTK_CHKERRQ(ierr);
        ierr = VecSwap(d_W_current_data[ln]->getVec(), d_W_new_data[ln]->getVec());
        IBTK_CHKERRQ(ierr);
    }

    // Deallocate Lagrangian scratch data.
    d_D_current_data.clear();
    d_D_new_data.clear();
    d_N_current_data.clear();
    d_N_new_data.clear();
    d_W_current_data.clear();
    d_W_new_data.clear();
    return;
} // postprocessIntegrateData

void
GeneralizedIBMethod::interpolateVelocity(const int u_data_idx,
                                         const std::vector<Pointer<CoarsenSchedule<NDIM> > >& u_synch_scheds,
                                         const std::vector<Pointer<RefineSchedule<NDIM> > >& u_ghost_fill_scheds,
                                         const double data_time)
{
    // Interpolate the linear velocities.
    IBMethod::interpolateVelocity(u_data_idx, u_synch_scheds, u_ghost_fill_scheds, data_time);

    // Interpolate the angular velocities.
    std::vector<Pointer<LData> >* W_data = nullptr;
    if (MathUtilities<double>::equalEps(data_time, d_current_time))
    {
        W_data = &d_W_current_data;
    }
    else if (MathUtilities<double>::equalEps(data_time, d_half_time))
    {
        TBOX_ERROR(d_object_name << "::interpolateVelocity():\n"
                                 << "  time-stepping type MIDPOINT_RULE not supported by "
                                    "class GeneralizedIBMethod;\n"
                                 << "  use TRAPEZOIDAL_RULE instead.\n");
    }
    else if (MathUtilities<double>::equalEps(data_time, d_new_time))
    {
        W_data = &d_W_new_data;
    }
    TBOX_ASSERT(W_data);

    Pointer<Variable<NDIM> > u_var = d_ib_solver->getVelocityVariable();
    Pointer<CellVariable<NDIM, double> > u_cc_var = u_var;
    Pointer<SideVariable<NDIM, double> > u_sc_var = u_var;
    if (u_cc_var)
    {
        Pointer<CellVariable<NDIM, double> > w_cc_var = d_w_var;
        getHierarchyMathOps()->curl(d_w_idx, w_cc_var, u_data_idx, u_cc_var, nullptr, data_time);
    }
    else if (u_sc_var)
    {
        Pointer<SideVariable<NDIM, double> > w_sc_var = d_w_var;
        getHierarchyMathOps()->curl(d_w_idx, w_sc_var, u_data_idx, u_sc_var, nullptr, data_time);
    }
    else
    {
        TBOX_ERROR(d_object_name << "::interpolateVelocity():\n"
                                 << "  unsupported velocity data centering" << std::endl);
    }
    std::vector<Pointer<LData> >* X_LE_data;
    bool* X_LE_needs_ghost_fill;
    getLECouplingPositionData(&X_LE_data, &X_LE_needs_ghost_fill, data_time);
    getVelocityHierarchyDataOps()->scale(d_w_idx, 0.5, d_w_idx);
    d_l_data_manager->interp(d_w_idx,
                             *W_data,
                             *X_LE_data,
                             std::vector<Pointer<CoarsenSchedule<NDIM> > >(),
                             getGhostfillRefineSchedules(d_object_name + "::w"),
                             data_time);
    resetAnchorPointValues(*W_data,
                           /*coarsest_ln*/ 0,
                           /*finest_ln*/ d_hierarchy->getFinestLevelNumber());
    return;
} // interpolateVelocity

void
GeneralizedIBMethod::forwardEulerStep(const double current_time, const double new_time)
{
    IBMethod::forwardEulerStep(current_time, new_time);

    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();
    const double dt = new_time - current_time;

    // Update the value of D^{n+1} using forward Euler.
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        boost::multi_array_ref<double, 2>& D_current_data = *d_D_current_data[ln]->getLocalFormVecArray();
        boost::multi_array_ref<double, 2>& W_current_data = *d_W_current_data[ln]->getLocalFormVecArray();
        boost::multi_array_ref<double, 2>& D_new_data = *d_D_new_data[ln]->getLocalFormVecArray();
        const int n_local = d_l_data_manager->getNumberOfLocalNodes(ln);
        Matrix3d R;
        Vector3d e;
        for (int l = 0; l < n_local; ++l)
        {
            for (int d = 0; d < NDIM; ++d)
            {
                e(d) = W_current_data[l][d];
            }
            const double norm_e = e.norm();
            if (norm_e > std::numeric_limits<double>::epsilon())
            {
                const double theta = norm_e * dt;
                e /= norm_e;
                const double c_t = std::cos(theta);
                const double s_t = std::sin(theta);
                R << c_t + (1.0 - c_t) * e(0) * e(0), (1.0 - c_t) * e(0) * e(1) - s_t * e(2),
                    (1.0 - c_t) * e(0) * e(2) + s_t * e(1), (1.0 - c_t) * e(1) * e(0) + s_t * e(2),
                    c_t + (1.0 - c_t) * e(1) * e(1), (1.0 - c_t) * e(1) * e(2) - s_t * e(0),
                    (1.0 - c_t) * e(2) * e(0) - s_t * e(1), (1.0 - c_t) * e(2) * e(1) + s_t * e(0),
                    c_t + (1.0 - c_t) * e(2) * e(2);
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    Eigen::Map<const Vector3d> D_current_alpha(&D_current_data[l][3 * alpha]);
                    Eigen::Map<Vector3d> D_new_alpha(&D_new_data[l][3 * alpha]);
                    D_new_alpha = R * D_current_alpha;
                }
            }
            else
            {
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    Eigen::Map<const Vector3d> D_current_alpha(&D_current_data[l][3 * alpha]);
                    Eigen::Map<Vector3d> D_new_alpha(&D_new_data[l][3 * alpha]);
                    D_new_alpha = D_current_alpha;
                }
            }
        }
    }
    return;
} // eulerStep

void
GeneralizedIBMethod::midpointStep(const double /*current_time*/, const double /*new_time*/)
{
    TBOX_ERROR(d_object_name << "::midpointStep():\n"
                             << "  time-stepping type MIDPOINT_RULE not "
                                "supported by class GeneralizedIBMethod;\n"
                             << "  use TRAPEZOIDAL_RULE instead.\n");
    return;
} // midpointStep

void
GeneralizedIBMethod::trapezoidalStep(const double current_time, const double new_time)
{
    IBMethod::trapezoidalStep(current_time, new_time);

    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();
    const double dt = new_time - current_time;

    // Update the value of D^{n+1} using the trapezoidal rule.
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        boost::multi_array_ref<double, 2>& D_current_data = *d_D_current_data[ln]->getLocalFormVecArray();
        boost::multi_array_ref<double, 2>& W_current_data = *d_W_current_data[ln]->getLocalFormVecArray();
        boost::multi_array_ref<double, 2>& D_new_data = *d_D_new_data[ln]->getLocalFormVecArray();
        boost::multi_array_ref<double, 2>& W_new_data = *d_W_new_data[ln]->getLocalFormVecArray();
        const int n_local = d_l_data_manager->getNumberOfLocalNodes(ln);
        Matrix3d R;
        Vector3d e;
        for (int l = 0; l < n_local; ++l)
        {
            for (int d = 0; d < NDIM; ++d)
            {
                e(d) = 0.5 * (W_current_data[l][d] + W_new_data[l][d]);
            }
            const double norm_e = e.norm();
            if (norm_e > std::numeric_limits<double>::epsilon())
            {
                const double theta = norm_e * dt;
                e /= norm_e;
                const double c_t = std::cos(theta);
                const double s_t = std::sin(theta);
                R << c_t + (1.0 - c_t) * e(0) * e(0), (1.0 - c_t) * e(0) * e(1) - s_t * e(2),
                    (1.0 - c_t) * e(0) * e(2) + s_t * e(1), (1.0 - c_t) * e(1) * e(0) + s_t * e(2),
                    c_t + (1.0 - c_t) * e(1) * e(1), (1.0 - c_t) * e(1) * e(2) - s_t * e(0),
                    (1.0 - c_t) * e(2) * e(0) - s_t * e(1), (1.0 - c_t) * e(2) * e(1) + s_t * e(0),
                    c_t + (1.0 - c_t) * e(2) * e(2);
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    Eigen::Map<const Vector3d> D_current_alpha(&D_current_data[l][3 * alpha]);
                    Eigen::Map<Vector3d> D_new_alpha(&D_new_data[l][3 * alpha]);
                    D_new_alpha = R * D_current_alpha;
                }
            }
            else
            {
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    Eigen::Map<const Vector3d> D_current_alpha(&D_current_data[l][3 * alpha]);
                    Eigen::Map<Vector3d> D_new_alpha(&D_new_data[l][3 * alpha]);
                    D_new_alpha = D_current_alpha;
                }
            }
        }
    }
    return;
} // trapezoidalStep

void
GeneralizedIBMethod::computeLagrangianForce(const double data_time)
{
    IBMethod::computeLagrangianForce(data_time);

    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();
    int ierr;
    std::vector<Pointer<LData> >* F_data = nullptr;
    std::vector<Pointer<LData> >* N_data = nullptr;
    std::vector<Pointer<LData> >* X_data = nullptr;
    std::vector<Pointer<LData> >* D_data = nullptr;
    if (MathUtilities<double>::equalEps(data_time, d_current_time))
    {
        d_F_current_needs_ghost_fill = true;
        d_N_current_needs_ghost_fill = true;
        F_data = &d_F_current_data;
        N_data = &d_N_current_data;
        X_data = &d_X_current_data;
        D_data = &d_D_current_data;
    }
    else if (MathUtilities<double>::equalEps(data_time, d_half_time))
    {
        TBOX_ERROR(d_object_name << "::computeLagrangianForce():\n"
                                 << "  time-stepping type MIDPOINT_RULE not supported by "
                                    "class GeneralizedIBMethod;\n"
                                 << "  use TRAPEZOIDAL_RULE instead.\n");
    }
    else if (MathUtilities<double>::equalEps(data_time, d_new_time))
    {
        d_F_new_needs_ghost_fill = true;
        d_N_new_needs_ghost_fill = true;
        F_data = &d_F_new_data;
        N_data = &d_N_new_data;
        X_data = &d_X_new_data;
        D_data = &d_D_new_data;
    }
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        ierr = VecSet((*N_data)[ln]->getVec(), 0.0);
        IBTK_CHKERRQ(ierr);
        if (d_ib_force_and_torque_fcn)
        {
            d_ib_force_and_torque_fcn->computeLagrangianForceAndTorque((*F_data)[ln],
                                                                       (*N_data)[ln],
                                                                       (*X_data)[ln],
                                                                       (*D_data)[ln],
                                                                       d_hierarchy,
                                                                       ln,
                                                                       data_time,
                                                                       d_l_data_manager);
        }
    }
    resetAnchorPointValues(*F_data, coarsest_ln, finest_ln);
    resetAnchorPointValues(*N_data, coarsest_ln, finest_ln);
    return;
} // computeLagrangianForce

void
GeneralizedIBMethod::spreadForce(const int f_data_idx,
                                 RobinPhysBdryPatchStrategy* f_phys_bdry_op,
                                 const std::vector<Pointer<RefineSchedule<NDIM> > >& f_prolongation_scheds,
                                 const double data_time)
{
    IBMethod::spreadForce(f_data_idx, f_phys_bdry_op, f_prolongation_scheds, data_time);

    std::vector<Pointer<LData> >* N_data = nullptr;
    bool* N_needs_ghost_fill = nullptr;
    if (MathUtilities<double>::equalEps(data_time, d_current_time))
    {
        N_data = &d_N_current_data;
        N_needs_ghost_fill = &d_N_current_needs_ghost_fill;
    }
    else if (MathUtilities<double>::equalEps(data_time, d_half_time))
    {
        TBOX_ERROR(d_object_name << "::spreadForce():\n"
                                 << "  time-stepping type MIDPOINT_RULE not supported by "
                                    "class GeneralizedIBMethod;\n"
                                 << "  use TRAPEZOIDAL_RULE instead.\n");
    }
    else if (MathUtilities<double>::equalEps(data_time, d_new_time))
    {
        N_data = &d_N_new_data;
        N_needs_ghost_fill = &d_N_new_needs_ghost_fill;
    }
    TBOX_ASSERT(N_data);
    TBOX_ASSERT(N_needs_ghost_fill);

    std::vector<Pointer<LData> >* X_LE_data;
    bool* X_LE_needs_ghost_fill;
    getLECouplingPositionData(&X_LE_data, &X_LE_needs_ghost_fill, data_time);
    getVelocityHierarchyDataOps()->setToScalar(d_n_idx, 0.0, false);
    d_l_data_manager->spread(d_n_idx,
                             *N_data,
                             *X_LE_data,
                             f_phys_bdry_op,
                             std::vector<Pointer<RefineSchedule<NDIM> > >(),
                             data_time,
                             *N_needs_ghost_fill,
                             *X_LE_needs_ghost_fill);
    *N_needs_ghost_fill = false;
    *X_LE_needs_ghost_fill = false;
    const int coarsest_ln = 0;
    const int finest_ln = d_hierarchy->getFinestLevelNumber();
    const std::vector<Pointer<RefineSchedule<NDIM> > >& n_ghostfill_scheds =
        getGhostfillRefineSchedules(d_object_name + "::n");
    for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = d_hierarchy->getPatchLevel(ln);
        n_ghostfill_scheds[ln]->fillData(data_time);
    }
    Pointer<Variable<NDIM> > u_var = d_ib_solver->getVelocityVariable();
    Pointer<CellVariable<NDIM, double> > u_cc_var = u_var;
    Pointer<SideVariable<NDIM, double> > u_sc_var = u_var;
    if (u_cc_var)
    {
        Pointer<CellVariable<NDIM, double> > f_cc_var = d_f_var;
        Pointer<CellVariable<NDIM, double> > n_cc_var = d_n_var;
        getHierarchyMathOps()->curl(d_f_idx, f_cc_var, d_n_idx, n_cc_var, nullptr, data_time);
    }
    else if (u_sc_var)
    {
        Pointer<SideVariable<NDIM, double> > f_sc_var = d_f_var;
        Pointer<SideVariable<NDIM, double> > n_sc_var = d_n_var;
        getHierarchyMathOps()->curl(d_f_idx, f_sc_var, d_n_idx, n_sc_var, nullptr, data_time);
    }
    else
    {
        TBOX_ERROR(d_object_name << "::spreadForce():\n"
                                 << "  unsupported velocity data centering" << std::endl);
    }
    getVelocityHierarchyDataOps()->axpy(f_data_idx, 0.5, d_f_idx, f_data_idx);
    return;
} // spreadForce

void
GeneralizedIBMethod::initializePatchHierarchy(Pointer<PatchHierarchy<NDIM> > hierarchy,
                                              Pointer<GriddingAlgorithm<NDIM> > gridding_alg,
                                              int u_data_idx,
                                              const std::vector<Pointer<CoarsenSchedule<NDIM> > >& u_synch_scheds,
                                              const std::vector<Pointer<RefineSchedule<NDIM> > >& u_ghost_fill_scheds,
                                              int integrator_step,
                                              double init_data_time,
                                              bool initial_time)
{
    // Initialize various Lagrangian data objects required by the conventional
    // IB method.
    IBMethod::initializePatchHierarchy(hierarchy,
                                       gridding_alg,
                                       u_data_idx,
                                       u_synch_scheds,
                                       u_ghost_fill_scheds,
                                       integrator_step,
                                       init_data_time,
                                       initial_time);

    // Initialize various Lagrangian data objects required by the gIB method.
    if (initial_time)
    {
        // Lookup the range of hierarchy levels.
        const int coarsest_ln = 0;
        const int finest_ln = d_hierarchy->getFinestLevelNumber();

        // Initialize the interpolated angular velocity field.
        std::vector<Pointer<LData> > W_data(finest_ln + 1);
        std::vector<Pointer<LData> > X_data(finest_ln + 1);
        for (int ln = coarsest_ln; ln <= finest_ln; ++ln)
        {
            if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
            X_data[ln] = d_l_data_manager->getLData(LDataManager::POSN_DATA_NAME, ln);
            W_data[ln] = d_l_data_manager->getLData("W", ln);
        }
        Pointer<Variable<NDIM> > u_var = d_ib_solver->getVelocityVariable();
        Pointer<CellVariable<NDIM, double> > u_cc_var = u_var;
        Pointer<SideVariable<NDIM, double> > u_sc_var = u_var;
        if (u_cc_var)
        {
            Pointer<CellVariable<NDIM, double> > w_cc_var = d_w_var;
            getHierarchyMathOps()->curl(d_w_idx, w_cc_var, u_data_idx, u_cc_var, nullptr, init_data_time);
        }
        else if (u_sc_var)
        {
            Pointer<SideVariable<NDIM, double> > w_sc_var = d_w_var;
            getHierarchyMathOps()->curl(d_w_idx, w_sc_var, u_data_idx, u_sc_var, nullptr, init_data_time);
        }
        else
        {
            TBOX_ERROR(d_object_name << "::initializePatchHierarchy():\n"
                                     << "  unsupported velocity data centering" << std::endl);
        }
        getVelocityHierarchyDataOps()->scale(d_w_idx, 0.5, d_w_idx);
        d_l_data_manager->interp(d_w_idx,
                                 W_data,
                                 X_data,
                                 std::vector<Pointer<CoarsenSchedule<NDIM> > >(),
                                 getGhostfillRefineSchedules(d_object_name + "::w"),
                                 init_data_time);
        resetAnchorPointValues(W_data, coarsest_ln, finest_ln);
    }

    // Indicate that the force-and-torque strategy needs to be re-initialized.
    d_ib_force_and_torque_fcn_needs_init = true;
    return;
} // initializePatchHierarchy

void
GeneralizedIBMethod::initializeLevelData(Pointer<BasePatchHierarchy<NDIM> > hierarchy,
                                         int level_number,
                                         double init_data_time,
                                         bool can_be_refined,
                                         bool initial_time,
                                         Pointer<BasePatchLevel<NDIM> > old_level,
                                         bool allocate_data)
{
    IBMethod::initializeLevelData(
        hierarchy, level_number, init_data_time, can_be_refined, initial_time, old_level, allocate_data);
    if (initial_time && d_l_data_manager->levelContainsLagrangianData(level_number))
    {
        // 1. Allocate LData corresponding to the curvilinear mesh node
        //    directors and angular velocities.
        Pointer<LData> D_data = d_l_data_manager->createLData("D",
                                                              level_number,
                                                              NDIM * NDIM,
                                                              /*manage_data*/ true);
        Pointer<LData> W_data = d_l_data_manager->createLData("W", level_number, NDIM, /*manage_data*/ true);

        // 2. Initialize the Lagrangian data.
        static const int global_index_offset = 0;
        static const int local_index_offset = 0;
        d_l_initializer->initializeDirectorDataOnPatchLevel(global_index_offset,
                                                            local_index_offset,
                                                            D_data,
                                                            hierarchy,
                                                            level_number,
                                                            init_data_time,
                                                            can_be_refined,
                                                            initial_time,
                                                            d_l_data_manager);

        // 3. Register data with any registered data writer.
        if (d_silo_writer)
        {
            d_silo_writer->registerVariableData("D1", D_data, 0, 3, level_number);
            d_silo_writer->registerVariableData("D2", D_data, 3, 3, level_number);
            d_silo_writer->registerVariableData("D3", D_data, 6, 3, level_number);
            d_silo_writer->registerVariableData("W", W_data, level_number);
        }
    }
    return;
} // initializeLevelData

void
GeneralizedIBMethod::putToDatabase(Pointer<Database> db)
{
    IBMethod::putToDatabase(db);
    db->putInteger("GENERALIZED_IB_METHOD_VERSION", GENERALIZED_IB_METHOD_VERSION);
    return;
} // putToDatabase

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

void
GeneralizedIBMethod::resetLagrangianForceAndTorqueFunction(const double init_data_time, const bool initial_time)
{
    if (!d_ib_force_and_torque_fcn) return;
    for (int ln = 0; ln <= d_hierarchy->getFinestLevelNumber(); ++ln)
    {
        if (!d_l_data_manager->levelContainsLagrangianData(ln)) continue;
        d_ib_force_and_torque_fcn->initializeLevelData(d_hierarchy, ln, init_data_time, initial_time, d_l_data_manager);
    }
    return;
} // resetLagrangianForceAndTorqueFunction

void
GeneralizedIBMethod::getFromInput(Pointer<Database> /*db*/, bool /*is_from_restart*/)
{
    // intentionally blank
    return;
} // getFromInput

void
GeneralizedIBMethod::getFromRestart()
{
    Pointer<Database> restart_db = RestartManager::getManager()->getRootDatabase();
    Pointer<Database> db;
    if (restart_db->isDatabase(d_object_name))
    {
        db = restart_db->getDatabase(d_object_name);
    }
    else
    {
        TBOX_ERROR(d_object_name << ":  Restart database corresponding to " << d_object_name
                                 << " not found in restart file." << std::endl);
    }
    int ver = db->getInteger("GENERALIZED_IB_METHOD_VERSION");
    if (ver != GENERALIZED_IB_METHOD_VERSION)
    {
        TBOX_ERROR(d_object_name << ":  Restart file version different than class version." << std::endl);
    }
    return;
} // getFromRestart

/////////////////////////////// NAMESPACE ////////////////////////////////////

} // namespace IBAMR

//////////////////////////////////////////////////////////////////////////////


/*
 * Copyright (C) 2014, Computing Systems Laboratory (CSLab), NTUA.
 * Copyright (C) 2014, Vasileios Karakasis
 * All rights reserved.
 *
 * This file is distributed under the BSD License. See LICENSE.txt for details.
 */

/**
 * \file Xform.hpp
 * \brief Matrix coordinate transformations
 *
 * \author Computing Systems Laboratory (CSLab), NTUA
 * \date 2011–2014
 * \copyright This file is distributed under the BSD License. See LICENSE.txt
 * for details.
 */

#ifndef SPARSEX_INTERNALS_XFORM_HPP
#define SPARSEX_INTERNALS_XFORM_HPP

#include <sparsex/internals/Element.hpp>
#include <sparsex/internals/Encodings.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/function.hpp>
#include <cassert>

namespace bll = boost::lambda;
using namespace std;

namespace sparsex {
  namespace csx {

    // Coordinate transformation functions
    // The variant with the pair argument is used for chaining transformations

    template<typename IndexType>
    pair<IndexType, IndexType> XformHoriz(IndexType r, IndexType c,
					  IndexType nr_rows = 0,
					  IndexType nr_cols = 0)
    {
      // Identinty function
      return make_pair(r, c);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformHoriz(pair<IndexType, IndexType> coord,
					  IndexType nr_rows = 0,
					  IndexType nr_cols = 0)
    {
      return XformHoriz<IndexType>(coord.first, coord.second);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformHoriz(IndexType r, IndexType c,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      // Identinty transformation
      return make_pair(r, c);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformHoriz(pair<IndexType, IndexType> coord,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      return RevXformHoriz<IndexType>(coord.first, coord.second);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformVert(IndexType r, IndexType c,
					 IndexType nr_rows = 0,
					 IndexType nr_cols = 0)
    {
      return make_pair(c, r);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformVert(pair<IndexType, IndexType> coord,
					 IndexType nr_rows = 0,
					 IndexType nr_cols = 0)
    {
      return XformVert<IndexType>(coord.first, coord.second);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformVert(IndexType r, IndexType c,
					    IndexType nr_rows = 0,
					    IndexType nr_cols = 0)
    {
      return XformVert<IndexType>(r, c);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformVert(pair<IndexType, IndexType> coord,
					    IndexType nr_rows = 0,
					    IndexType nr_cols = 0)
    {
      return RevXformVert<IndexType>(coord.first, coord.second);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformDiag(IndexType r, IndexType c,
					 IndexType nr_rows,
					 IndexType nr_cols = 0)
    {
      assert(nr_rows + c - r > 0);
      return make_pair(nr_rows + c - r, (c < r) ? c : r);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformDiag(pair<IndexType, IndexType> coord,
					 IndexType nr_rows,
					 IndexType nr_cols = 0)
    {
      return XformDiag<IndexType>(coord.first, coord.second, nr_rows);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformDiag(IndexType r, IndexType c,
					    IndexType nr_rows,
					    IndexType nr_cols = 0)
    {
      if (r < nr_rows)
        return make_pair(nr_rows + c - r, c);
      else
        return make_pair(c, r + c - nr_rows);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformDiag(pair<IndexType, IndexType> coord,
					    IndexType nr_rows,
					    IndexType nr_cols = 0)
    {
      return RevXformDiag<IndexType>(coord.first, coord.second, nr_rows);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformAntiDiag(IndexType r, IndexType c,
					     IndexType nr_rows,
					     IndexType nr_cols)
    {
      IndexType new_r = r + c - 1;
      return make_pair(new_r, (new_r <= nr_cols) ? r : nr_cols - c + 1);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> XformAntiDiag(pair<IndexType, IndexType> coord,
					     IndexType nr_rows,
					     IndexType nr_cols)
    {
      return XformAntiDiag<IndexType>(coord.first, coord.second,
				      nr_rows, nr_cols);
    }

    template<typename IndexType>
    pair<IndexType, IndexType> RevXformAntiDiag(IndexType r, IndexType c,
						IndexType nr_rows,
						IndexType nr_cols)
    {
      if (r <= nr_cols)
        return make_pair(c, r - c + 1);
      else
        return make_pair(r + c - nr_cols, nr_cols - c + 1);
    }

    template<typename IndexType>
    pair<IndexType, IndexType>
    RevXformAntiDiag(pair<IndexType, IndexType> coord,
		     IndexType nr_rows,
		     IndexType nr_cols)
    {
      return RevXformAntiDiag<IndexType>(coord.first, coord.second,
					 nr_rows, nr_cols);
    }

    template<typename IndexType, IndexType R>
    pair<IndexType, IndexType> XformBlockRow(IndexType r, IndexType c,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      return make_pair((r - 1) / R + 1,
		       (r - 1) % R + R*(c - 1) + 1);
    }

    template<typename IndexType, IndexType R>
    pair<IndexType, IndexType> XformBlockRow(pair<IndexType, IndexType> coord,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      return XformBlockRow<IndexType, R>(coord.first, coord.second);
    }

    template<typename IndexType, IndexType R>
    pair<IndexType, IndexType> RevXformBlockRow(IndexType r, IndexType c,
						IndexType nr_rows = 0,
						IndexType nr_cols = 0)
    {
      return make_pair(R*(r - 1) + (c - 1) % R + 1,
		       (c - 1) / R + 1);
    }

    template<typename IndexType, IndexType R>
    pair<IndexType, IndexType>
    RevXformBlockRow(pair<IndexType, IndexType> coord,
		     IndexType nr_rows = 0,
		     IndexType nr_cols = 0)
    {
      return RevXformBlockRow<IndexType, R>(coord.first, coord.second);
    }

    template<typename IndexType, IndexType C>
    pair<IndexType, IndexType> XformBlockCol(IndexType r, IndexType c,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      pair<IndexType, IndexType> vert = XformVert<IndexType>(r, c);
      return XformBlockRow<IndexType, C>(vert.first, vert.second);
    }

    template<typename IndexType, IndexType C>
    pair<IndexType, IndexType> XformBlockCol(pair<IndexType, IndexType> coord,
					     IndexType nr_rows = 0,
					     IndexType nr_cols = 0)
    {
      return XformBlockCol<IndexType, C>(coord.first, coord.second);
    }

    template<typename IndexType, IndexType C>
    pair<IndexType, IndexType> RevXformBlockCol(IndexType r, IndexType c,
						IndexType nr_rows = 0,
						IndexType nr_cols = 0)
    {
      pair<IndexType, IndexType> rblock = RevXformBlockRow<IndexType, C>(r, c);
      return RevXformVert<IndexType>(rblock.first, rblock.second);
    }

    template<typename IndexType, IndexType C>
    pair<IndexType, IndexType>
    RevXformBlockCol(pair<IndexType, IndexType> coord,
		     IndexType nr_rows = 0,
		     IndexType nr_cols = 0)
    {
      return RevXformBlockCol<IndexType, C>(coord.first, coord.second);
    }

    // Transform function type
    template<typename IndexType>
    struct TransformFn {
      typedef boost::function<
        pair<IndexType, IndexType>
        (pair<IndexType, IndexType>, IndexType, IndexType)> type;
    };

    template<typename IndexType>
    typename TransformFn<IndexType>::type GetXformFnFromHoriz(Encoding::Type to)
    {
      typename TransformFn<IndexType>::type ret;

      switch (to) {
      case Encoding::Horizontal:
        ret = bll::bind(XformHoriz<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::Vertical:
        ret = bll::bind(XformVert<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::Diagonal:
        ret = bll::bind(XformDiag<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::AntiDiagonal:
        ret = bll::bind(XformAntiDiag<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow1:
        ret = bll::bind(XformBlockRow<IndexType, 1>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow2:
        ret = bll::bind(XformBlockRow<IndexType, 2>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow3:
        ret = bll::bind(XformBlockRow<IndexType, 3>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow4:
        ret = bll::bind(XformBlockRow<IndexType, 4>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow5:
        ret = bll::bind(XformBlockRow<IndexType, 5>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow6:
        ret = bll::bind(XformBlockRow<IndexType, 6>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow7:
        ret = bll::bind(XformBlockRow<IndexType, 7>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow8:
        ret = bll::bind(XformBlockRow<IndexType, 8>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol1:
        ret = bll::bind(XformBlockCol<IndexType, 1>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol2:
        ret = bll::bind(XformBlockCol<IndexType, 2>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol3:
        ret = bll::bind(XformBlockCol<IndexType, 3>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol4:
        ret = bll::bind(XformBlockCol<IndexType, 4>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol5:
        ret = bll::bind(XformBlockCol<IndexType, 5>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol6:
        ret = bll::bind(XformBlockCol<IndexType, 6>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol7:
        ret = bll::bind(XformBlockCol<IndexType, 7>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol8:
        ret = bll::bind(XformBlockCol<IndexType, 8>, bll::_1, bll::_2, bll::_3);
        break;
      default:
        assert(false && "[BUG] `to' is not handled");
      }

      return ret;
    }

    template<typename IndexType>
    typename TransformFn<IndexType>::type GetXformFnToHoriz(Encoding::Type from)
    {
      typename TransformFn<IndexType>::type ret;

      switch (from) {
      case Encoding::Horizontal:
        ret = bll::bind(RevXformHoriz<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::Vertical:
        ret = bll::bind(RevXformVert<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::Diagonal:
        ret = bll::bind(RevXformDiag<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::AntiDiagonal:
        ret = bll::bind(RevXformAntiDiag<IndexType>, bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow1:
        ret = bll::bind(RevXformBlockRow<IndexType, 1>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow2:
        ret = bll::bind(RevXformBlockRow<IndexType, 2>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow3:
        ret = bll::bind(RevXformBlockRow<IndexType, 3>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow4:
        ret = bll::bind(RevXformBlockRow<IndexType, 4>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow5:
        ret = bll::bind(RevXformBlockRow<IndexType, 5>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow6:
        ret = bll::bind(RevXformBlockRow<IndexType, 6>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow7:
        ret = bll::bind(RevXformBlockRow<IndexType, 7>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockRow8:
        ret = bll::bind(RevXformBlockRow<IndexType, 8>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol1:
        ret = bll::bind(RevXformBlockCol<IndexType, 1>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol2:
        ret = bll::bind(RevXformBlockCol<IndexType, 2>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol3:
        ret = bll::bind(RevXformBlockCol<IndexType, 3>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol4:
        ret = bll::bind(RevXformBlockCol<IndexType, 4>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol5:
        ret = bll::bind(RevXformBlockCol<IndexType, 5>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol6:
        ret = bll::bind(RevXformBlockCol<IndexType, 6>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol7:
        ret = bll::bind(RevXformBlockCol<IndexType, 7>,
                        bll::_1, bll::_2, bll::_3);
        break;
      case Encoding::BlockCol8:
        ret = bll::bind(RevXformBlockCol<IndexType, 8>,
                        bll::_1, bll::_2, bll::_3);
        break;
      default:
        assert(false && "[BUG] `from' is not handled");
      }

      return ret;
    }

    template<typename IndexType>
    typename TransformFn<IndexType>::type GetXformFn(Encoding::Type from,
						     Encoding::Type to)
    {
      typename TransformFn<IndexType>::type ret;

      if (from == to)
        ret = bll::bind(XformHoriz<IndexType>, bll::_1, bll::_2, bll::_3);
      else if (from == Encoding::Horizontal)
        ret = GetXformFnFromHoriz<IndexType>(to);
      else if (to == Encoding::Horizontal)
        ret = GetXformFnToHoriz<IndexType>(from);
      else {
        typename TransformFn<IndexType>::type from_horiz =
	  GetXformFnFromHoriz<IndexType>(to);
        typename TransformFn<IndexType>::type to_horiz =
	  GetXformFnToHoriz<IndexType>(from);
        ret = bll::bind(from_horiz,
                        bll::bind(to_horiz, bll::_1, bll::_2, bll::_3),
                        bll::_2, bll::_3);
      }

      return ret;
    }

  }   // end of namespace csx
}   // end of namespace sparsex

#endif  // SPARSEX_INTERNALS_XFORM_HPP


#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <armadillo>

namespace py = pybind11;

using namespace arma;

typedef py::array_t<double, py::array::f_style | py::array::forcecast> array_tf;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> array_tc;

cube array_to_cube(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];
    int n_slices = _m_buff.shape[2];

    cube _m_arma((double *)_m_buff.ptr, n_rows, n_cols, n_slices);

    return _m_arma;
}

mat array_to_mat(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];

    mat _m_arma((double *)_m_buff.ptr, n_rows, n_cols);

    return _m_arma;
}

vec array_to_vec(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];

    vec _m_vec((double *)_m_buff.ptr, n_rows);

    return _m_vec;
}

array_tf cube_to_array(cube m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols, m.n_slices});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols * m.n_slices);

    return _m_array;
}

array_tf mat_to_array(mat m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols);

    return _m_array;
}

array_tf vec_to_array(vec m) {

    auto _m_array = array_tf({m.n_rows});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows);

    return _m_array;
}

py::tuple backward_pass(array_tf _Q, array_tf _q,
                        array_tf _R, array_tf _r,
                        array_tf _P, array_tf _p,
                        array_tf _F, array_tf _G,
                        array_tf _T, array_tf _U,
                        array_tf _V, array_tf _X,
                        array_tf _Y, array_tf _Z,
                        double lmbda, int reg,
                        int nb_bdim, int nb_udim, int nb_steps) {

    // inputs
    cube Q = array_to_cube(_Q);
    mat q = array_to_mat(_q);

    cube R = array_to_cube(_R);
    mat r = array_to_mat(_r);

    cube P = array_to_cube(_P);
    mat p = array_to_mat(_p);

    cube F = array_to_cube(_F);
    cube G = array_to_cube(_G);

    cube T = array_to_cube(_T);
    cube U = array_to_cube(_U);
    cube V = array_to_cube(_V);

    cube X = array_to_cube(_X);
    cube Y = array_to_cube(_Y);
    cube Z = array_to_cube(_Z);

    // outputs
    cube C(nb_bdim, nb_bdim, nb_steps);
    mat c(nb_bdim, nb_steps);

    cube D(nb_udim, nb_udim, nb_steps);
    mat d(nb_udim, nb_steps);

    cube E(nb_udim, nb_bdim, nb_steps);
    mat e(nb_bdim * nb_bdim, nb_steps);

    cube Ereg(nb_udim, nb_bdim, nb_steps);
    cube Dreg(nb_udim, nb_udim, nb_steps);
    cube Dinv(nb_udim, nb_udim, nb_steps);

    cube S(nb_bdim, nb_bdim, nb_steps + 1);
    mat s(nb_bdim, nb_steps + 1);
    mat tau(nb_bdim * nb_bdim, nb_steps + 1);

    vec dS(2);

    cube Sreg(nb_bdim, nb_bdim, nb_steps + 1);

    cube K(nb_udim, nb_bdim, nb_steps);
    mat kff(nb_udim, nb_steps);

    int _diverge = 0;

    // init last time step
    S.slice(nb_steps) = Q.slice(nb_steps);
    s.col(nb_steps) = q.col(nb_steps);
    tau.col(nb_steps) = p.col(nb_steps);

	for(int i = nb_steps - 1; i>= 0; --i)
	{
        C.slice(i) = Q.slice(i) + F.slice(i).t() * S.slice(i+1) * F.slice(i);
        D.slice(i) = R.slice(i) + G.slice(i).t() * S.slice(i+1) * G.slice(i);
        E.slice(i) = (P.slice(i) + F.slice(i).t() * S.slice(i+1) * G.slice(i)).t();

        c.col(i) = q.col(i) + F.slice(i).t() * s.col(i+1) + T.slice(i).t() * tau.col(i+1)
                   + 0.5 * X.slice(i).t() * vectorise(S.slice(i+1));

        d.col(i) = r.col(i) + G.slice(i).t() * s.col(i+1) + V.slice(i).t() * tau.col(i+1)
                   + 0.5 * Z.slice(i).t() * vectorise(S.slice(i+1));

        e.col(i) = p.col(i) + U.slice(i).t() * tau.col(i) + 0.5 * Y.slice(i).t() * vectorise(S.slice(i+1));

        Sreg.slice(i+1) = S.slice(i+1);
        if (reg==2)
            Sreg.slice(i+1) += lmbda * eye(nb_bdim, nb_bdim);

        Ereg.slice(i) = (P.slice(i) + F.slice(i).t() * Sreg.slice(i+1) * G.slice(i)).t();

        Dreg.slice(i) = R.slice(i) + G.slice(i).t() * Sreg.slice(i+1) * G.slice(i);
        if (reg==1)
            Dreg.slice(i) += lmbda * eye(nb_udim, nb_udim);

        if (!(Dreg.slice(i)).is_sympd()) {
            _diverge = i;
            break;
        }

        Dinv.slice(i) = inv(Dreg.slice(i));
        K.slice(i) = - Dinv.slice(i) * Ereg.slice(i);
        kff.col(i) = - Dinv.slice(i) * d.col(i);

        dS += join_vert(kff.col(i).t() * d.col(i), 0.5 * kff.col(i).t() * D.slice(i) * kff.col(i));

        tau.col(i) = e.col(i);

        s.col(i) = c.col(i) + K.slice(i).t() * D.slice(i) * kff.col(i) +
                   K.slice(i).t() * d.col(i) + E.slice(i).t() * kff.col(i);

        S.slice(i) = C.slice(i) + K.slice(i).t() * D.slice(i) * K.slice(i) +
                     K.slice(i).t() * E.slice(i) + E.slice(i).t() * K.slice(i);
        S.slice(i) = 0.5 * (S.slice(i) + S.slice(i).t());
	}

    // transform outputs to numpy
    array_tf _S = cube_to_array(S);
    array_tf _s = mat_to_array(s);
    array_tf _tau = mat_to_array(tau);

    array_tf _dS = vec_to_array(dS);

    array_tf _K = cube_to_array(K);
    array_tf _kff = mat_to_array(kff);

    py::tuple output =  py::make_tuple(_S, _s, _tau,
                                       _dS, _K, _kff, _diverge);
	return output;
}

PYBIND11_MODULE(core, m)
{
    m.def("backward_pass", &backward_pass);
}