/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include "galois/Galois.h"
#include "galois/AtomicHelpers.h"
#include "galois/FlatMap.h"
#include "galois/Reduction.h"
#include "galois/PriorityQueue.h"
#include "galois/Timer.h"
#include "galois/gstl.h"
#include "galois/Timer.h"
#include "galois/graphs/LCGraph.h"
#include "galois/graphs/TypeTraits.h"
#include "llvm/Support/CommandLine.h"

#include "Lonestar/BoilerPlate.h"
#include "Lonestar/BFS_SSSP.h"

#include <iostream>
#include <chrono>
#include <iomanip>
#include <unordered_set>
#include <fstream>
#include "stack_vector.h"

#ifndef STACK_BITVECTOR_SIZE
#define STACK_BITVECTOR_SIZE 23947347
#endif

namespace cll = llvm::cl;
using namespace std::chrono;
namespace acc = boost::accumulators;

static const char* name = "Single Source Shortest Path";
static const char* desc =
    "Computes the shortest path from a source node to all nodes in a directed "
    "graph using a modified chaotic iteration algorithm";
static const char* url = "single_source_shortest_path";

static cll::opt<std::string>
    filename(cll::Positional, cll::desc("<input graph>"), cll::Required);

static cll::opt<unsigned int>
    startNode("startNode",
              cll::desc("Node to start search from (default value 0)"),
              cll::init(0));
static cll::opt<unsigned int>
    reportNode("reportNode",
               cll::desc("Node to report distance to(default value 1)"),
               cll::init(1));
static cll::opt<unsigned int>
    stepShift("delta",
              cll::desc("Shift value for the deltastep (default value 13)"),
              cll::init(13));

enum Algo {
  deltaTile = 0,
  deltaStep,
  serDeltaTile,
  serDelta,
  dijkstraTile,
  dijkstra,
  topo,
  topoTile,
  serSP1,
  serSP2,
  parSP1,
  parSP2
};

const char* const ALGO_NAMES[] = {"deltaTile", "deltaStep",    "serDeltaTile",
                                  "serDelta",  "dijkstraTile", "dijkstra",
                                  "topo", "topoTile", "serSP1", "serSP2", "parSP1", "parSP2"};

static cll::opt<Algo>
    algo("algo", cll::desc("Choose an algorithm:"),
         cll::values(clEnumVal(deltaTile, "deltaTile"),
                     clEnumVal(deltaStep, "deltaStep"),
                     clEnumVal(serDeltaTile, "serDeltaTile"),
                     clEnumVal(serDelta, "serDelta"),
                     clEnumVal(dijkstraTile, "dijkstraTile"),
                     clEnumVal(dijkstra, "dijkstra"), clEnumVal(topo, "topo"),
                     clEnumVal(topoTile, "topoTile"),
                     clEnumVal(serSP1, "serSP1"),
                     clEnumVal(serSP2, "serSP2"),
                     clEnumVal(parSP1, "parSP1"),
                     clEnumVal(parSP2, "parSP2"),
                     clEnumValEnd),
         cll::init(deltaTile));

constexpr static const bool TRACK_WORK          = false;
constexpr static const unsigned CHUNK_SIZE      = 64u;
constexpr static const ptrdiff_t EDGE_TILE_SIZE = 512;

high_resolution_clock::time_point start;

template<typename GraphTypes>
struct SerNodeData {
  typename GraphTypes::SerGraphType::GraphNode node;
  int pred;
  uint32_t dist;
  bool fixed;
  high_resolution_clock::time_point fixed_ts;
  uint32_t min_in_weight;
  SerNodeData(): node(-1), pred(0), dist(0), fixed(false) {}
};

template<typename GraphTypes>
struct ParNodeData {
  typename GraphTypes::ParGraphType::GraphNode node;
  std::atomic<int> pred;
  std::atomic<uint32_t> dist;
  std::atomic<bool> fixed;
  high_resolution_clock::time_point fixed_ts;
  uint32_t min_in_weight;
  ParNodeData(): node(-1), pred(0), dist(0), fixed(false) {}
};

struct GraphTypes {
  typedef SerNodeData<GraphTypes> SerNodeDataType;
  typedef galois::graphs::LC_CSR_Graph<typename GraphTypes::SerNodeDataType, uint32_t>::
  with_no_lockable<true>::type ::with_numa_alloc<true>::type SerGraphType;
  typedef ParNodeData<GraphTypes> ParNodeDataType;
  typedef galois::graphs::LC_CSR_Graph<typename GraphTypes::ParNodeDataType, uint32_t>::
  with_no_lockable<true>::type ::with_numa_alloc<true>::type ParGraphType;
};

std::vector<uint32_t> total_pred;

template<typename GType>
struct GraphTypeTraits {
  typedef BFS_SSSP<GType, uint32_t, true, EDGE_TILE_SIZE> SSSP;
  typedef typename SSSP::Dist Dist;
  typedef typename SSSP::UpdateRequest UpdateRequest;
  typedef typename SSSP::UpdateRequestIndexer UpdateRequestIndexer;
  typedef typename SSSP::SrcEdgeTile SrcEdgeTile;
  typedef typename SSSP::SrcEdgeTileMaker SrcEdgeTileMaker;
  typedef typename SSSP::SrcEdgeTilePushWrap SrcEdgeTilePushWrap;
  typedef typename SSSP::ReqPushWrap ReqPushWrap;
  typedef typename SSSP::OutEdgeRangeFn OutEdgeRangeFn;
  typedef typename SSSP::TileRangeFn TileRangeFn;
};

template<typename GType>
struct GraphType {
  typedef GType Graph;
  typedef typename GType::GraphNode GNode;
  typedef GraphTypeTraits<GType> Traits;
};

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void deltaStepAlgo(Graph& graph,
                   GNode source,
                   const P& pushWrap,
                   const R& edgeRange) {

  using Dist                 = typename Traits::Dist;
  using UpdateRequestIndexer = typename Traits::UpdateRequestIndexer;

  //! [reducible for self-defined stats]
  galois::GAccumulator<size_t> BadWork;
  //! [reducible for self-defined stats]
  galois::GAccumulator<size_t> WLEmptyWork;

  namespace gwl = galois::worklists;

  using PSchunk = gwl::PerSocketChunkFIFO<CHUNK_SIZE>;
  using OBIM = galois::worklists::OrderedByIntegerMetric<UpdateRequestIndexer, PSchunk>;

  graph.getData(source).dist = 0;

  galois::InsertBag<T> initBag;
  pushWrap(initBag, source, 0, "parallel");

  galois::for_each(galois::iterate(initBag),
                   [&](const T& item, auto& ctx) {
                     constexpr galois::MethodFlag flag =
                         galois::MethodFlag::UNPROTECTED;
                     const auto& sdata = graph.getData(item.src, flag);

                     if (sdata.dist < item.dist) {
                       if (TRACK_WORK)
                         WLEmptyWork += 1;
                       return;
                     }

                     for (auto ii : edgeRange(item)) {

                       GNode dst          = graph.getEdgeDst(ii);
                       auto& ddist        = graph.getData(dst, flag);
                       Dist ew            = graph.getEdgeData(ii, flag);
                       const Dist newDist = sdata.dist + ew;

                       while (true) {
                         Dist oldDist = ddist.dist;

                         if (oldDist <= newDist) {
                           break;
                         }

                         if (ddist.dist.compare_exchange_weak(
                                 oldDist, newDist, std::memory_order_relaxed)) {

                           ddist.fixed = true;
                           ddist.fixed_ts = high_resolution_clock::now();

                           if (TRACK_WORK) {
                             //! [per-thread contribution of self-defined stats]
                             if (oldDist != GType::Traits::SSSP::DIST_INFINITY) {
                               BadWork += 1;
                             }
                             //! [per-thread contribution of self-defined stats]
                           }

                           pushWrap(ctx, dst, newDist);
                           break;
                         }
                       }
                     }
                   },
                   galois::wl<OBIM>(UpdateRequestIndexer{stepShift}),
                   galois::no_conflicts(), galois::loopname("SSSP"));

  if (TRACK_WORK) {
    //! [report self-defined stats]
    galois::runtime::reportStat_Single("SSSP", "BadWork", BadWork.reduce());
    //! [report self-defined stats]
    galois::runtime::reportStat_Single("SSSP", "WLEmptyWork",
                                       WLEmptyWork.reduce());
  }
}

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void serDeltaAlgo(Graph& graph, const GNode& source,
                  const P& pushWrap, const R& edgeRange) {

  using UpdateRequestIndexer = typename Traits::UpdateRequestIndexer;

  SerialBucketWL<T, UpdateRequestIndexer> wl(UpdateRequestIndexer{stepShift});
  graph.getData(source).dist = 0;

  pushWrap(wl, source, 0);

  size_t iter = 0ul;
  while (!wl.empty()) {

    auto& curr = wl.minBucket();

    while (!curr.empty()) {
      ++iter;
      auto item = curr.front();
      curr.pop_front();

      if (graph.getData(item.src).dist < item.dist) {
        // empty work
        continue;
      }

      for (auto e : edgeRange(item)) {

        GNode dst   = graph.getEdgeDst(e);
        auto& ddata = graph.getData(dst);

        const auto newDist = item.dist + graph.getEdgeData(e);

        if (newDist < ddata.dist) {
          ddata.dist = newDist;
          pushWrap(wl, dst, newDist);
        }
      }
    }

    wl.goToNextBucket();
  }

  if (!wl.allEmpty()) {
    std::abort();
  }
  galois::runtime::reportStat_Single("SSSP-Serial-Delta", "Iterations", iter);
}

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void dijkstraAlgo(Graph& graph, const GNode& source, const P& pushWrap,
                  const R& edgeRange) {

  using WL = galois::MinHeap<T>;

  graph.getData(source).dist = 0;

  WL wl;
  pushWrap(wl, source, 0);

  while (!wl.empty()) {

    T item = wl.pop();

    if (graph.getData(item.src).dist < item.dist) {
      continue;
    }

    auto& nd = graph.getData(item.src);
    nd.fixed = true;
    nd.fixed_ts = high_resolution_clock::now();

    for (auto e : edgeRange(item)) {

      GNode dst   = graph.getEdgeDst(e);
      auto& ddata = graph.getData(dst);

      const auto newDist = item.dist + graph.getEdgeData(e);

      if (newDist < ddata.dist) {
        ddata.dist = newDist;
        pushWrap(wl, dst, newDist);
      }
    }
  }
}

template<typename Graph, typename R>
void calc_graph_predecessors(Graph& graph, R& edgeRange) {
  // Fill the pred array
  for (auto vertex : graph) {
    for (auto edge : edgeRange(vertex)) {
      auto& k_data = graph.getData(graph.getEdgeDst(edge));
      k_data.pred++;
      total_pred[k_data.node]++;
      k_data.min_in_weight = std::min(k_data.min_in_weight, graph.getEdgeData(edge));
    }
  }
}

#ifdef DEBUG
#define D(x) x
#else
#define D(x)
#endif

#define LIKELY(condition) __builtin_expect(static_cast<bool>(condition), 1)
#define UNLIKELY(condition) __builtin_expect(static_cast<bool>(condition), 0)

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename GNodeData = typename GType::Graph::node_data_type,
          typename Traits = typename GType::Traits,
	  typename Cmp = std::less<typename Traits::Dist>>
void serSP1Algo(Graph& graph, const GNode& source,
                const P& pushWrap, const R& edgeRange) {

  using Heap = galois::MinHeap<T>;
  using Dist = typename Traits::Dist;

  bool changed;

  Heap heap;
  pushWrap(heap, source, 0);

  galois::gstl::Vector<GNodeData*> r_set;
  r_set.reserve(100);

  GNodeData* min = nullptr;
  Cmp cmp;

  // While the heap is not empty
  while (!heap.empty() || !r_set.empty()) {

    while (LIKELY(!heap.empty())) {
      T item = heap.top();
      GNodeData* item_data = &graph.getData(item.src);

      if (item_data->fixed || item_data->dist < item.dist) {
        // If we got a min, go do some work.
        heap.pop();
        if (!r_set.empty()) break;
        continue;
      }
      heap.pop();
      min = item_data;
      min->fixed = true;
      min->fixed_ts = high_resolution_clock::now();
      r_set.push_back(min);

      if (!(UNLIKELY(!heap.empty()) && heap.top().dist == min->dist)) break;
    }

    // Inner loop, go through the all the elements in R
    while(r_set.size() > 0) {
      GNodeData* z = r_set.back();
      r_set.pop_back();

      // Get all the vertices that have edges from z
      for (auto e : edgeRange(z->node)) {
        // If k vertex is not fixed, process the edge between z and k.
        auto k = &graph.getData(graph.getEdgeDst(e));
        if (k->fixed) continue;

        Dist z_k_dist = graph.getEdgeData(e);

        changed = false;
        if(z->dist + z_k_dist < k->dist) {
          k->dist = z->dist + z_k_dist;
          changed = true;
          if (k->dist < min->dist) min = k;
        }
        if (--k->pred <= 0) {
          k->fixed = true;
          k->fixed_ts = high_resolution_clock::now();
          r_set.push_back(k);
        } else if (changed) {
          pushWrap(heap,k->node,k->dist);
        }
      }

      if (r_set.empty() && !min->fixed && cmp(min->dist, heap.top().dist)){
        // We're done, but before we break, let's just check whether we have the new min in the q set
        // That is, if the heap is not empty and the current min is higher than the min in the q
        // set no point in pushing back to the heap, where it would have to bubble up.
        min->fixed = true;
        min->fixed_ts = high_resolution_clock::now();
        r_set.push_back(min);
      }
    }
  }
}

// template <typename GType,
//           typename T,
//           typename P,
//           typename R,
//           typename Graph = typename GType::Graph,
//           typename GNode = typename GType::GNode,
//           typename GNodeData = typename GType::Graph::node_data_type,
//           typename Traits = typename GType::Traits,
// 	  typename Cmp = std::less<typename Traits::Dist>>
// void serSP2Algo(Graph& graph, const GNode& source,
//                 const P& pushWrap, const R& edgeRange) {

//   using Heap = galois::MinHeap<T>;
//   using Dist = typename Traits::Dist;

//   bool changed;

//   Heap heap;
//   pushWrap(heap, source, 0);

//   galois::gstl::Vector<GNodeData*> r_set;
//   r_set.reserve(100);

//   GNodeData* min = nullptr;
//   Cmp cmp;

//   // While the heap is not empty
//   while (!heap.empty() || !r_set.empty()) {

//     while (LIKELY(!heap.empty())) {
//       T item = heap.top();
//       GNodeData* item_data = &graph.getData(item.src);

//       if (item_data->fixed || item_data->dist < item.dist) {
//         // If we got a min, go do some work.
//         heap.pop();
//         if (!r_set.empty()) break;
//         continue;
//       }
//       heap.pop();
//       min = item_data;
//       min->fixed = true;
//       min->fixed_ts = high_resolution_clock::now();
//       r_set.push_back(min);

//       if (heap.empty() || heap.top().dist != min->dist) break;
//     }

//     galois::gstl::Vector<GNodeData*> q_set1;
//     galois::gstl::Vector<GNodeData*> q_set2;
//     galois::gstl::Vector<GNodeData*>* q_set_current = &q_set1;
//     galois::gstl::Vector<GNodeData*>* q_set_aux = &q_set2;
//     // Inner loop, go through the all the elements in R
//     while(r_set.size() > 0) {
//       GNodeData* z = r_set.back();
//       r_set.pop_back();

//       // Get all the vertices that have edges from z
//       for (auto e : edgeRange(z->node)) {
//         auto k = &graph.getData(graph.getEdgeDst(e));
//         Dist z_k_dist = graph.getEdgeData(e);
//         // If k vertex is not fixed, process the edge between z and k.
//         if (k->fixed) continue;

//         changed = false;
//         if(z->dist + z_k_dist < k->dist) {
//           //std::cout << "Trying to relax to " <<  (z->dist + z_k_dist) << std::endl;
//           k->dist = z->dist + z_k_dist;
//           changed = true;
//           if (k->dist < min->dist) min = k;
//         }
//         if (--k->pred == 0 || k->dist <= (min->dist + k->min_in_weight)) {
//           k->fixed = true;
//           k->fixed_ts = high_resolution_clock::now();
//           r_set.push_back(k);
//         } else if (changed) {
//           q_set_current->push_back(k);
//         }
//       }

//       if (r_set.empty()) {
//         if (!min->fixed && min->dist <= heap.top().dist) {
//           // We're done, but before we break, let's just check whether we have the new min in the q set
//           // That is, if the heap is not empty and the current min is higher than the min in the q
//           // set no point in pushing back to the heap, where it would have to bubble up.
//           min->fixed = true;
//           min->fixed_ts = high_resolution_clock::now();
//           r_set.push_back(min);
//         }
//         for (auto& q : *q_set_current) {
//           if (q->fixed) {
//             r_set.push_back(q);
//           } else {
//             q_set_aux->push_back(q);
//           }
//         }
//         if (r_set.empty()) {
//           for (auto& q: *q_set_aux) {
//             pushWrap(heap, q->node, q->dist);
//           }
//         } else {
//           q_set_current->clear();
//           auto temp =  q_set_current;
//           q_set_current = q_set_aux;
//           q_set_aux = temp;
//         }
//       }
//     }
//   }
// }

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename GNodeData = typename GType::Graph::node_data_type,
          typename Traits = typename GType::Traits,
	  typename Cmp = std::less<typename Traits::Dist>>
void serSP2Algo(Graph& graph, const GNode& source,
                const P& pushWrap, const R& edgeRange) {

  using Heap = galois::MinHeap<T>;
  using Dist = typename Traits::Dist;

  bool changed;

  Heap heap;
  pushWrap(heap, source, 0);

  galois::gstl::Vector<GNodeData*> r_set;
  r_set.reserve(100);

  GNodeData* min = nullptr;
  Cmp cmp;

  // While the heap is not empty
  while (!heap.empty() || !r_set.empty()) {

    while (LIKELY(!heap.empty())) {
      T item = heap.top();
      GNodeData* item_data = &graph.getData(item.src);

       heap.pop();
      if (item_data->fixed || item_data->dist < item.dist) {
        // If we got a min, go do some work.
        if (!r_set.empty()) break;
        continue;
      }
      min = item_data;
      min->fixed = true;
      min->fixed_ts = high_resolution_clock::now();
      r_set.push_back(min);

      if (heap.empty() || heap.top().dist != min->dist) break;
    }

    // Inner loop, go through the all the elements in R
    while(r_set.size() > 0) {
      GNodeData* z = r_set.back();
      //std::cout << "Processing node " << z->node << std::endl;
      r_set.pop_back();

      // Get all the vertices that have edges from z
      for (auto e : edgeRange(z->node)) {
        auto k = &graph.getData(graph.getEdgeDst(e));
        Dist z_k_dist = graph.getEdgeData(e);
        // If k vertex is not fixed, process the edge between z and k.
        if (k->fixed) continue;

        changed = false;
        if(z->dist + z_k_dist < k->dist) {
          k->dist = z->dist + z_k_dist;
          changed = true;
          if (k->dist < min->dist) min = k;
        }
        if (--k->pred == 0 || k->dist <= (min->dist + k->min_in_weight)) {
          k->fixed = true;
          k->fixed_ts = high_resolution_clock::now();
          r_set.push_back(k);
        } else if (changed) {
          pushWrap(heap,k->node,k->dist);
        }
      }

      if (r_set.empty() && !min->fixed && cmp(min->dist, heap.top().dist)){
        // We're done, but before we break, let's just check whether we have the new min in the q set
        // That is, if the heap is not empty and the current min is higher than the min in the q
        // set no point in pushing back to the heap, where it would have to bubble up.
        min->fixed = true;
        min->fixed_ts = high_resolution_clock::now();
        r_set.push_back(min);
      }
    }
  }
}

template<typename GNode,
         typename Graph,
         typename Container,
         typename R>
inline int push_back_edges_of_node(const GNode& node,
                                   Graph& graph,
                                   Container& c,
                                   const R& edgeRange) {
  int edges_pushed = 0;
  for (auto& edge : edgeRange(node)) {
    c.push_back(std::make_pair(node, *edge));
    edges_pushed++;
  }
  return edges_pushed;
}

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename GEdge = typename GType::Graph::edge_iterator::value_type,
          typename GNodeData = typename GType::Graph::node_data_type,
          typename Traits = typename GType::Traits>
void parSP1VerticesAlgo(Graph& graph,
                        const GNode& source,
                        const P& pushWrap,
                        const R& edgeRange) {

  using Heap = galois::ThreadSafeMinHeap<T>;
  using WorkItem = T;
  using PSchunk = galois::worklists::PerSocketChunkFIFO<CHUNK_SIZE>;
  using Dist = typename Traits::Dist;

  constexpr galois::MethodFlag flag = galois::MethodFlag::UNPROTECTED;

  Heap heap;
  std::atomic<uint32_t> last_heap_pop_dist(0);
  std::atomic<int> r_set_counter(1);
  std::mutex mutex;
  auto& source_data = graph.getData(source);
  source_data.fixed = true;
  source_data.fixed_ts = high_resolution_clock::now();

  galois::for_each(galois::iterate({T{source, 0}}),
                   [&](WorkItem z_item, auto& ctx) {

                     const auto& z_data = graph.getData(z_item.src, flag);
                     //std::cout << "Processing node " << z_data.node << std::endl;

                     for (auto& z_edge : edgeRange(z_item.src)) {
                       auto k = graph.getEdgeDst(z_edge);
                       auto& k_data = graph.getData(k, flag);

                       //std::cout << "Processing edge " << *z_edge << " from " << z_item.src << " to dst : " << k_data.node << " Current dist: " << k_dist << " New dist: " << new_dist << std::endl;
                       if (k_data.fixed) continue;

                       auto z_k_dist = graph.getEdgeData(z_edge, flag);
                       auto new_dist = z_data.dist + z_k_dist;
                       unsigned int k_dist = k_data.dist;;
                       bool changed = false;

                       while (new_dist < k_dist) {
                         //std::cout << "Trying to relax to " << new_dist << std::endl;
                         if (k_data.dist.compare_exchange_weak(k_dist, new_dist, std::memory_order_relaxed)) {
                           changed = true;
                           break;
                         }
                         k_dist = k_data.dist;
                         new_dist = z_data.dist + z_k_dist;
                       }

                       // If the k vertex is now fixed, push the edges to the r set, otherwise push k
                       // to the heap.
                       if (--k_data.pred == 0) {
                         k_data.fixed = true;
                         k_data.fixed_ts = high_resolution_clock::now();
                         //std::cout << "Fixed " << k_data.node << " to " << k_data.dist << " at: " << duration_cast<microseconds>(k_data.fixed_ts - start).count() << std::endl;
                         //std::cout << "SP1 cond " << k_data.pred << " SP2 cond" << (last_heap_pop_dist.load() + k_data.min_in_weight) << std::endl;
                         pushWrap(ctx, k, k_data.dist);
                         ++r_set_counter;
                       } else if (changed){
                         pushWrap(heap, k, k_data.dist);
                       }
                     }

                     if (--r_set_counter == 0) {

                       uint32_t min_dist = 0;
                       uint32_t items_pushed = 0;
                       while(!heap.empty()) {
                         auto j = heap.top();
                         auto& j_data = graph.getData(j.src, flag);

                         if (j_data.fixed || j_data.dist < j.dist) {
                           heap.pop();
                           if (r_set_counter.load() > 0) break;
                           continue;
                         }

                         heap.pop();
                         last_heap_pop_dist = (unsigned int)j_data.dist;
                         min_dist = j_data.dist;
                         j_data.fixed = true;
			 j_data.fixed_ts = high_resolution_clock::now();
                         ++r_set_counter;
                         pushWrap(ctx, j.src, j.dist);
                         items_pushed++;

                         if (!(UNLIKELY(!heap.empty()) && heap.top().dist == min_dist)) break;
                       }
                     }
                   },
                   galois::wl<PSchunk>(),
                   galois::no_conflicts(),
                   galois::loopname("sssp_parsp2_inner"));
}

template <typename GType,
          typename T,
          typename P,
          typename R,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename GEdge = typename GType::Graph::edge_iterator::value_type,
          typename GNodeData = typename GType::Graph::node_data_type,
          typename Traits = typename GType::Traits>
void parSP2VerticesAlgo(Graph& graph,
                        const GNode& source,
                        const P& pushWrap,
                        const R& edgeRange) {

  using Heap = galois::ThreadSafeMinHeap<T>;
  using WorkItem = T;
  using PSchunk = galois::worklists::PerSocketChunkFIFO<CHUNK_SIZE>;
  using Dist = typename Traits::Dist;

  constexpr galois::MethodFlag flag = galois::MethodFlag::UNPROTECTED;

  Heap heap;
  std::atomic<uint32_t> min_dist(0);
  std::atomic<int> r_set_counter(1);
  std::mutex mutex;
  auto& source_data = graph.getData(source);
  source_data.fixed = true;
  source_data.fixed_ts = high_resolution_clock::now();

  galois::for_each(galois::iterate({T{source, 0}}),
                   [&](WorkItem z_item, auto& ctx) {

                     const auto& z_data = graph.getData(z_item.src, flag);
                     //std::cout << "Processing node " << z_data.node << std::endl;

                     for (auto& z_edge : edgeRange(z_item.src)) {
                       auto k = graph.getEdgeDst(z_edge);
                       auto& k_data = graph.getData(k, flag);

                       //std::cout << "Processing edge " << *z_edge << " from " << z_item.src << " to dst : " << k_data.node << " Current dist: " << k_dist << " New dist: " << new_dist << std::endl;
                       if (k_data.fixed) continue;

                       auto z_k_dist = graph.getEdgeData(z_edge, flag);
                       auto new_dist = z_data.dist + z_k_dist;
                       unsigned int k_dist = k_data.dist;;
                       bool changed = false;

                       while (new_dist < k_dist) {
                         //std::cout << "Trying to relax to " << new_dist << std::endl;
                         if (k_data.dist.compare_exchange_weak(k_dist, new_dist, std::memory_order_relaxed)) {
                           changed = true;
                           break;
                         }
                         k_dist = k_data.dist;
                         new_dist = z_data.dist + z_k_dist;
                       }

                       // If the k vertex is now fixed, push the edges to the r set, otherwise push k
                       // to the heap.
                       --k_data.pred;
                       if (k_dist <= min_dist + k_data.min_in_weight || k_data.pred <= 0) {
                         k_data.fixed = true;
                         k_data.fixed_ts = high_resolution_clock::now();
                         //std::cout << "Fixed " << k_data.node << " to " << k_data.dist << " at: " << duration_cast<microseconds>(k_data.fixed_ts - start).count() << std::endl;
                         //std::cout << "SP1 cond " << k_data.pred << " SP2 cond" << (last_heap_pop_dist.load() + k_data.min_in_weight) << std::endl;
                         ++r_set_counter;
                         pushWrap(ctx, k, k_data.dist);
                       } else if (changed){
                         pushWrap(heap, k, k_data.dist);
                       }
                     }

                     if (--r_set_counter == 0) {
                       while(!heap.empty()) {
                         auto j = heap.top();
                         auto& j_data = graph.getData(j.src, flag);
                         heap.pop();
                         if (j_data.fixed || j_data.dist < j.dist) {
                           if (r_set_counter.load() > 0) break;
                           continue;
                         }

                         min_dist = (unsigned int)j_data.dist;
                         j_data.fixed = true;
			 j_data.fixed_ts = high_resolution_clock::now();
                         ++r_set_counter;
                         pushWrap(ctx, j.src, j.dist);

                         if (heap.empty() || heap.top().dist != min_dist) break;
                       }
                     }
                   },
                   galois::wl<PSchunk>(),
                   galois::no_conflicts(),
                   galois::loopname("sssp_parsp2_inner"));
}

template <typename GType,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void topoAlgo(Graph& graph, const GNode& source) {
  using SSSP                 = typename Traits::SSSP;
  using Dist                 = typename Traits::Dist;

  galois::LargeArray<Dist> oldDist;
  oldDist.allocateInterleaved(graph.size());

  constexpr Dist INFTY = SSSP::DIST_INFINITY;
  galois::do_all(galois::iterate(0ul, graph.size()),
                 [&](size_t i) { oldDist.constructAt(i, INFTY); },
                 galois::no_stats(), galois::loopname("initDistArray"));

  graph.getData(source).dist = 0;

  galois::GReduceLogicalOR changed;
  size_t rounds = 0;

  do {

    ++rounds;
    changed.reset();

    galois::do_all(galois::iterate(graph),
                   [&](const GNode& n) {
                     const auto& sdata = graph.getData(n);

                     if (oldDist[n] > sdata.dist) {

                       oldDist[n] = sdata.dist;
                       changed.update(true);

                       for (auto e : graph.edges(n)) {
                         const auto newDist = sdata.dist + graph.getEdgeData(e);
                         auto dst           = graph.getEdgeDst(e);
                         auto& ddata        = graph.getData(dst);
                         galois::atomicMin(ddata.dist, newDist);
                       }
                     }
                   },
                   galois::steal(), galois::loopname("Update"));

  } while (changed.reduce());

  galois::runtime::reportStat_Single("SSSP-topo", "rounds", rounds);
}

template <typename GType,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void topoTileAlgo(Graph& graph, const GNode& source) {

  using SSSP                 = typename Traits::SSSP;
  using SrcEdgeTile          = typename Traits::SrcEdgeTile;
  using SrcEdgeTileMaker     = typename Traits::SrcEdgeTileMaker;

  galois::InsertBag<SrcEdgeTile> tiles;

  graph.getData(source).dist = 0;

  galois::do_all(galois::iterate(graph),
                 [&](const GNode& n) {
                   SSSP::pushEdgeTiles(
                       tiles, graph, n,
                       SrcEdgeTileMaker{n, SSSP::DIST_INFINITY});
                 },
                 galois::steal(), galois::loopname("MakeTiles"));

  galois::GReduceLogicalOR changed;
  size_t rounds = 0;

  do {
    ++rounds;
    changed.reset();

    galois::do_all(galois::iterate(tiles),
                   [&](SrcEdgeTile& t) {
                     const auto& sdata = graph.getData(t.src);

                     if (t.dist > sdata.dist) {

                       t.dist = sdata.dist;
                       changed.update(true);

                       for (auto e = t.beg; e != t.end; ++e) {
                         const auto newDist = sdata.dist + graph.getEdgeData(e);
                         auto dst           = graph.getEdgeDst(e);
                         auto& ddata        = graph.getData(dst);
                         galois::atomicMin(ddata.dist, newDist);
                       }
                     }
                   },
                   galois::steal(), galois::loopname("Update"));

  } while (changed.reduce());

  galois::runtime::reportStat_Single("SSSP-topo", "rounds", rounds);
}

template <typename GType,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode>
void reset_graph(Graph& graph, GNode& source){

  using SSSP = typename GType::Traits::SSSP;

  galois::do_all(galois::iterate(graph),
                 [&graph](GNode n) {
                   auto& data = graph.getData(n);
                   data.dist = SSSP::DIST_INFINITY;
		   data.pred = total_pred[data.node];
		   data.fixed = false;
                 });

  graph.getData(source).dist = 0;
}

template <typename GType,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void init_graph(Graph& graph, GNode& source, GNode& report) {

  using SSSP = typename Traits::SSSP;
  using Dist = typename Traits::Dist;
  std::cout << "Reading from file: " << filename << std::endl;
  galois::graphs::readGraph(graph, filename);
  std::cout << "Read " << graph.size() << " nodes, " << graph.sizeEdges()
            << " edges" << std::endl;

  if (startNode >= graph.size() || reportNode >= graph.size()) {
    std::cerr << "failed to set report: " << reportNode
              << " or failed to set source: " << startNode << "\n";
    assert(0);
    abort();
  }

  auto it = graph.begin();
  std::advance(it, startNode);
  source = *it;
  it     = graph.begin();
  std::advance(it, reportNode);
  report = *it;

  size_t approxNodeData = graph.size() * 64;
  // size_t approxEdgeData = graph.sizeEdges() * sizeof(typename
  // Graph::edge_data_type) * 2;
  galois::preAlloc(numThreads +
                   approxNodeData / galois::runtime::pagePoolSize());
  galois::reportPageAlloc("MeminfoPre");

  if (algo == deltaStep || algo == deltaTile || algo == serDelta ||
      algo == serDeltaTile) {
    std::cout << "INFO: Using delta-step of " << (1 << stepShift) << "\n";
    std::cout
        << "WARNING: Performance varies considerably due to delta parameter.\n";
    std::cout
        << "WARNING: Do not expect the default to be good for your graph.\n";
  }

   auto edgeRange = typename GType::Traits::OutEdgeRangeFn{graph};

   struct edgeComp {
     bool operator()( const Dist& lhs, const Dist& rhs ) const {
       return lhs < rhs;
     }
   };

  galois::do_all(galois::iterate(graph),
                 [&](GNode n) {
                   auto& data = graph.getData(n);
                   data.dist = SSSP::DIST_INFINITY;
                   data.min_in_weight = SSSP::DIST_INFINITY;
                   data.node = n;
                   //graph.sortEdgesByEdgeData(n, edgeComp());
                   //                   for (auto& e : edgeRange(n)) {
                   //  std::cout << "Node: " << n << " Edge: " << *e << " dist: " << graph.getEdgeData(e) << std::endl;
                   //}
                 });

  graph.getData(source).dist = 0;
  total_pred.resize(graph.size());
}

struct MatchPathSeparator {
  bool operator()( char ch ) const {
    return ch == '/';
  }
};

std::string basename( std::string const& pathname ) {
    return std::string(
        std::find_if( pathname.rbegin(), pathname.rend(),
                      MatchPathSeparator() ).base(),
        pathname.end() );
}

std::string removeExtension( std::string const& filename ) {
    std::string::const_reverse_iterator
                        pivot
            = std::find( filename.rbegin(), filename.rend(), '.' );
    return pivot == filename.rend()
        ? filename
        : std::string( filename.begin(), pivot.base() - 1 );
}

template <typename GType,
          typename Graph = typename GType::Graph,
          typename GNode = typename GType::GNode,
          typename Traits = typename GType::Traits>
void verify_and_report(Graph& graph, GNode& source, GNode& report, unsigned long exec_time) {
  galois::reportPageAlloc("MeminfoPost");

  std::cout << "Node " << reportNode << " has distance "
            << graph.getData(report).dist << "\n";

  if (!skipVerify) {
    if (Traits::SSSP::verify(graph, source)) {
      std::cout << "Verification successful.\n";
    } else {
      GALOIS_DIE("Verification failed");
    }
  }

  acc::accumulator_set<double, acc::stats<acc::tag::count,
                                          acc::tag::mean,
                                          acc::tag::variance,
                                          acc::tag::max,
                                          acc::tag::min,
                                          acc::tag::median>> stats;

  for (auto& node : graph) {
    auto& node_data = graph.getData(node);
    if (node_data.fixed) stats(duration_cast<microseconds>(node_data.fixed_ts - start).count());
  }

  auto count =  acc::count(stats);
  auto mean =  acc::mean(stats);
  auto median = acc::median(stats);
  auto max =  acc::max(stats);
  auto min = acc::min(stats);
  auto variance =  acc::variance(stats);

  std::cout << std::setprecision(16)
            << "count:    " << count    << '\n'
            << "mean:     " << mean     << '\n'
            << "median:   " << median   << '\n'
            << "max:      " << max      << '\n'
            << "min:      " << min      << '\n'
            << "variance: " << variance << '\n';

    //Auxiliary code for stat collection to CSV file
  std::string resultfile_name = "results.csv";
  std::string graph_name = removeExtension(basename(filename));
  std::ifstream in_data_stream(resultfile_name);
  bool print_header = false;
  if(in_data_stream.peek() == std::ifstream::traits_type::eof())
    print_header = true;
  in_data_stream.close();
  std::ofstream data_stream;
  data_stream.open(resultfile_name, std::ofstream::out | std::ofstream::app);
  if(print_header)
    data_stream << "Algo;Graph_name;Main_time;Count;Mean;Median;max;min;variance;";
  data_stream << std::endl << ALGO_NAMES[algo] << ';' << graph_name << ';' << exec_time << ';' << count << ';' << mean << ';' << median << ';' << max << ';' << min << ';' << variance;
  data_stream.close();

}

int main(int argc, char** argv) {
  using ParGType = GraphType<GraphTypes::ParGraphType>;
  using SerGType = GraphType<GraphTypes::SerGraphType>;

  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  //skipVerify = true;
  std::cout << "Running " << ALGO_NAMES[algo] << " algorithm" << std::endl;
  galois::StatTimer Tinit;
  galois::StatTimer Tmain;
  unsigned long Init_Time, Main_Time;

  switch (algo) {
    case deltaTile: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      deltaStepAlgo<ParGType, ParGType::Traits::SrcEdgeTile>(
          graph, source,
          ParGType::Traits::SrcEdgeTilePushWrap{graph},
          ParGType::Traits::TileRangeFn());
      Tmain.stop();

      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    case deltaStep: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      deltaStepAlgo<ParGType, ParGType::Traits::UpdateRequest>(
          graph, source,
          ParGType::Traits::ReqPushWrap(),
          ParGType::Traits::OutEdgeRangeFn{graph});
      Tmain.stop();

      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    case serDeltaTile: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      serDeltaAlgo<SerGType, SerGType::Traits::SrcEdgeTile>(
          graph, source,
          SerGType::Traits::SrcEdgeTilePushWrap{graph},
          SerGType::Traits::TileRangeFn());
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case serDelta: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      start = high_resolution_clock::now();

      Tmain.start();
      serDeltaAlgo<SerGType, SerGType::Traits::UpdateRequest>(
          graph, source,
          SerGType::Traits::ReqPushWrap(),
          SerGType::Traits::OutEdgeRangeFn{graph});
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case dijkstraTile: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      dijkstraAlgo<SerGType, SerGType::Traits::SrcEdgeTile>(
          graph, source,
          SerGType::Traits::SrcEdgeTilePushWrap{graph},
          SerGType::Traits::TileRangeFn());
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case dijkstra: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      dijkstraAlgo<SerGType, SerGType::Traits::UpdateRequest>(
          graph, source,
          SerGType::Traits::ReqPushWrap(),
          SerGType::Traits::OutEdgeRangeFn{graph});
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case topo: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      topoAlgo<ParGType>(graph, source);
      Tmain.stop();

      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    case topoTile: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      topoTileAlgo<ParGType>(graph, source);
      Tmain.stop();

      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    case serSP1: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      serSP1Algo<SerGType, SerGType::Traits::UpdateRequest>(
          graph, source,
          SerGType::Traits::ReqPushWrap(),
          edgeRange);
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case serSP2: {
      SerGType::Graph graph;
      SerGType::GNode source, report;
      init_graph<SerGType>(graph, source, report);
      auto edgeRange = SerGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      serSP2Algo<SerGType, SerGType::Traits::UpdateRequest>(
          graph, source,
          SerGType::Traits::ReqPushWrap(),
          edgeRange);
      Tmain.stop();

      verify_and_report<SerGType>(graph, source, report, Tmain.get());
      break;
    }
    case parSP1: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      parSP1VerticesAlgo<ParGType, ParGType::Traits::UpdateRequest>(
          graph, source,
          ParGType::Traits::ReqPushWrap(),
          edgeRange);
      Tmain.stop();

      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    case parSP2: {
      ParGType::Graph graph;
      ParGType::GNode source, report;
      init_graph<ParGType>(graph, source, report);
      auto edgeRange = ParGType::Traits::OutEdgeRangeFn{graph};
      calc_graph_predecessors(graph, edgeRange);

      start = high_resolution_clock::now();

      Tmain.start();
      parSP2VerticesAlgo<ParGType, ParGType::Traits::UpdateRequest>(
          graph, source,
          ParGType::Traits::ReqPushWrap(),
          edgeRange);
      Tmain.stop();

      //skipVerify = true;
      verify_and_report<ParGType>(graph, source, report, Tmain.get());
      break;
    }
    default:
      std::abort();
  }
  return 0;
}


//
//  This file is part of the FFEA simulation package
//
//  Copyright (c) by the Theory and Development FFEA teams,
//  as they appear in the README.md file.
//
//  FFEA is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  FFEA is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with FFEA.  If not, see <http://www.gnu.org/licenses/>.
//
//  To help us fund FFEA development, we humbly ask that you cite
//  the research papers on the package.
//

#include <cstring>
#include <iostream>
#include "dimensions.h"
#include "mat_vec_types.h"
#include "mat_vec_fns_II.h"
#include "FFEA_return_codes.h"
#include "BlobLite.h"
#include "VolumeIntersection.h"
#include <Eigen/Geometry>

int skipnlines(FILE *iFile, int n);
FILE *set_up_trajfile(const char *traj_filename);
void storeCoordsEigen(BlobLite &blob, Eigen::MatrixXd &coords);
double getRotationAngle(Eigen::MatrixXd &m1, Eigen::MatrixXd &m2);

FILE *set_up_trajfile(const char *traj_filename){

   FILE *trj;
   char c;
   if ((trj = fopen(traj_filename, "r")) == NULL) {
      cout << "Failed to open: " << traj_filename << endl;
      return NULL;
   }

   while(c != '*') {
     c = fgetc(trj);
   }

   skipnlines(trj, 2);

   return trj;

}

int skipnlines(FILE *iFile, int n) {

   int i=0;
   char *ignore;
   int len_crap = 256;
   char crap[len_crap];
   for (i=0; i<n; i++){
     ignore = fgets(crap, len_crap, iFile);
     //printf("crap: %s", crap);
   }
   return 0;

}

void storeCoordsEigen(BlobLite &blob, Eigen::MatrixXd &coords){

   for (int i=0; i<blob.num_nodes; i++){
      coords(0,i) = blob.coord[3*i  ];
      coords(1,i) = blob.coord[3*i+1];
      coords(2,i) = blob.coord[3*i+2];
   }

}

double getRotationAngle(Eigen::MatrixXd &m1, Eigen::MatrixXd &m2) {

   Eigen::Matrix<double,4,4> t = Eigen::umeyama(m1,m2);
   Eigen::Matrix<double,3,3> r = t.block<3,3>(0,0);
   Eigen::AngleAxisd t_angleAxis(r);

   return t_angleAxis.angle();
}

int main(int argc, char** argv) {

   // usage:
   // ./find_intersections scale1 nodes_file1 topology_file1 scale2 nodes_file2 topology_file2 traj_file num_steps
   cout << "argc = " << argc << endl;
   for(int i = 0; i < argc; i++)
      cout << "argv[" << i << "] = " << argv[i] << endl;

   int num_steps = atoi(argv[8]);

   // initialise the blobs:
   BlobLite b1, b2;
   b1.load_nodes(argv[2], atof(argv[1]));
   b1.load_topology(argv[3]);
   b2.load_nodes(argv[5], atof(argv[4]));
   b2.load_topology(argv[6]);

   // initialise the bouncing condition:
   bool bouncing = false;

   // initialise two arrays with the starting coordinates:
   Eigen::MatrixXd b1_ini(3,b1.num_nodes), b1_end(3,b1.num_nodes);
   Eigen::MatrixXd b2_ini(3,b2.num_nodes), b2_end(3,b2.num_nodes);
   // and the corresponding tolerance:
   scalar tol = 3; // degrees

   // initialise the trajectory file:
   FILE *trj = set_up_trajfile(argv[7]);
   if (trj == NULL) return 1;

   int checks = 0;
   int i_vol = 0;
   int i_cms = 0;
   arr3 cm1, cm2, aux;
   scalar d0 = 1000;
   scalar d = 10*d0;
   // for every time step:
   for (int t=0; t<num_steps; t++) {
     // load the coordinates of this time step:
     if (b1.read_nodes_from_file(trj)) break;
     skipnlines(trj,1);
     if (b2.read_nodes_from_file(trj)) break;
     skipnlines(trj,6);

     // 1st Check - Nodes only move on the x axis.
     // we store these coordinates because the system has to be put into box.
     if (t == 0) {
       storeCoordsEigen(b1, b1_ini);
       storeCoordsEigen(b2, b2_ini);
     } else if (t == num_steps - 2) {
       storeCoordsEigen(b1, b1_end);
       storeCoordsEigen(b2, b2_end);
     }

     // 2nd Check - Eventually the CMs change direction:
     b1.center_of_coord(cm1);
     b2.center_of_coord(cm2);
     d = arr3arr3Distance<scalar,arr3>(cm2, cm1);
     if (d >= d0) bouncing = true;
     d0 = d;

     // 3rd Check - Calculate if there is any tetrahedra intersecting:
     checks = 0;
     i_vol = 0;
     scalar tet1[4][3], tet2[4][3];
     for (int i=0; i<b1.num_elements; i++){
       // set up tetrahedron 1:
       for (int nn = 0; nn<4; nn++) {
         for (int nc = 0; nc<3; nc++) {
           tet1[nn][nc] = b1.coord[ b1.elem[ i*NUM_NODES_QUADRATIC_TET + nn ]*3 + nc ];
         }
       }
       for (int j=0; j<b2.num_elements; j++){
         // set up tetrahedron 2:
         for (int nn = 0; nn<4; nn++) {
           for (int nc = 0; nc<3; nc++) {
             tet2[nn][nc] = b2.coord[ b2.elem[ j*NUM_NODES_QUADRATIC_TET + nn ]*3 + nc ];
           }
         }
         checks += 1;
         if (volumeIntersection<scalar,arr3>(tet1, tet2, false, aux)){
           // cout << " ivol " << endl;
           i_vol += 1;
         }
       }   // elements in b2
     }     // elements in b1
   }       // num_steps
   fclose(trj);

   cout << "There were vol:" << i_vol << " in " << checks << " checkings" << endl;
   if (( ((float) i_vol) / checks ) > 0.00500 ) {
     cout << " There were too many intersections: " << (( (float) i_vol) / checks ) << endl;
     return 1;
   }

   if (!bouncing) {
     cout << " The blobs did not bounce back: the trajectory could be too short, or the steric repulsion not taken into account" << endl;
     return 1;
   } else cout << " The blobs did bounce back " << endl;

   // check the rotation angle:
   double b1_angle = getRotationAngle(b1_ini, b1_end);
   double b2_angle = getRotationAngle(b2_ini, b2_end);
   if (b1_angle > tol) {
     cout << " Blob 1 ended up with a too large rotation: " << b1_angle << endl;
     return 1;
   }
   if (b2_angle > tol) {
     cout << " Blob 2 ended up with a too large rotation: " << b2_angle << endl;
     return 1;
   }

   cout << "b1 suffered a rotation of angle: " << b1_angle << endl;
   cout << "b2 suffered a rotation of angle: " << b2_angle << endl;

   return 0;

}

     // scalar *tet3[4], tet4[4][3];
         // tet3[nn] = &b1.coord[ b1.elem[ i*NUM_NODES_QUADRATIC_TET + nn ]*3];
       /*for (int nn = 0; nn<4; nn++) {
         cout << "tet1: n" << nn << ": " << tet1[nn][0] << " " << tet1[nn][1] << " " << tet1[nn][2] << endl;
         cout << "tet3: n" << nn << ": " << tet3[nn][0] << " " << tet3[nn][1] << " " << tet3[nn][2] << endl;
       }*/


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2018 Sylko Olzscher
 *
 */

#include <smf/sml/scaler.h>
#include <boost/assert.hpp>

namespace node
{
	namespace sml
	{
		std::string scale_value(std::int64_t value, std::int8_t scaler)
		{
			//	Working with a string operation prevents us of losing precision.
			std::string str_value = std::to_string(value);

			//	treat 0 as special value
			if (value == 0)	return str_value;

			//	should at least contain a "0"
			BOOST_ASSERT_MSG(!str_value.empty(), "no number");
			if (str_value.empty())	return "0.0";	//	emergency exit

			if (scaler < 0)
			{

				//	negative
				while ((std::size_t)std::abs(scaler - 1) > str_value.size())
				{
					//	pad missing zeros
					str_value.insert(0, "0");
				}

				//	finish
				if (str_value.size() > (std::size_t)std::abs(scaler))
				{
					const std::size_t pos = str_value.size() + scaler;
					str_value.insert(pos, ".");
				}
			}
			else
			{
				//	simulating: result = value * pow10(scaler)
				while (scaler-- > 0)
				{
					//	append missing zeros
					str_value.append("0");
				}
			}

			return str_value;
		}
	}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_PARTITION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_PARTITION_HPP

#include <vector>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/geometry/core/coordinate_type.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace partition
{

typedef std::vector<std::size_t> index_vector_type;

template <int Dimension, typename Box>
inline void divide_box(Box const& box, Box& lower_box, Box& upper_box)
{
    typedef typename coordinate_type<Box>::type ctype;

    // Divide input box into two parts, e.g. left/right
    ctype two = 2;
    ctype mid = (geometry::get<min_corner, Dimension>(box)
            + geometry::get<max_corner, Dimension>(box)) / two;

    lower_box = box;
    upper_box = box;
    geometry::set<max_corner, Dimension>(lower_box, mid);
    geometry::set<min_corner, Dimension>(upper_box, mid);
}

// Divide collection into three subsets: lower, upper and oversized (not-fitting)
// (lower == left or bottom, upper == right or top)
template <typename OverlapsPolicy, typename InputCollection, typename Box>
static inline void divide_into_subsets(Box const& lower_box, Box const& upper_box,
        InputCollection const& collection,
        index_vector_type const& input,
        index_vector_type& lower,
        index_vector_type& upper,
        index_vector_type& exceeding)
{
    typedef boost::range_iterator<index_vector_type const>::type index_iterator_type;

    for(index_iterator_type it = boost::begin(input);
        it != boost::end(input);
        ++it)
    {
        bool const lower_overlapping = OverlapsPolicy::apply(lower_box, collection[*it]);
        bool const upper_overlapping = OverlapsPolicy::apply(upper_box, collection[*it]);

        if (lower_overlapping && upper_overlapping)
        {
            exceeding.push_back(*it);
        }
        else if (lower_overlapping)
        {
            lower.push_back(*it);
        }
        else if (upper_overlapping)
        {
            upper.push_back(*it);
        }
        else
        {
            // Is nowhere! Should not occur!
            BOOST_ASSERT(true);
        }
    }
}

// Match collection with itself
template <typename InputCollection, typename Policy>
static inline void handle_one(InputCollection const& collection,
        index_vector_type const& input,
        Policy& policy)
{
    typedef boost::range_iterator<index_vector_type const>::type index_iterator_type;
    // Quadratic behaviour at lowest level (lowest quad, or all exceeding)
    for(index_iterator_type it1 = boost::begin(input); it1 != boost::end(input); ++it1)
    {
        index_iterator_type it2 = it1;
        for(++it2; it2 != boost::end(input); ++it2)
        {
            policy.apply(collection[*it1], collection[*it2]);
        }
    }
}

// Match collection 1 with collection 2
template <typename InputCollection, typename Policy>
static inline void handle_two(
        InputCollection const& collection1, index_vector_type const& input1,
        InputCollection const& collection2, index_vector_type const& input2,
        Policy& policy)
{
    typedef boost::range_iterator<index_vector_type const>::type index_iterator_type;
    for(index_iterator_type it1 = boost::begin(input1); it1 != boost::end(input1); ++it1)
    {
        for(index_iterator_type it2 = boost::begin(input2); it2 != boost::end(input2); ++it2)
        {
            policy.apply(collection1[*it1], collection2[*it2]);
        }
    }
}

template
<
    int Dimension,
    typename Box,
    typename OverlapsPolicy,
    typename VisitBoxPolicy
>
class partition_one_collection
{
    typedef std::vector<std::size_t> index_vector_type;
    typedef typename coordinate_type<Box>::type ctype;
    typedef partition_one_collection
            <
                1 - Dimension,
                Box,
                OverlapsPolicy,
                VisitBoxPolicy
            > sub_divide;

    template <typename InputCollection, typename Policy>
    static inline void next_level(Box const& box,
            InputCollection const& collection,
            index_vector_type const& input,
            int level, int min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        if (boost::size(input) > 0)
        {
            if (boost::size(input) > min_elements && level < 100)
            {
                sub_divide::apply(box, collection, input, level + 1, min_elements, policy, box_policy);
            }
            else
            {
                handle_one(collection, input, policy);
            }
        }
    }

public :
    template <typename InputCollection, typename Policy>
    static inline void apply(Box const& box,
            InputCollection const& collection,
            index_vector_type const& input,
            int level,
            int min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        box_policy.apply(box, level);

        Box lower_box, upper_box;
        divide_box<Dimension>(box, lower_box, upper_box);

        index_vector_type lower, upper, exceeding;
        divide_into_subsets<OverlapsPolicy>(lower_box, upper_box, collection, input, lower, upper, exceeding);

        if (boost::size(exceeding) > 0)
        {
            // All what is not fitting a partition should be combined
            // with each other, and with all which is fitting.
            handle_one(collection, exceeding, policy);
            handle_two(collection, exceeding, collection, lower, policy);
            handle_two(collection, exceeding, collection, upper, policy);
        }

        // Recursively call operation both parts
        next_level(lower_box, collection, lower, level, min_elements, policy, box_policy);
        next_level(upper_box, collection, upper, level, min_elements, policy, box_policy);
    }
};

template
<
    int Dimension,
    typename Box,
    typename OverlapsPolicy,
    typename VisitBoxPolicy
>
class partition_two_collections
{
    typedef std::vector<std::size_t> index_vector_type;
    typedef typename coordinate_type<Box>::type ctype;
    typedef partition_two_collections
            <
                1 - Dimension,
                Box,
                OverlapsPolicy,
                VisitBoxPolicy
            > sub_divide;

    template <typename InputCollection, typename Policy>
    static inline void next_level(Box const& box,
            InputCollection const& collection1, index_vector_type const& input1,
            InputCollection const& collection2, index_vector_type const& input2,
            int level, int min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        if (boost::size(input1) > 0 && boost::size(input2) > 0)
        {
            if (boost::size(input1) > min_elements
                && boost::size(input2) > min_elements
                && level < 100)
            {
                sub_divide::apply(box, collection1, input1, collection2, input2, level + 1, min_elements, policy, box_policy);
            }
            else
            {
                box_policy.apply(box, level + 1);
                handle_two(collection1, input1, collection2, input2, policy);
            }
        }
    }

public :
    template <typename InputCollection, typename Policy>
    static inline void apply(Box const& box,
            InputCollection const& collection1, index_vector_type const& input1,
            InputCollection const& collection2, index_vector_type const& input2,
            int level,
            int min_elements,
            Policy& policy, VisitBoxPolicy& box_policy)
    {
        box_policy.apply(box, level);

        Box lower_box, upper_box;
        divide_box<Dimension>(box, lower_box, upper_box);

        index_vector_type lower1, upper1, exceeding1;
        index_vector_type lower2, upper2, exceeding2;
        divide_into_subsets<OverlapsPolicy>(lower_box, upper_box, collection1, input1, lower1, upper1, exceeding1);
        divide_into_subsets<OverlapsPolicy>(lower_box, upper_box, collection2, input2, lower2, upper2, exceeding2);

        if (boost::size(exceeding1) > 0)
        {
            // All exceeding from 1 with 2:
            handle_two(collection1, exceeding1, collection2, exceeding2, policy);

            // All exceeding from 1 with lower and upper of 2:
            handle_two(collection1, exceeding1, collection2, lower2, policy);
            handle_two(collection1, exceeding1, collection2, upper2, policy);
        }
        if (boost::size(exceeding2) > 0)
        {
            // All exceeding from 2 with lower and upper of 1:
            handle_two(collection1, lower1, collection2, exceeding2, policy);
            handle_two(collection1, upper1, collection2, exceeding2, policy);
        }

        next_level(lower_box, collection1, lower1, collection2, lower2, level, min_elements, policy, box_policy);
        next_level(upper_box, collection1, upper1, collection2, upper2, level, min_elements, policy, box_policy);
    }
};

}} // namespace detail::partition

struct visit_no_policy
{
    template <typename Box>
    static inline void apply(Box const&, int )
    {}
};

template
<
    typename Box,
    typename ExpandPolicy,
    typename OverlapsPolicy,
    typename VisitBoxPolicy = visit_no_policy
>
class partition
{
    typedef std::vector<std::size_t> index_vector_type;

    template <typename InputCollection>
    static inline void expand_to_collection(InputCollection const& collection, Box& total, index_vector_type& index_vector)
    {
        std::size_t index = 0;
        for(typename boost::range_iterator<InputCollection const>::type it
            = boost::begin(collection);
            it != boost::end(collection);
            ++it, ++index)
        {
            ExpandPolicy::apply(total, *it);
            index_vector.push_back(index);
        }
    }

public :
    template <typename InputCollection, typename VisitPolicy>
    static inline void apply(InputCollection const& collection,
            VisitPolicy& visitor,
            int min_elements = 16,
            VisitBoxPolicy box_visitor = visit_no_policy()
            )
    {
        if (boost::size(collection) > min_elements)
        {
            index_vector_type index_vector;
            Box total;
            assign_inverse(total);
            expand_to_collection(collection, total, index_vector);

            detail::partition::partition_one_collection
                <
                    0, Box,
                    OverlapsPolicy,
                    VisitBoxPolicy
                >::apply(total, collection, index_vector, 0, min_elements, visitor, box_visitor);
        }
        else
        {
            typedef typename boost::range_iterator<InputCollection const>::type iterator_type;
            for(iterator_type it1 = boost::begin(collection); it1 != boost::end(collection); ++it1)
            {
                iterator_type it2 = it1;
                for(++it2; it2 != boost::end(collection); ++it2)
                {
                    visitor.apply(*it1, *it2);
                }
            }
        }
    }

    template <typename InputCollection, typename VisitPolicy>
    static inline void apply(InputCollection const& collection1,
                InputCollection const& collection2,
                VisitPolicy& visitor,
                int min_elements = 16,
                VisitBoxPolicy box_visitor = visit_no_policy()
                )
    {
        if (boost::size(collection1) > min_elements && boost::size(collection2) > min_elements)
        {
            index_vector_type index_vector1, index_vector2;
            Box total;
            assign_inverse(total);
            expand_to_collection(collection1, total, index_vector1);
            expand_to_collection(collection2, total, index_vector2);

            detail::partition::partition_two_collections
                <
                    0, Box, OverlapsPolicy, VisitBoxPolicy
                >::apply(total,
                    collection1, index_vector1,
                    collection2, index_vector2,
                    0, min_elements, visitor, box_visitor);
        }
        else
        {
            typedef typename boost::range_iterator<InputCollection const>::type iterator_type;
            for(iterator_type it1 = boost::begin(collection1); it1 != boost::end(collection1); ++it1)
            {
                for(iterator_type it2 = boost::begin(collection2); it2 != boost::end(collection2); ++it2)
                {
                    visitor.apply(*it1, *it2);
                }
            }
        }
    }

};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RING_IDENTIFIER_HPP


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

#ifndef AMIRAMESHREADER_HH
#define AMIRAMESHREADER_HH

#include <algorithm>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <cassert>
#include <stdexcept>

#include <boost/timer/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <dune/common/fvector.hh>

#include <amiramesh/AmiraMesh.h>
#include "fem/lagrangespace.hh"
#include "io/ioTools.hh"
#include "io/vtk.hh"

// forward declaration
template <> class std::complex<float>;

namespace Kaskade
{
  /// Reader for Amira meshes.
  /**
   * Note that the Amira mesh file must be in ASCII format. Currently UG and ALUSimplex grids are supported.
   *
   */
  namespace AmiraMeshReader
  {
    namespace ImplementationDetails{

      /**
       * \cond internals
       */
      template <class value_type> struct TypeId{};

      template <> struct TypeId<unsigned char>        { static int const id = HxBYTE; };
      template <> struct TypeId<int>                  { static int const id = HxINT32; };
      template <> struct TypeId<float>                { static int const id = HxFLOAT; };
      template <> struct TypeId<double>               { static int const id = HxDOUBLE; };
      template <> struct TypeId<std::complex<float> > { static int const id = HxCOMPLEX; };
      /**
       * \endcond
       */

      std::string exceptionMessage(std::string const& function, std::string const& gridfile, int line)
      {
        return std::string("In AmiraMeshReader::") + function + ", line " + boost::lexical_cast<std::string>(line) + ": Error reading " + gridfile + "\n";
      }

      std::string exceptionMessage(std::string const& function, std::string const& location, std::string const& component, int line)
      {
        return std::string("In AmiraMeshReader::") + function + ", line " + boost::lexical_cast<std::string>(line)  + std::string(": Error reading ") + component + " of " + location + "\n";
      }

      template <class value_type, int components>
      void extractData(AmiraMesh::Data* dataObj, std::vector<Dune::FieldVector<value_type,components> >& data)
      {
        size_t const n= dataObj->location()->nNodes();
        std::cout << n << " entries...";
        data.clear();
        data.reserve(n);
        data.insert(data.begin(),n,Dune::FieldVector<value_type,components>(0));

        value_type const* dataPtr = static_cast<value_type const*>(dataObj->dataPtr());
        for (size_t i=0; i<n; ++i)
          for (int j=0; j<components; ++j)
            data[i][j] = dataPtr[i*components+j];

      }

      template <class value_type, int components>
      bool readData(AmiraMesh& am, char const* location, char const* name, std::vector<Dune::FieldVector<value_type,components> >& data)
      {
        std::string pname=name;
        std::cout << "trying to read " << pname << " of " << location << ": " << std::flush;
        int const id = ImplementationDetails::TypeId<value_type>::id;
        AmiraMesh::Data* dataObj = am.findData(location,id,components,name);
        if (!dataObj){
          std::cout << "empty." << std::endl << std::flush;
          return false;
        }

        extractData(dataObj,data);
        std::cout << "done." << std::endl << std::flush;
        return true;
      }

    } // End of ImplementationDetails

    /// function reading an Amira mesh file in ASCII format.
    /**
     * This function reads vertices, tetrahedra and, if defined, boundary triangles
     * of an Amira mesh file into a grid and returns this grid.
     *
     * \param Scalar scalar type of the vertex coordinates
     * \param gridfile the name of the Amira mesh file
     * \param initialGridSize optional parameter for reserving memory for the grid
     * \return pointer on the created grid
     */
    template<class Grid, class Scalar>
    std::unique_ptr<Grid> readGrid(std::string const& gridfile, int initialGridSize = 0, bool measureTime = false){
      int const dim = Grid::dimension;
      std::vector<Dune::FieldVector<Scalar,dim> >    vertices;
      std::vector<Dune::FieldVector<int,dim+1> >     cells;
      std::vector<Dune::FieldVector<int,dim> >       boundary;

      // open grid file.
      std::cout << "Reading amira mesh\n";
      std::cout << "opening " << gridfile << '\n';
      AmiraMesh* am = AmiraMesh::read(gridfile.c_str());
      if(!am){
        std::string const mes = ImplementationDetails::exceptionMessage("readGrid(std::string const&, int)", gridfile, __LINE__);
        throw std::runtime_error(mes);
      }

      // read vertices
      bool exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",vertices);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, int)", "Nodes", "Coordinates", __LINE__));
      }
      // read cells
      std::string cellName;
      if(dim == 2) cellName = "Triangles";
      if(dim == 3) cellName = "Tetrahedra";
      if(dim != 2 && dim != 3){
        delete(am);
        throw std::runtime_error("Sorry!\nOnly 2D and 3D grids are supported.");
      }
      exists = ImplementationDetails::readData(*am,cellName.c_str(),"Nodes",cells);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, int)", cellName, "Nodes", __LINE__));
      }
      // read boundary segments if defined
      bool boundaryExists = ImplementationDetails::readData(*am,"BoundaryTriangles","Nodes",boundary);

      // close grid file
      delete(am);

      // We use start indices 0. Amira uses 1. Correct indices.
      for (size_t i=0; i<cells.size(); ++i) {
        for (int j=0; j<dim+1; ++j) {
          cells[i][j] -= 1;
          if (cells[i][j]>=vertices.size()){
            std::string message = std::string("In ") + __FILE__ + " line " + boost::lexical_cast<std::string>(__LINE__) + ": cell node " + boost::lexical_cast<std::string>(i) + "[" + boost::lexical_cast<std::string>(j) + "]=" + boost::lexical_cast<std::string>(cells[i][j]) + " exceeds vertex numbers!\n";
            throw std::runtime_error(message);
          }
        }
      }

      if(boundaryExists)
        for (size_t i=0; i<boundary.size(); ++i)
          for (int j=0; j<dim; ++j)
            boundary[i][j] -= 1;

      // Create grid.
      std::cout << std::endl;
      std::cout << "preparing grid creation\n";

      Dune::GridFactory<Grid> factory = IOTools::FactoryGenerator<dim,Grid>::createFactory(initialGridSize);
      std::cout << "inserting vertices...";
      for (size_t i=0; i<vertices.size(); ++i) {
        Dune::FieldVector<double,dim> pos;
        for(int j=0; j<dim; ++j)
          pos[j] = (double)vertices[i][j];
        factory.insertVertex(pos);
      }
      std::cout << "done." << std::endl;

      if(boundaryExists){
        std::cout << "inserting boundary segments...";
        for(size_t i=0; i<boundary.size(); ++i){
          std::vector<unsigned int> tmp(dim);
          for(size_t j=0; j<dim; ++j){
            assert(boundary[i][j]>=0 && boundary[i][j]<vertices.size());
            tmp[j] = boundary[i][j];
          }
          factory.insertBoundarySegment(tmp);
        }
        std::cout << "done." << std::endl;
      }

      std::cout << "inserting elements...";
      for (size_t i=0; i<cells.size(); ++i) {
        std::vector<unsigned int> tmp(dim+1);
        for (int j=0; j<dim+1; ++j) {
          assert(cells[i][j]>=0 && cells[i][j]<vertices.size());
          tmp[j] = cells[i][j];
        }
        factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,dim),tmp);
      }
      std::cout << "done." << std::endl;

      boost::timer::cpu_timer timer;
      std::unique_ptr<Grid> result(factory.createGrid());
      std::cout << "grid creation finished in " << boost::timer::format(timer.elapsed()) << std::endl;
      std::cout << std::endl;

      return result;
    }

    /// function reading an Amira mesh file in ASCII format.
    /**
     * This function reads vertices, tetrahedra and, if defined, boundary triangles
     * of an Amira mesh file into a grid and returns this grid.
     *
     * \param Scalar scalar type of the vertex coordinates
     * \param gridfile the name of the Amira mesh file
     * \param initialGridSize optional parameter for reserving memory for the grid
     * \return pointer on the created grid
     */
    template<class Grid, class Scalar>
    std::unique_ptr<Grid> readGrid(std::string const& gridfile, std::vector<int>& boundaryIds, int initialGridSize = 0){
      int const dim = Grid::dimension;
      std::vector<Dune::FieldVector<Scalar,dim> >    vertices;
      std::vector<Dune::FieldVector<int,dim+1> >     cells;
      std::vector<Dune::FieldVector<int,dim> >       boundary;

      // open grid file.
      std::cout << "Reading amira mesh\n";
      std::cout << "opening " << gridfile << '\n';
      AmiraMesh* am = AmiraMesh::read(gridfile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, std::vector<int>&, int)", gridfile, __LINE__));

      // read vertices
      bool exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",vertices);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, std::vector<int>&, int)", "Nodes", "Coordinates", __LINE__));
      }
      // read cells
      std::string cellName;
      if(dim == 2) cellName = "Triangles";
      if(dim == 3) cellName = "Tetrahedra";
      if(dim != 2 && dim != 3){
        delete(am);
        throw std::runtime_error("Sorry!\nOnly 2D and 3D grids are supported.");
      }
      exists = ImplementationDetails::readData(*am,cellName.c_str(),"Nodes",cells);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, std::vector<int>&, int)", cellName, "Nodes", __LINE__));
      }
      // read boundary segments if defined
      exists = ImplementationDetails::readData(*am,"BoundaryTriangles","Nodes",boundary);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readGrid(std::string const&, std::vector<int>&, int)", "BoundaryTriangles", "Nodes", __LINE__));
      }

      typedef std::vector<Dune::FieldVector<unsigned char, 1> > TmpIdVector;
      TmpIdVector boundaryID;
      ImplementationDetails::readData(*am,"BoundaryTriangles","Id",boundaryID);

      // close grid file
      delete(am);

      // We use start indices 0. Amira uses 1. Correct indices.
      for (size_t i=0; i<cells.size(); ++i) {
        for (int j=0; j<dim+1; ++j) {
          cells[i][j] -= 1;
          if (cells[i][j]>=vertices.size()){
            std::string message = std::string("In ") + __FILE__ + " line " + boost::lexical_cast<std::string>(__LINE__) + ": cell node " + boost::lexical_cast<std::string>(i) + "[" + boost::lexical_cast<std::string>(j) + "]=" + boost::lexical_cast<std::string>(cells[i][j]) + " exceeds vertex numbers!\n";
            throw std::runtime_error(message);
          }
        }
      }

      for (size_t i=0; i<boundary.size(); ++i)
        for (int j=0; j<dim; ++j)
          boundary[i][j] -= 1;

      // Create grid.
      std::cout << std::endl;
      std::cout << "preparing grid creation\n";

      Dune::GridFactory<Grid> factory = IOTools::FactoryGenerator<dim,Grid>::createFactory(initialGridSize);
      std::cout << "inserting vertices...";
      for (size_t i=0; i<vertices.size(); ++i) {
        Dune::FieldVector<double,dim> pos;
        for(int j=0; j<dim; ++j)
          pos[j] = (double)vertices[i][j];
        factory.insertVertex(pos);
      }
      std::cout << "done." << std::endl;

      std::cout << "inserting boundary segments...";
      for(size_t i=0; i<boundary.size(); ++i){
        std::vector<unsigned int> tmp(dim);
        for(size_t j=0; j<dim; ++j){
          assert(boundary[i][j]>=0 && boundary[i][j]<vertices.size());
          tmp[j] = boundary[i][j];
        }
        factory.insertBoundarySegment(tmp);
      }
      std::cout << "done." << std::endl;

      std::cout << "inserting elements...";
      for (size_t i=0; i<cells.size(); ++i) {
        std::vector<unsigned int> tmp(dim+1);
        for (int j=0; j<dim+1; ++j) {
          assert(cells[i][j]>=0 && cells[i][j]<vertices.size());
          tmp[j] = cells[i][j];
        }
        factory.insertElement(Dune::GeometryType(Dune::GeometryType::simplex,dim),tmp);
      }
      std::cout << "done." << std::endl;

      boost::timer::cpu_timer timer;
      std::unique_ptr<Grid> grid(factory.createGrid());
      std::cout << "grid creation finished in " << boost::timer::format(timer.elapsed()) << std::endl;

      // get boundary segment indices
      std::cout << "sorting boundary ids...";
      IOTools::BoundarySegmentIndexWrapper<dim, Grid>::readBoundarySegmentIndices(*grid, factory, vertices, boundary, boundaryID, boundaryIds);
      std::cout << "done." << std::endl;

      return grid;
    }

    /// function reading additional data from an Amira mesh file in ASCII format.
    /**
     * \param Scalar scalar type of the data in the Amira mesh file
     * \param FunctionSpaceElement type of FE-Function to store the additional data in
     * \param datafile the name of the Amira mesh file containing additional data
     * \param dataname the name of the data in the Amira mesh file
     * \param componentname the name of the data's components in the Amira mesh file
     * \param data fe-function object for storing the additional data
     */
    template<class Scalar, class FunctionSpaceElement>
    void readData(std::string const& datafile, std::string const& dataname, std::string const& componentname, FunctionSpaceElement &data){
      constexpr int numberOfComponents = FunctionSpaceElement::StorageValueType::dimension;
      // FunctionSpaceElement::StorageValueType in general uses floating point precision.
      // Thus, in order to read integer values or characters, the DataVector is defined manually.
      // When reading the information into the function space element the data is
      // casted to the desired type.
      typedef std::vector<Dune::FieldVector<Scalar, numberOfComponents> > DataVector;
      DataVector dataVector;

      // open data file
      std::cout << std::endl;
      std::cout << "Reading additional data" << std::endl;
      std::cout << "opening " << datafile << std::endl;
      AmiraMesh* am = AmiraMesh::read(datafile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readData(std::string const&, std::string const&, std::string const&, FunctionSpaceElement&)", datafile, __LINE__));

      bool exists = ImplementationDetails::readData(*am, dataname.c_str(), componentname.c_str(), dataVector);
      if(!exists){
        delete(am);
        throw std::runtime_error( ImplementationDetails::exceptionMessage("readData(std::string const&, std::string const&, std::string const&, FunctionSpaceElement&)", dataname, componentname, __LINE__) );
      }

      // close data file
      delete(am);

      std::cout << "number of entries: " << dataVector.size() << std::endl;
      std::cout << "number of components: " << numberOfComponents << std::endl;
      std::cout << "data object: " << data.coefficients().size() << "x" << data.coefficients()[0].size() << std::endl;

      // reading prescribed data into function space element
      for(size_t i=0; i<dataVector.size(); ++i)
        for(size_t j=0; j<numberOfComponents; ++j)
          data.coefficients()[i][j] = (typename FunctionSpaceElement::Scalar) dataVector[i][j];

    }

    /// function reading additional data for each vertex from an Amira mesh file in ASCII format.
    /**
     * \param Scalar scalar type of the data in the Amira mesh file
     * \param numberOfComponents number of components of each entry of the data
     * \param datafile the name of the Amira mesh file containing additional node data
     * \param dataname the name of the data in the Amira mesh file
     * \param componentname the name of the data's components in the Amira mesh file
     * \param data vector containing the data
     */
    template<class Scalar, int numberOfComponents>
    void readData(std::string const& datafile, std::string const& dataname, std::string const& componentname,
        std::vector<Dune::FieldVector<Scalar,numberOfComponents> > &data){
      // open data file
      std::cout << std::endl;
      std::cout << "Reading additional vertex data" << std::endl;
      std::cout << "opening " << datafile << std::endl;
      AmiraMesh* am = AmiraMesh::read(datafile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readData(std::string const&, std::string const&, std::string const&, std::vector<Dune::FieldVector>&)", datafile, __LINE__));

      bool exists = ImplementationDetails::readData(*am, dataname.c_str(), componentname.c_str(), data);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readData(std::string const&, std::string const&, std::string const&, std::vector<Dune::FieldVector>&)", dataname, componentname, __LINE__));
      }

      // close data file
      delete(am);
    }

    /// function reading return a FE-function describing the deformation.
    /**
     * Function reading a deformed and an undeformed geometry from Amira mesh files
     * in ASCII format. These geometries must possess the same number of nodes. For each
     * vertex the difference between the geometries is calculated. This difference is stored
     * into the FE-function at index 0 of the variable set data.
     *
     * \param datafile the name of the Amira mesh file containing the deformed geometry
     * \param gridfile the name of the Amira mesh file containing the undeformed geometry
     * \param data fe-function object for storing the deformation of type VariableSet::VariableSet
     */
    template <class VarSetDesc>
    void readDeformationIntoVariableSetRepresentation(std::string const& gridfile, std::string const& datafile,
						      typename VarSetDesc::VariableSet& data){
      using namespace boost::fusion;
      int const numberOfComponents = result_of::at_c<typename VarSetDesc::VariableSet::Functions, 0>::type::Components;
      typedef std::vector<Dune::FieldVector<float,numberOfComponents> > DataVector;
      std::vector<Dune::FieldVector<float,numberOfComponents> > vertices;
      DataVector nodeData;

      // open grid file.
      std::cout << "Reading undeformed geometry\n";
      std::cout << "opening " << gridfile << '\n';
      AmiraMesh* am = AmiraMesh::read(gridfile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformationIntoVariableSetRepresentation(std::string const&, std::string const&,  VariableSet::VariableSet&)", gridfile, __LINE__));

      // read vertices
      bool exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",vertices);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformationIntoVariableSetRepresentation(std::string const&, std::string const&,  VariableSet::VariableSet&)","Nodes","Coordinates",__LINE__));
      }

      // close grid file
      delete(am);

      // open data file
      std::cout << std::endl;
      std::cout << "Reading deformed geometry" << std::endl;
      std::cout << "opening " << datafile << std::endl;
      am = AmiraMesh::read(datafile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformationIntoVariableSetRepresentation(std::string const&, std::string const&,  VariableSet::VariableSet&)", datafile, __LINE__));

      exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",nodeData);
      if(!exists){
        delete(am);
        throw std::runtime_error("readDeformationIntoVariableSetRepresentation(std::string const&, std::string const&,  VariableSet::VariableSet&)", "Nodes", "Coordinates", __LINE__);
      }

      // close data file
      delete(am);

      // check for correct sizes
      assert(nodeData.size()==vertices.size());

      // get deformation
      for(size_t i=0; i<nodeData.size(); ++i)
        nodeData[i]=nodeData[i]-vertices[i];

      // reading prescribed data into function space
      std::vector<double> tmpVec(nodeData.size()*numberOfComponents);
      for(size_t i=0; i<nodeData.size(); ++i)
        for(size_t j=0; j<numberOfComponents; ++j)
          tmpVec[numberOfComponents*i+j] = (double)nodeData[i][j];

      data.read(tmpVec.begin());
    }

    /// function reading return a fe-function describing the deformation.
    /**
     * Function reading a deformed and a undeformed geometry from Amira mesh files
     * in ASCII format. These geometry must possess the same number of nodes. For each
     * vertex the difference between the geometries is calculated. This difference is read
     * into the fe-function object data.
     *
     * \param datafile the name of the Amira mesh file containing the deformed geometry
     * \param gridfile the name of the Amira mesh file containing the undeformed geometry
     * \param data fe-function object for storing the deformation
     */
    template<class FunctionSpaceElement, class FileScalar=float>
    void readDeformation(std::string const& gridfile, std::string const& datafile, FunctionSpaceElement &data) {
      int const numberOfComponents = FunctionSpaceElement::Components;
      typedef std::vector<Dune::FieldVector<FileScalar,numberOfComponents> > DataVector;
      typedef typename FunctionSpaceElement::Scalar Scalar;
      std::vector<Dune::FieldVector<FileScalar,numberOfComponents> > vertices;
      DataVector nodeData;

      // open grid file.
      std::cout << "Reading undeformed geometry\n";
      std::cout << "opening " << gridfile << '\n';
      AmiraMesh* am = AmiraMesh::read(gridfile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", gridfile, __LINE__));

      // read vertices
      bool exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",vertices);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", "Nodes", "Coordinates", __LINE__));
      }

      // close grid file
      delete(am);

      // open data file
      std::cout << std::endl;
      std::cout << "Reading deformed geometry" << std::endl;
      std::cout << "opening " << datafile << std::endl;
      am = AmiraMesh::read(datafile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", datafile, __LINE__));

      exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",nodeData);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", "Nodes", "Coordinates", __LINE__));
      }

      // close data file
      delete(am);

      // check for correct sizes
      assert(nodeData.size()==vertices.size());

      // get deformation
      for(size_t i=0; i<nodeData.size(); ++i)
        nodeData[i]=nodeData[i]-vertices[i];

      // reading prescribed data into function space
      std::vector<Scalar> tmpVec(nodeData.size()*numberOfComponents);
      for(size_t i=0; i<nodeData.size(); ++i)
        for(size_t j=0; j<FunctionSpaceElement::Components; ++j)
          data.coefficients()[i][j] = (Scalar) nodeData[i][j];
    }

    template<class GridView, class FunctionSpaceElement, class FileScalar=float>
    void readDeformation2(GridView const& gridView, std::string const& datafile, FunctionSpaceElement &data){
      int const numberOfComponents = FunctionSpaceElement::Components;
      typedef std::vector<Dune::FieldVector<FileScalar,numberOfComponents> > DataVector;
      typedef typename FunctionSpaceElement::Scalar Scalar;
      std::vector<Dune::FieldVector<FileScalar,numberOfComponents> > vertices;
      DataVector nodeData;

      // open data file
      std::cout << "Reading deformed geometry" << std::endl;
      std::cout << "opening " << datafile << std::endl;
      AmiraMesh *am = AmiraMesh::read(datafile.c_str());
      if(!am) throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", datafile, __LINE__));

      bool exists = ImplementationDetails::readData(*am,"Nodes","Coordinates",nodeData);
      if(!exists){
        delete(am);
        throw std::runtime_error(ImplementationDetails::exceptionMessage("readDeformation(std::string const&, std::string const&, FunctionSpaceElement&)", "Nodes", "Coordinates", __LINE__));
      }

      // close data file
      delete(am);

      // check for correct sizes
      assert(nodeData.size()==vertices.size());

      // get deformation
      auto vIter = gridView.template begin<GridView::dimension>();
      auto vend = gridView.template end<GridView::dimension>();
      size_t i=0;
      for(;vIter!=vend;++vIter)
      {
        nodeData[i] -= vIter->geometry().corner(0);
        ++i;
      }
      //      for(size_t i=0; i<nodeData.size(); ++i)
//        nodeData[i]=nodeData[i]-vertices[i];

      // reading prescribed data into function space
      std::vector<Scalar> tmpVec(nodeData.size()*numberOfComponents);
      for(size_t i=0; i<nodeData.size(); ++i)
        for(size_t j=0; j<FunctionSpaceElement::Components; ++j)
          data.coefficients()[i][j] = (Scalar) nodeData[i][j];
    }

    /// Read boundary indices and save as scalar field in .vtu-file
    /**
     * \param gridfile name of the Amira mesh file
     * \param savefilename name of the output file (optional), default: "boundaryConditions.vtu"
     * \param initialGridSize initial grid size in mb(only for UGGrid)
     */
    template <class Grid>
    void boundaryConditionsToScalarField(std::string const& gridfile, std::string const& savefilename=std::string("boundaryConditions"), int initialGridSize = 0) {

      int const dim = Grid::dimension;
      // read grid
      typedef typename Grid::LeafGridView LeafView;
      std::vector<int> boundaryIndices;
      std::unique_ptr<Grid> grid = readGrid<Grid,float>(gridfile, boundaryIndices);
      GridManager<Grid> gridManager(grid);

      // create variable set
      typedef FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> > Space;
      Space space(gridManager, gridManager.grid().leafView(), 1);
      typedef boost::fusion::vector<Space const*> Spaces;
      Spaces spaces(&space);
      typedef boost::fusion::vector< VariableDescription<0,1,0> > VariableDescriptions;
      typedef VariableSetDescription<Spaces,VariableDescriptions> VarSetDesc;
      std::string name[1] = { "boundary ids" };
      VarSetDesc varSetDesc(spaces, name);
      typename VarSetDesc::VariableSet indices(varSetDesc);

      // read boundary indices
      std::vector<Dune::FieldVector<int,dim> > boundaryVertices;

      std::string const comp_name("BoundaryTriangles");
      std::string const node_name("Nodes");

      ImplementationDetails::readData<int,dim>(gridfile, comp_name, node_name, boundaryVertices);
      std::vector<double> tmp(gridManager.grid().size(dim),0);
      for(int i=0; i<boundaryIndices.size(); ++i){
        tmp[boundaryVertices[i][0]-1] = boundaryIndices[i];
        tmp[boundaryVertices[i][1]-1] = boundaryIndices[i];
        tmp[boundaryVertices[i][2]-1] = boundaryIndices[i];
      }
      indices.read(tmp.begin());

      // write file
      IoOptions options;
      options.outputType = IoOptions::ascii;
      writeVTKFile(gridManager.grid().leafView(), varSetDesc, indices, savefilename,options);
    }
  }
} // namespace Kaskade
#endif