// Copyright (C) 2007 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

/**************************************************************************
 * This source file implements the Delta-stepping algorithm:              *
 *                                                                        *
 *   Ulrich Meyer and Peter Sanders. Parallel Shortest Path for Arbitrary *
 *   Graphs. In Proceedings from the 6th International Euro-Par           *
 *   Conference on Parallel Processing, pages 461--470, 2000.             *
 *                                                                        *
 *   Ulrich Meyer, Peter Sanders: [Delta]-stepping: A Parallelizable      *
 *   Shortest Path Algorithm. J. Algorithms 49(1): 114-152, 2003.         *
 *                                                                        *
 * There are several potential optimizations we could still perform for   *
 * this algorithm implementation:                                         *
 *                                                                        *
 *   - Implement "shortcuts", which bound the number of reinsertions      *
 *     in a single bucket (to one). The computation of shortcuts looks    *
 *     expensive in a distributed-memory setting, but it could be         *
 *     ammortized over many queries.                                      *
 *                                                                        *
 *   - The size of the "buckets" data structure can be bounded to         *
 *     max_edge_weight/delta buckets, if we treat the buckets as a        *
 *     circular array.                                                    *
 *                                                                        *
 *   - If we partition light/heavy edges ahead of time, we could improve  *
 *     relaxation performance by only iterating over the right portion    *
 *     of the out-edge list each time.                                    *
 *                                                                        *
 *   - Implement a more sophisticated algorithm for guessing "delta",     *
 *     based on the shortcut-finding algorithm.                           *
 **************************************************************************/
#ifndef BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <limits>
#include <list>
#include <vector>
#include <boost/graph/parallel/container_traits.hpp>
#include <boost/graph/parallel/properties.hpp>
#include <boost/graph/distributed/detail/dijkstra_shortest_paths.hpp>
#include <utility> // for std::pair
#include <functional> // for std::logical_or
#include <boost/graph/parallel/algorithm.hpp> // for all_reduce
#include <cassert>
#include <algorithm> // for std::min, std::max
#include <boost/graph/parallel/simple_trigger.hpp>

#ifdef PBGL_DELTA_STEPPING_DEBUG
#  include <iostream> // for std::cerr
#endif

namespace boost { namespace graph { namespace distributed {

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
class delta_stepping_impl {
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::degree_size_type Degree;
  typedef typename property_traits<EdgeWeightMap>::value_type Dist;
  typedef typename boost::graph::parallel::process_group_type<Graph>::type
    ProcessGroup;

  typedef std::list<Vertex> Bucket;
  typedef typename Bucket::iterator BucketIterator;
  typedef typename std::vector<Bucket*>::size_type BucketIndex;

  typedef detail::dijkstra_msg_value<DistanceMap, PredecessorMap> MessageValue;

  enum {
    // Relax a remote vertex. The message contains a pair<Vertex,
    // MessageValue>, the first part of which is the vertex whose
    // tentative distance is being relaxed and the second part
    // contains either the new distance (if there is no predecessor
    // map) or a pair with the distance and predecessor.
    msg_relax
  };

public:
  delta_stepping_impl(const Graph& g,
                      PredecessorMap predecessor,
                      DistanceMap distance,
                      EdgeWeightMap weight,
                      Dist delta);

  delta_stepping_impl(const Graph& g,
                      PredecessorMap predecessor,
                      DistanceMap distance,
                      EdgeWeightMap weight);

  void run(Vertex s);

private:
  // Relax the edge (u, v), creating a new best path of distance x.
  void relax(Vertex u, Vertex v, Dist x);

  // Synchronize all of the processes, by receiving all messages that
  // have not yet been received.
  void synchronize();

  // Handle a relax message that contains only the target and
  // distance. This kind of message will be received when the
  // predecessor map is a dummy_property_map.
  void handle_relax_message(Vertex v, Dist x) { relax(v, v, x); }

  // Handle a relax message that contains the source (predecessor),
  // target, and distance. This kind of message will be received when
  // the predecessor map is not a dummy_property_map.
  void handle_relax_message(Vertex v, const std::pair<Dist, Vertex>& p)
  { relax(p.second, v, p.first); }

  // Setup triggers for msg_relax messages
  void setup_triggers();

  void handle_msg_relax(int /*source*/, int /*tag*/,
                        const std::pair<Vertex, typename MessageValue::type>& data,
                        trigger_receive_context)
  { handle_relax_message(data.first, data.second); }

  const Graph& g;
  PredecessorMap predecessor;
  DistanceMap distance;
  EdgeWeightMap weight;
  Dist delta;
  ProcessGroup pg;
  typename property_map<Graph, vertex_owner_t>::const_type owner;
  typename property_map<Graph, vertex_local_t>::const_type local;

  // A "property map" that contains the position of each vertex in
  // whatever bucket it resides in.
  std::vector<BucketIterator> position_in_bucket;

  // Bucket data structure. The ith bucket contains all local vertices
  // with (tentative) distance in the range [i*delta,
  // (i+1)*delta).
  std::vector<Bucket*> buckets;

  // This "dummy" list is used only so that we can initialize the
  // position_in_bucket property map with non-singular iterators. This
  // won't matter for most implementations of the C++ Standard
  // Library, but it avoids undefined behavior and allows us to run
  // with library "debug modes".
  std::list<Vertex> dummy_list;

  // A "property map" that states which vertices have been deleted
  // from the bucket in this iteration.
  std::vector<bool> vertex_was_deleted;
};

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
delta_stepping_impl(const Graph& g,
                    PredecessorMap predecessor,
                    DistanceMap distance,
                    EdgeWeightMap weight,
                    Dist delta)
    : g(g),
      predecessor(predecessor),
      distance(distance),
      weight(weight),
      delta(delta),
      pg(boost::graph::parallel::process_group_adl(g), attach_distributed_object()),
      owner(get(vertex_owner, g)),
      local(get(vertex_local, g))
{
  setup_triggers();
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
delta_stepping_impl(const Graph& g,
                    PredecessorMap predecessor,
                    DistanceMap distance,
                    EdgeWeightMap weight)
    : g(g),
      predecessor(predecessor),
      distance(distance),
      weight(weight),
      pg(boost::graph::parallel::process_group_adl(g), attach_distributed_object()),
      owner(get(vertex_owner, g)),
      local(get(vertex_local, g))
{
  using boost::parallel::all_reduce;
  using boost::parallel::maximum;
  using std::max;

  // Compute the maximum edge weight and degree
  Dist max_edge_weight = 0;
  Degree max_degree = 0;
  BGL_FORALL_VERTICES_T(u, g, Graph) {
    max_degree = max BOOST_PREVENT_MACRO_SUBSTITUTION (max_degree, out_degree(u, g));
    BGL_FORALL_OUTEDGES_T(u, e, g, Graph)
      max_edge_weight = max BOOST_PREVENT_MACRO_SUBSTITUTION (max_edge_weight, get(weight, e));
  }

  max_edge_weight = all_reduce(pg, max_edge_weight, maximum<Dist>());
  max_degree = all_reduce(pg, max_degree, maximum<Degree>());

  // Take a guess at delta, based on what works well for random
  // graphs.
  delta = max_edge_weight / max_degree;
  if (delta == 0)
    delta = 1;

  setup_triggers();
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
run(Vertex s)
{
  Dist inf = (std::numeric_limits<Dist>::max)();

  // None of the vertices are stored in the bucket.
  position_in_bucket.clear();
  position_in_bucket.resize(num_vertices(g), dummy_list.end());

  // None of the vertices have been deleted
  vertex_was_deleted.clear();
  vertex_was_deleted.resize(num_vertices(g), false);

  // No path from s to any other vertex, yet
  BGL_FORALL_VERTICES_T(v, g, Graph)
    put(distance, v, inf);

  // The distance to the starting node is zero
  if (get(owner, s) == process_id(pg))
    // Put "s" into its bucket (bucket 0)
    relax(s, s, 0);
  else
    // Note that we know the distance to s is zero
    cache(distance, s, 0);

  BucketIndex max_bucket = (std::numeric_limits<BucketIndex>::max)();
  BucketIndex current_bucket = 0;
  do {
    // Synchronize with all of the other processes.
    synchronize();

    // Find the next bucket that has something in it.
    while (current_bucket < buckets.size()
           && (!buckets[current_bucket] || buckets[current_bucket]->empty()))
      ++current_bucket;
    if (current_bucket >= buckets.size())
      current_bucket = max_bucket;

#ifdef PBGL_DELTA_STEPPING_DEBUG
    std::cerr << "#" << process_id(pg) << ": lowest bucket is #"
              << current_bucket << std::endl;
#endif
    // Find the smallest bucket (over all processes) that has vertices
    // that need to be processed.
    using boost::parallel::all_reduce;
    using boost::parallel::minimum;
    current_bucket = all_reduce(pg, current_bucket, minimum<BucketIndex>());

    if (current_bucket == max_bucket)
      // There are no non-empty buckets in any process; exit.
      break;

#ifdef PBGL_DELTA_STEPPING_DEBUG
    if (process_id(pg) == 0)
      std::cerr << "Processing bucket #" << current_bucket << std::endl;
#endif

    // Contains the set of vertices that have been deleted in the
    // relaxation of "light" edges. Note that we keep track of which
    // vertices were deleted with the property map
    // "vertex_was_deleted".
    std::vector<Vertex> deleted_vertices;

    // Repeatedly relax light edges
    bool nonempty_bucket;
    do {
      // Someone has work to do in this bucket.

      if (current_bucket < buckets.size() && buckets[current_bucket]) {
        Bucket& bucket = *buckets[current_bucket];
        // For each element in the bucket
        while (!bucket.empty()) {
          Vertex u = bucket.front();

#ifdef PBGL_DELTA_STEPPING_DEBUG
          std::cerr << "#" << process_id(pg) << ": processing vertex "
                    << get(vertex_global, g, u).second << "@"
                    << get(vertex_global, g, u).first
                    << std::endl;
#endif

          // Remove u from the front of the bucket
          bucket.pop_front();

          // Insert u into the set of deleted vertices, if it hasn't
          // been done already.
          if (!vertex_was_deleted[get(local, u)]) {
            vertex_was_deleted[get(local, u)] = true;
            deleted_vertices.push_back(u);
          }

          // Relax each light edge.
          Dist u_dist = get(distance, u);
          BGL_FORALL_OUTEDGES_T(u, e, g, Graph)
            if (get(weight, e) <= delta) // light edge
              relax(u, target(e, g), u_dist + get(weight, e));
        }
      }

      // Synchronize with all of the other processes.
      synchronize();

      // Is the bucket empty now?
      nonempty_bucket = (current_bucket < buckets.size()
                         && buckets[current_bucket]
                         && !buckets[current_bucket]->empty());
     } while (all_reduce(pg, nonempty_bucket, std::logical_or<bool>()));

    // Relax heavy edges for each of the vertices that we previously
    // deleted.
    for (typename std::vector<Vertex>::iterator iter = deleted_vertices.begin();
         iter != deleted_vertices.end(); ++iter) {
      // Relax each heavy edge.
      Vertex u = *iter;
      Dist u_dist = get(distance, u);
      BGL_FORALL_OUTEDGES_T(u, e, g, Graph)
        if (get(weight, e) > delta) // heavy edge
          relax(u, target(e, g), u_dist + get(weight, e));
    }

    // Go to the next bucket: the current bucket must already be empty.
    ++current_bucket;
  } while (true);

  // Delete all of the buckets.
  for (typename std::vector<Bucket*>::iterator iter = buckets.begin();
       iter != buckets.end(); ++iter) {
    if (*iter) {
      delete *iter;
      *iter = 0;
    }
  }
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
relax(Vertex u, Vertex v, Dist x)
{
#ifdef PBGL_DELTA_STEPPING_DEBUG
  std::cerr << "#" << process_id(pg) << ": relax("
            << get(vertex_global, g, u).second << "@"
            << get(vertex_global, g, u).first << ", "
            << get(vertex_global, g, v).second << "@"
            << get(vertex_global, g, v).first << ", "
            << x << ")" << std::endl;
#endif

  if (x < get(distance, v)) {
    // We're relaxing the edge to vertex v.
    if (get(owner, v) == process_id(pg)) {
      // Compute the new bucket index for v
      BucketIndex new_index = static_cast<BucketIndex>(x / delta);

      // Make sure there is enough room in the buckets data structure.
      if (new_index >= buckets.size()) buckets.resize(new_index + 1, 0);

      // Make sure that we have allocated the bucket itself.
      if (!buckets[new_index]) buckets[new_index] = new Bucket;

      if (get(distance, v) != (std::numeric_limits<Dist>::max)()
          && !vertex_was_deleted[get(local, v)]) {
        // We're moving v from an old bucket into a new one. Compute
        // the old index, then splice it in.
        BucketIndex old_index
          = static_cast<BucketIndex>(get(distance, v) / delta);
        buckets[new_index]->splice(buckets[new_index]->end(),
                                   *buckets[old_index],
                                   position_in_bucket[get(local, v)]);
      } else {
        // We're inserting v into a bucket for the first time. Put it
        // at the end.
        buckets[new_index]->push_back(v);
      }

      // v is now at the last position in the new bucket
      position_in_bucket[get(local, v)] = buckets[new_index]->end();
      --position_in_bucket[get(local, v)];

      // Update predecessor and tentative distance information
      put(predecessor, v, u);
      put(distance, v, x);
    } else {
#ifdef PBGL_DELTA_STEPPING_DEBUG
      std::cerr << "#" << process_id(pg) << ": sending relax("
                << get(vertex_global, g, u).second << "@"
                << get(vertex_global, g, u).first << ", "
                << get(vertex_global, g, v).second << "@"
                << get(vertex_global, g, v).first << ", "
            << x << ") to " << get(owner, v) << std::endl;

#endif
      // The vertex is remote: send a request to the vertex's owner
      send(pg, get(owner, v), msg_relax,
           std::make_pair(v, MessageValue::create(x, u)));

      // Cache tentative distance information
      cache(distance, v, x);
    }
  }
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
synchronize()
{
  using boost::parallel::synchronize;

  // Synchronize at the process group level.
  synchronize(pg);

  // Receive any relaxation request messages.
//   typedef typename ProcessGroup::process_id_type process_id_type;
//   while (optional<std::pair<process_id_type, int> > stp = probe(pg)) {
//     // Receive the relaxation message
//     assert(stp->second == msg_relax);
//     std::pair<Vertex, typename MessageValue::type> data;
//     receive(pg, stp->first, stp->second, data);

//     // Turn it into a "relax" call
//     handle_relax_message(data.first, data.second);
//   }
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>::
setup_triggers()
{
  using boost::graph::parallel::simple_trigger;

  simple_trigger(pg, msg_relax, this,
                 &delta_stepping_impl::handle_msg_relax);
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_shortest_paths
  (const Graph& g,
   typename graph_traits<Graph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance, EdgeWeightMap weight,
   typename property_traits<EdgeWeightMap>::value_type delta)
{
  // The "distance" map needs to act like one, retrieving the default
  // value of infinity.
  set_property_map_role(vertex_distance, distance);

  // Construct the implementation object, which will perform all of
  // the actual work.
  delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>
    impl(g, predecessor, distance, weight, delta);

  // Run the delta-stepping algorithm. The results will show up in
  // "predecessor" and "weight".
  impl.run(s);
}

template<typename Graph, typename PredecessorMap, typename DistanceMap,
         typename EdgeWeightMap>
void
delta_stepping_shortest_paths
  (const Graph& g,
   typename graph_traits<Graph>::vertex_descriptor s,
   PredecessorMap predecessor, DistanceMap distance, EdgeWeightMap weight)
{
  // The "distance" map needs to act like one, retrieving the default
  // value of infinity.
  set_property_map_role(vertex_distance, distance);

  // Construct the implementation object, which will perform all of
  // the actual work.
  delta_stepping_impl<Graph, PredecessorMap, DistanceMap, EdgeWeightMap>
    impl(g, predecessor, distance, weight);

  // Run the delta-stepping algorithm. The results will show up in
  // "predecessor" and "weight".
  impl.run(s);
}

} } } // end namespace boost::graph::distributed

#endif // BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP


/*=========================================================================
 *
 *  Copyright 2011-2013 The University of North Carolina at Chapel Hill
 *  All rights reserved.
 *
 *  Licensed under the MADAI Software License. You may obtain a copy of
 *  this license at
 *
 *         https://madai-public.cs.unc.edu/visualization/software-license/
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
#include <cassert> // assert
#include <cstdlib> // std::atof
#include <cmath> // std::sqrt, std::pow
#include <iostream> // std::cout
#include <fstream> // std::ifstream
#include <vector> // std::vector
#include <string> // std::string
#include <sstream> // std::stringstream
#include <iomanip> // std::setw
#include <limits> // inifity
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include "ApplicationUtilities.h"
#include "GaussianProcessEmulatorDirectoryFormatIO.h"
#include "Paths.h"
#include "System.h"

int main(int argc, char ** argv) {
  if (argc < 3) {
    std::cerr
      << "Usage\n  " << argv[0]
      << " statistics_directory trace_file\n\n";
    return EXIT_FAILURE;
  }
  std::string statisticsDirectory( argv[1] );
  madai::EnsurePathSeparatorAtEnd( statisticsDirectory );

  std::vector< madai::Parameter > parameters;
  int numberOfParameters = 0;

  if ( ! madai::GaussianProcessEmulatorDirectoryFormatIO::ParseParameters(
           parameters, numberOfParameters, statisticsDirectory, false )) {
    std::cerr
      << "Could not read parameters from prior file '"
      << statisticsDirectory << madai::Paths::PARAMETER_PRIORS_FILE << "'\n";
    return EXIT_FAILURE;
  }
  assert (numberOfParameters = parameters.size());

  std::vector< std::string > outputNames;
  int numberOfOutputs = 0;

  if ( ! madai::GaussianProcessEmulatorDirectoryFormatIO::ParseOutputs(
           outputNames, numberOfOutputs, statisticsDirectory, false )) {
    std::cerr
      << "Could not read outputs from prior file '"
      << statisticsDirectory << madai::Paths::PARAMETER_PRIORS_FILE << "'\n";
    return EXIT_FAILURE;
  }
  assert (numberOfOutputs = outputNames.size());

  std::string traceFile( statisticsDirectory );
  traceFile.append( argv[2] );

  if ( !madai::System::IsFile( traceFile ) ) {
    std::cerr << "Trace file '" << traceFile << "' does not exist or is a directory.\n";
    return EXIT_FAILURE;
  }

  std::ifstream file(traceFile.c_str(), std::ios_base::in | std::ios_base::binary);
  if ( !file.good() ) {
    std::cerr << "Error reading trace file '" << traceFile << "'.\n";
    return EXIT_FAILURE;
  }
  boost::iostreams::filtering_streambuf<boost::iostreams::input> inbuf;
  if( madai::IsTraceCompressed(traceFile) ) {
      inbuf.push(boost::iostreams::gzip_decompressor());
  }
  inbuf.push(file);
  std::istream trace(&inbuf);

  std::string header;
  std::getline(trace,header);
  std::vector<std::string> headers = madai::SplitString(header, ',');

  int numberOfFields = headers.size();
  bool gradientsPresent = false;
  if( numberOfFields == numberOfParameters + 3*numberOfOutputs + 1 ) {
    numberOfFields -= 2*numberOfOutputs;
    gradientsPresent = true;
  }
  assert(numberOfFields == numberOfParameters + numberOfOutputs + 1);

  std::string line;
  size_t lineCount = 0, bestIndex = 0;
  std::vector< double > sums(numberOfFields - 1, 0.0);
  std::vector< std::vector< double > > values(numberOfFields - 1);

  double bestLogLikelihood = -std::numeric_limits< double >::infinity();

  while (std::getline(trace,line)) {
    std::vector<std::string> fields = madai::SplitString(line, ',');
    assert(numberOfFields == (int) fields.size() - (gradientsPresent?2*numberOfOutputs:0));
    for (int i = 0; i < numberOfFields - 1; ++i) {
      double value = std::atof(fields[i].c_str());
      values[i].push_back(value);
      sums[i] += value;
    }
    double logLikelihood = std::atof(fields[numberOfFields - 1].c_str());
    if (logLikelihood > bestLogLikelihood) {
      bestLogLikelihood = logLikelihood;
      bestIndex = lineCount;
    }
    ++lineCount;
  }

  file.close();
  std::vector< double > means(numberOfFields - 1,0.0);

  std::vector< double > priorStdDev(numberOfFields - 1,0.0);
  for (int i = 0; i < numberOfParameters; ++i) {
    priorStdDev[i] =
      parameters[i].GetPriorDistribution()->GetStandardDeviation();
  }
  std::cout << std::setw(14) << "parameter";
  std::cout << std::setw(14) << "mean";
  std::cout << std::setw(14) << "std.dev.";
  std::cout << std::setw(14) << "scaled dev.";
  std::cout << std::setw(14) << "best value";
  std::cout << '\n';

  for (int i = 0; i < numberOfFields - 1; ++i) {
    means[i] = sums[i] / lineCount;
  }
  for (int i = 0; i < numberOfParameters; ++i) {
    double variance = 0.0;
    for (size_t k = 0; k < lineCount; ++k) {
      variance += std::pow(values[i][k] - means[i], 2);
    }
    variance /= lineCount;
    std::cout
      << std::setw(14) << parameters[i].m_Name
      << std::setw(14) << means[i]
      << std::setw(14) << std::sqrt(variance)
      << std::setw(14) << std::sqrt(variance) / priorStdDev[i]
      << std::setw(14) << values[i][bestIndex]
      << '\n';
  }

  // Print the relative log-likelihood from the best point
  std::cout << "\nbest log likelihood\n";
  std::cout << std::setw(14) << bestLogLikelihood << "\n";

  std::vector< std::vector< double > > covariancematrix;
  for (int i = 0; i < numberOfFields - 1; ++i)
    covariancematrix.push_back(std::vector< double >(numberOfFields - 1, 0.0));

  for (int i = 0; i < numberOfFields - 1; ++i) {
    for (int j = 0; j <= i; ++j) {
      double covariance = 0.0;
      for (size_t k = 0; k < lineCount; ++k) {
        covariance += (values[i][k] - means[i]) * (values[j][k] - means[j]);
      }
      covariancematrix[i][j] = covariance /= lineCount;
      if (i != j)
        covariancematrix[j][i] = covariancematrix[i][j];
    }
  }

  std::cout << "\ncovariance:\n";
  std::cout << std::setw(14) << "";
  for (int j = 0; j < numberOfParameters; ++j)
    std::cout << std::setw(14) << parameters[j].m_Name;
  std::cout << "\n";
  for (int i = 0; i < numberOfParameters; ++i) {
    std::cout << std::setw(14) << parameters[i].m_Name;
    for (int j = 0; j < numberOfParameters; ++j) {
      std::cout << std::setw(14) << covariancematrix[i][j];
    }
    std::cout << "\n";
  }

  std::cout << "\nscaled covariance:\n";
  std::cout << std::setw(14) << "";
  for (int j = 0; j < numberOfParameters; ++j)
    std::cout << std::setw(14) << parameters[j].m_Name;
  std::cout << "\n";
  for (int i = 0; i < numberOfParameters; ++i) {
    std::cout << std::setw(14) << parameters[i].m_Name;
    for (int j = 0; j < numberOfParameters; ++j) {
      std::cout << std::setw(14) << covariancematrix[i][j] / (
          priorStdDev[i] * priorStdDev[j]);
    }
    std::cout << "\n";
  }

  std::cout << "\nobservable-parameter correlation:\n";
  std::cout << std::setw(14) << "";
  for (int j = 0; j < numberOfParameters; ++j)
    std::cout << std::setw(14) << parameters[j].m_Name;
  std::cout << "\n";
  for (int i = 0; i < numberOfOutputs; ++i) {
    std::cout << std::setw(14) << outputNames[i];
    for (int j = 0; j < numberOfParameters; ++j) {
      std::cout << std::setw(14) << covariancematrix[numberOfParameters + i][j] / (
          std::sqrt(covariancematrix[numberOfParameters + i][numberOfParameters + i]*
          covariancematrix[j][j]));
    }
    std::cout << "\n";
  }

  return EXIT_SUCCESS;
}


/*********************************************************************************
*  This file is part of reference implementation of SIGGRAPH Asia 2021 Paper     *
*  `Efficient and Robust Discrete Conformal Equivalence with Boundary`           *
*  v1.0                                                                          *
*                                                                                *
*  The MIT License                                                               *
*                                                                                *
*  Permission is hereby granted, free of charge, to any person obtaining a       *
*  copy of this software and associated documentation files (the "Software"),    *
*  to deal in the Software without restriction, including without limitation     *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,      *
*  and/or sell copies of the Software, and to permit persons to whom the         *
*  Software is furnished to do so, subject to the following conditions:          *
*                                                                                *
*  The above copyright notice and this permission notice shall be included in    *
*  all copies or substantial portions of the Software.                           *
*                                                                                *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR    *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,      *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE  *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER        *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING       *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS  *
*  IN THE SOFTWARE.                                                              *
*                                                                                *
*  Author(s):                                                                    *
*  Marcel Campen, Institute for Computer Science, Osnabrück University, Germany. *
*  Ryan Capouellez, Hanxiao Shen, Leyi Zhu, Daniele Panozzo, Denis Zorin,        *
*  Courant Institute of Mathematical Sciences, New York University, USA          *
*                                          *                                     *
*********************************************************************************/

#include "Halfedge.hh"
#include <Eigen/Sparse>

void FV_to_NOB(const std::vector<std::vector<int>> &F,
               std::vector<int> &next_he,
               std::vector<int> &opp,
               std::vector<int> &bnd_loops,
               std::vector<int> &vtx_reindex)
{
    // Get the cumulative sum of the number of halfedges per face
    int n_f = F.size();
    int n_he = 0;
    std::vector<int> F_he_cumsum(n_f);
    for (int i = 0; i < n_f; ++i)
    {
        n_he += F[i].size();
        F_he_cumsum[i] = n_he;
    }

    // Create a list of indices of halfedges per face, sequentially numbered, not including
    // boundary-loop faces
    std::vector<std::vector<int>> F_he(n_f);
    F_he[0] = range(0, F_he_cumsum[0]);
    for (int i = 1; i < n_f; ++i)
    {
        F_he[i] = range(F_he_cumsum[i-1], F_he_cumsum[i]);
    }
    // Create the per face next halfedge map
    std::vector<std::vector<int>> F_n(n_f);
    for (int i = 0; i < n_f; ++i)
    {
        F_n[i] = std::vector<int>(F_he[i].size());
        for (int j = 0; j < F_he[i].size()-1; ++j)
        {
            F_n[i][j] = F_he[i][j+1];
        }
        F_n[i][F_he[i].size()-1] = F_he[i][0];
    }

    // Create the next halfedge map (without boundary-loop halfedges)
    next_he.clear();
    next_he.reserve(n_he);
    flatten<int>(F_n, next_he);

    // Get the indices of tail vertices of halfedges
    std::vector<int> tail;
    tail.reserve(n_he);
    flatten<int>(F, tail);

    // Get the per face indices of head vertices of halfedges
    std::vector<std::vector<int>> F_head(n_f);
    for (int i = 0; i < n_f; ++i)
    {
        F_head[i] = std::vector<int>(F[i].size());
        for (int j = 0; j < F[i].size()-1; ++j)
        {
            F_head[i][j] = F[i][j+1];
        }
        F_head[i][F[i].size()-1] = F[i][0];
    }

    // Get the indices of head vertices of halfedges
    std::vector<int> head;
    tail.reserve(n_he);
    flatten<int>(F_head, head);

    // Shift indices by 1 to distinguish from 0 entries in the matrix
    std::vector<int> he_index = range(1, n_he + 1);

    // Get the number of vertices
    int n_v = 0;
    for (int i = 0; i < tail.size(); ++i)
    {
        n_v = std::max<int>(n_v, tail[i] + 1);
    }

    // Create the adjacency matrix (tail, head) -> halfedge index+1
    Eigen::SparseMatrix<int> vv2he(n_v, n_v);
    typedef Eigen::Triplet<int> Trip;
    std::vector<Trip> trips(n_he);
    for (int he = 0; he < n_he; ++he)
    {
        trips[he] = Trip(tail[he], head[he], he_index[he]);
    }
    vv2he.setFromTriplets(trips.begin(), trips.end());

    // Create opp array
    opp.clear();
    opp.resize(n_he);
    for (int he = 0; he < n_he; ++he)
    {
        opp[he] = vv2he.coeffRef(head[he], tail[he]) - 1;
    }

    // Add boundary loop halfedges
    std::vector<int> next_he_ext;
    std::vector<int> opp_ext;
    build_boundary_loops(next_he, opp, next_he_ext, opp_ext);
    next_he = next_he_ext;
    opp = opp_ext;

    // Create previous halfedge array
    std::vector<int> prev_he(next_he.size(), -1);
    for (int he = 0; he < next_he.size(); ++he)
    {
        prev_he[next_he[he]] = he;
    }

    // Create circulator function array
    std::vector<int> circ(next_he.size());
    for (int he = 0; he < next_he.size(); ++he)
    {
        circ[he] = prev_he[opp[he]];
    }

    // Create vertex array from the circulator array
    std::vector<std::vector<int>> vert;
    build_orbits(circ, vert);

    // Map orbit indices to indices of input vertices
    vtx_reindex.clear();
    vtx_reindex.resize(n_v);
    for (int i = 0; i < n_v; ++i)
    {
        vtx_reindex[i] = head[vert[i][0]];
    }

    // Create boundary loops
    bnd_face_list(next_he, n_he, bnd_loops);
}

void FV_to_NOB(const Eigen::MatrixXi &F,
               std::vector<int> &next_he,
               std::vector<int> &opp,
               std::vector<int> &bnd_loops,
               std::vector<int> &vtx_reindex)
{
    // Convert eigen matrix to vector of vectors
    std::vector<std::vector<int>> F_vec(F.rows(),std::vector<int>(F.cols()));
    for (int i = 0; i < F.rows(); ++i)
    {
        for (int j = 0; j < F.cols(); ++j)
        {
            F_vec[i][j] = F(i,j);
        }
    }

    // Run FV_to_NOB method
    FV_to_NOB(F_vec, next_he, opp, bnd_loops, vtx_reindex);
}

void build_boundary_loops(const std::vector<int> &next_he,
                          const std::vector<int> &opp,
                          std::vector<int> &next_he_ext,
                          std::vector<int> &opp_ext)
{
    int n_he = opp.size();

    // Get halfedges on the boundary
    std::vector<int> bnd_he;
    for (int he = 0; he < n_he; ++he)
    {
        if (opp[he] == -1)
        {
            bnd_he.push_back(he);
        }
    }
    int n_bnd_he = bnd_he.size();

    // Add boundary halfedges to the extended opp array
    opp_ext = opp;
    std::vector<int> opp_ext_tail(n_bnd_he, 0);
    opp_ext.insert(opp_ext.end(), opp_ext_tail.begin(), opp_ext_tail.end());
    for (int i = 0; i < n_bnd_he; ++i)
    {
        opp_ext[bnd_he[i]] = n_he + i;
        opp_ext[n_he + i] = bnd_he[i];
    }

    // Add boundary halfedges to the extended next halfedge array
    next_he_ext = next_he;
    std::vector<int> next_he_ext_tail(n_bnd_he, -1);
    next_he_ext.insert(next_he_ext.end(), next_he_ext_tail.begin(), next_he_ext_tail.end());
    for (int i = 0; i < n_bnd_he; ++i)
    {
        int he = bnd_he[i];
        int he_it = next_he[he];
        while (opp[he_it] != -1)
        {
            he_it = next_he[opp[he_it]];
        }
        next_he_ext[opp_ext[he_it]] = opp_ext[he];
    }
}

void build_orbits(const std::vector<int> &perm,
                  std::vector<std::vector<int>> &cycles)
{
    int n_perm = perm.size();

    // Get the maximum value in perm
    int max_perm = 0;
    for (int i = 0; i < n_perm; ++i)
    {
        max_perm = std::max(perm[i], max_perm);
    }
    std::vector<bool> visited(max_perm+1, false);

    // Build the cycles of the permutation
    cycles.clear();
    for (int i = 0; i < max_perm + 1; ++i)
    {
        if (!visited[i])
        {
            cycles.push_back(std::vector<int>());
            int i_it = i;
            while (true)
            {
                visited[i_it] = true;
                cycles.back().push_back(i_it);
                i_it = perm[i_it];
                if (i_it == i)
                {
                    break;
                }
            }
        }
    }
}

void bnd_face_list(const std::vector<int> &next_he,
                   const int n_he,
                   std::vector<int> &bnd_loops)
{
    std::vector<std::vector<int>> faces;
    build_orbits(next_he, faces);
    int n_f = faces.size();
    bnd_loops.clear();
    for (int i = 0; i < n_f; ++i)
    {
        if (faces[i][0] >= n_he)
        {
           bnd_loops.push_back(i);
        }
    }
}

std::vector<int> range(int start, int end)
{
    std::vector<int> arr(end-start);
    for (int i = 0; i < end - start; ++i)
    {
        arr[i] = start + i;
    }

    return arr;
}

void NOB_to_connectivity(const std::vector<int> &next_he,
                         const std::vector<int> &opp,
                         const std::vector<int> &bnd_loops,
                         Connectivity &C,
                         const std::vector<int> &diag_he)
{
    // Build previous halfedge array
    int n_he = next_he.size();
    std::vector<int> prev_he = std::vector<int>(n_he, -1);
    for (int he = 0; he < n_he; ++he)
    {
        prev_he[next_he[he]] = he;
    }

    // Construct face loops
    std::vector<std::vector<int>> faces;
    build_orbits(next_he, faces);

    // Create a boolean array marking the diagonal halfedge indices
    std::vector<bool> reserved_he(n_he, false);
    for (int i = 0; i < diag_he.size(); ++i)
    {
        reserved_he[diag_he[i]] = true;
    }

    // Map faces to attached halfedges
    int n_f = faces.size();
    std::vector<int> f2he(n_f);
    for (int i = 0; i < n_f; ++i)
    {
        f2he[i] = faces[i][0];
    }

    // Map halfedges to faces
    std::vector<int> he2f_perm;
    he2f_perm.reserve(n_he);
    for (int i = 0; i < n_f; ++i)
    {
        for (int j = 0; j < faces[i].size(); ++j)
        {
            he2f_perm.push_back(i);
        }
    }
    std::vector<int> Fhe;
    Fhe.reserve(n_he);
    flatten<int>(faces, Fhe);
    std::vector<int> he2f(n_he);
    for (int i = 0; i < n_he; ++i)
    {
        he2f[Fhe[i]] = he2f_perm[i];
    }

    // Create vertex list
    std::vector<int> circ(next_he.size());
    for (int he = 0; he < next_he.size(); ++he)
    {
        circ[he] = prev_he[opp[he]];
    }
    std::vector<std::vector<int>> vert_all;
    build_orbits(circ, vert_all);
    std::vector<std::vector<int>> vert;
    vert.reserve(vert_all.size());
    for (int i = 0; i < vert_all.size(); ++i)
    {
        // Skip vertices originating from diagonal halfedges
        if (reserved_he[vert_all[i][0]])
        {
            continue;
        }
        vert.push_back(vert_all[i]);
    }

    // Create out array
    int n_v = vert.size();
    std::vector<int> out(n_v);
    for (int i = 0; i < n_v; ++i)
    {
        out[i] = next_he[vert[i][0]];
    }

    // Create to array
    std::vector<int> to(n_he);
    std::vector<int> vind;
    vind.reserve(n_he);
    for (int i = 0; i < n_v; ++i)
    {
        for (int j = 0; j < vert[i].size(); ++j)
        {
            vind.push_back(i);
        }
    }
    std::vector<int> vhe;
    vhe.reserve(n_he);
    flatten(vert, vhe);
    for (int i = 0; i < n_he; ++i)
    {
        to[vhe[i]] = vind[i];
    }

    // Copy arrays to connectivity struct
    C.n = next_he;
    C.prev = prev_he;
    C.to = to;
    C.f = he2f;
    C.h = f2he;
    C.out = out;
    C.opp = opp;
}

void NOB_to_double(const std::vector<int> &next_he_in,
                   const std::vector<int> &opp_in,
                   const std::vector<int> &bnd_loops_in,
                   Connectivity &C,
                   std::vector<char> &type,
                   std::vector<int> &R)
{
    int n_f = bnd_loops_in[0];

    // Build faces of the input mesh
    std::vector<std::vector<int>> faces_in;
    build_orbits(next_he_in, faces_in);

    // Construct list of halfedges of boundary faces
    std::vector<int> bnd_loop_he;
    for (int i = 0; i < bnd_loops_in.size(); ++ i)
    {
        int l = bnd_loops_in[i];
        bnd_loop_he.insert(bnd_loop_he.end(), faces_in[l].begin(), faces_in[l].end());
    }

    // Get the number of halfedges belonging to faces, which is the first boundary loop halfedge
    int n_he_nb = bnd_loop_he[0];
    for (int i = 0; i < bnd_loop_he.size(); ++i)
    {
        n_he_nb = std::min(bnd_loop_he[i], n_he_nb);
    }

    // Create truncated arrays excluding boundary loop halfedges
    std::vector<int> opp_nb(opp_in.begin(), opp_in.begin() + n_he_nb);
    std::vector<std::vector<int>> faces_nb(faces_in.begin(), faces_in.begin() + bnd_loops_in[0]);

    // The symmetry map simply reverses the order of halfedges
    R.clear();
    R.resize(2*n_he_nb);
    for (int i = 0; i < 2*n_he_nb; ++i)
    {
        R[i] = 2*n_he_nb - i - 1;
    }

    // The faces as a result of the above symmetry map definition also reverse order
    std::vector<std::vector<int>> faces_new(n_f, std::vector<int>());
    for (int i = 0; i < n_f; ++i)
    {
        int face_size = faces_nb[n_f-i-1].size();
        faces_new[i].resize(face_size);
        for (int j = 0; j < face_size; ++j)
        {
            faces_new[i][j] = R[faces_nb[n_f-i-1][face_size-j-1]];
        }
    }
    std::vector<std::vector<int>> faces_double(faces_nb);
    faces_double.insert(faces_double.end(), faces_new.begin(), faces_new.end());

    // Construct the next halfedge array for the doubled mesh
    std::vector<int> he_double;
    he_double.reserve(2*n_he_nb);
    flatten<int>(faces_double, he_double);
    std::vector<std::vector<int>> next_he_double_faces(2*n_f, std::vector<int>());
    for (int i = 0; i < 2*n_f; ++i)
    {
        next_he_double_faces[i].reserve(faces_double[i].size());
        next_he_double_faces[i].insert(next_he_double_faces[i].end(),
                                       faces_double[i].begin()+1,
                                       faces_double[i].end());
        next_he_double_faces[i].push_back(faces_double[i][0]);
    }
    std::vector<int> next_he_double;
    next_he_double.reserve(2*n_he_nb);
    flatten<int>(next_he_double_faces, next_he_double);
    permute<int>(next_he_double, he_double);

    // Rebuild the faces in the the canonical oder inferred from faces
    std::vector<std::vector<int>> ordered_faces_double;
    build_orbits(next_he_double, ordered_faces_double);

    // Rebuild the halfedges of the faces enumerated sequentially
    //std::vector<int> he_double_ordered;
    //he_double_ordered.reserve(2*n_he_nb);
    //flatten<int>(ordered_faces_double, he_double_ordered);

    // Construct opp for the doubled mesh
    std::vector<int> bnd_he;
    std::vector<int> inter_he;
    for (int i = 0; i < opp_nb.size(); ++i)
    {
        if (opp_nb[i] >= n_he_nb)
        {
            bnd_he.push_back(i);
        }
        else
        {
            inter_he.push_back(i);
        }
    }
    std::vector<int> bnd_he_new(bnd_he.size());
    for (int i = 0; i < bnd_he.size(); ++i)
    {
        bnd_he_new[i] = R[bnd_he[i]];
    }
    for (int i = 0; i < bnd_he.size(); ++i)
    {
        opp_nb[bnd_he[i]] = bnd_he_new[i];
    }
    std::vector<int> opp_new(n_he_nb);
    for (int i = 0; i < n_he_nb; ++i)
    {
        opp_new[i] = R[opp_nb[R[n_he_nb+i]]];
    }
    std::vector<int> opp_double(opp_nb);
    opp_double.insert(opp_double.end(), opp_new.begin(), opp_new.end());

    // There is no boundary in the doubled mesh
    std::vector<int> bnd_loops_double;

    // Create the connectivity arrays for the doubled mesh
    NOB_to_connectivity(next_he_double, opp_double, bnd_loops_double, C);

    // Label the halfedges from the original mesh with 1 and the new mesh with 2
    type.clear();
    type.resize(2*n_he_nb);
    for (int i = 0; i < n_he_nb; ++i)
    {
        type[i] = 1;
    }
    for (int i = n_he_nb; i < 2*n_he_nb; ++i)
    {
        type[i] = 2;
    }
    for (int i = 0; i < bnd_he.size(); ++i)
    {
        type[bnd_he[i]] = 1;
    }
    for (int i = 0; i < bnd_he_new.size(); ++i)
    {
        type[bnd_he_new[i]] = 2;
    }
}

void find_indep_vertices(const std::vector<int> &out,
                         const std::vector<int> &to,
                         const std::vector<char> &type,
                         const std::vector<int> &R,
                         std::vector<int> &indep_vtx,
                         std::vector<int> &dep_vtx,
                         std::vector<int> &v_rep)
{
    indep_vtx.clear();
    dep_vtx.clear();
    v_rep.clear();
    int n_v = out.size();
    int n_he = to.size();

    // Create a reflection map for the vertices
    std::vector<int> refl_v(n_v);
    for (int i = 0; i < n_v; ++i)
    {
        refl_v[i] = to[R[out[i]]];
    }

    // Label the vertices as type 1=D1, 2=D2, or 3=S (on the symmetry line)
    std::vector<char> vtype(n_v);
    for (int he = 0; he < n_he; ++he)
    {
        if ((type[he] == 1) || (type[he] == 2))
        {
            vtype[to[he]] = type[he];
        }
    }
    for (int i = 0; i < n_v; ++i)
    {
        if (refl_v[i] == i)
        {
            vtype[i] = 3;
        }
    }

    // Partition vertices of type 1 and 3 (arising from the original mesh) and vertices
    // of type 2 (created in the double)
    indep_vtx.reserve(n_v);
    dep_vtx.reserve(n_v);
    for (int i = 0; i < n_v; ++i)
    {
        if (vtype[i] == 2)
        {
            dep_vtx.push_back(i);
        }
        else
        {
            indep_vtx.push_back(i);
        }
    }

    // Map independent vertices to unique indices and dependent vertices to their reflection's
    // index
    v_rep.resize(n_v);
    for (int i = 0; i < indep_vtx.size(); ++i)
    {
        v_rep[indep_vtx[i]] = i;
    }
    for (int i = 0; i < dep_vtx.size(); ++i)
    {
        v_rep[dep_vtx[i]] = v_rep[refl_v[dep_vtx[i]]];
    }
}

void create_tufted_cover(const std::vector<char> &type,
                         const std::vector<int> &R,
                         const std::vector<int> &indep_vtx,
                         const std::vector<int> &dep_vtx,
                         const std::vector<int> &v_rep,
                         std::vector<int> &out,
                         std::vector<int> &to)
{
    int n_v = out.size();
    int n_he = to.size();

    // Modify the to and out arrays to identify dependent vertices with their reflection
    std::vector<int> out_tufted(indep_vtx.size());
    for (int i = 0; i < n_he; ++i)
    {
        to[i] = v_rep[to[i]];
    }
    for (int i = 0; i < indep_vtx.size(); ++i)
    {
        out_tufted[i] = out[indep_vtx[i]];
    }
    out = out_tufted;
}


#include "simulator.hpp"

#include <iostream>
#include <exception>
#include <cstdint>
#include <limits>
#include <Eigen>

Simulator::Simulator(double timeStep, uint64_t maxObjects, Integrator* integrator, DynamicsEngine* dynamicsEngine)
: timeStep(timeStep)
, maxObjects(maxObjects)
, integrator(integrator)
, dynamicsEngine(dynamicsEngine)
, m(1, maxObjects)
, r(1, maxObjects)
, pos(3, maxObjects)
, v(3, maxObjects)
, a(3, maxObjects)
, id2idx(maxObjects, RIGIDBODY_IDX_NULL)
, idx2id(maxObjects, RIGIDBODY_ID_NULL)
, availableUsedIDs{} {}

Simulator::~Simulator() {
    // Deallocate heap variables
    delete this->integrator;
    delete this->dynamicsEngine;
}

double Simulator::totalKineticEnergy() {
    // (1/2)*sum(m_i * |v_i|^2)
    return 0.5*active(m).cwiseProduct(active(v).colwise().squaredNorm()).sum();
}

double Simulator::totalPotentialEnergy() {
    // Sum of potential energies
    return this->dynamicsEngine->totalPotentialEnergy(active(pos), active(m));
}

double Simulator::totalEnergy() {
    return this->totalKineticEnergy() + this->totalPotentialEnergy();
}

double Simulator::totalMomentOfInertia() {
    // sum(m_i * |pos_i|^2)
    return active(m).cwiseProduct(active(pos).colwise().squaredNorm()).sum();
}

Eigen::Ref<Eigen::MatrixXd> Simulator::active(Eigen::Ref<Eigen::MatrixXd> mat) {
    // Return map to slice (1, this->nextIdx). Works because SoA is densely packed.
    if (this->nObjects() == 0) {
        std::cerr << "Error: Simulator method called on empty simulator." << std::endl;
        throw std::runtime_error("Error: Simulator method called on empty simulator.");
    }

    return mat(Eigen::all, Eigen::seq(0, this->nextIdx - 1));
}

Rigidbody Simulator::nObjects() {
    return this->nextIdx;
}

Rigidbody Simulator::addObject(double m, double r, const Eigen::Vector3d& p0, const Eigen::Vector3d& v0) {
    // Update SoA and nextIdx

    // Check for max number of objects reached
    if (this->nextIdx >= this->maxObjects) {
        std::cerr << "Error: Max number of objects reached." << std::endl;
        throw std::overflow_error("Error: Max number of objects reached.");
    }

    // Get new object's index in SoA
    RigidbodyIdx idx = this->nextIdx++;

    // If a used id is not available, increment this->nextID and use that
    Rigidbody id;
    if (this->availableUsedIDs.empty()) {
        id = this->nextID++;
    } else {
        id = this->availableUsedIDs.front();
        this->availableUsedIDs.pop();
    }

    // Update id-idx mappings
    this->id2idx[id] = idx;
    this->idx2id[idx] = id;

    // Update structure of arrays
    this->m(idx) = m;
    this->r(idx) = r;
    this->pos(Eigen::all, idx) = p0;
    this->v(Eigen::all,   idx) = v0;
    this->a(Eigen::all,   idx) = Eigen::Vector3d::Zero();

    return id;
}

void Simulator::delObject(Rigidbody id) {
    // Check object exists
    if (!this->rb_exists(id)) {
        std::cerr << "Error: Rigidbody id not valid." << std::endl;
        throw std::out_of_range("Error: Rigidbody id not valid.");
    }

    // Get deleted object's idx
    RigidbodyIdx idx = this->id2idx[id];

    // Get index and id of topmost object (object with greatest index)
    RigidbodyIdx topIdx = this->nextIdx - 1;
    Rigidbody    topID  = this->idx2id[topIdx];

    // Move data at topIdx to idx
    this->m(idx) = this->m(topIdx);
    this->r(idx) = this->r(topIdx);
    this->pos(Eigen::all, idx) = this->pos(Eigen::all, topIdx);
    this->v(Eigen::all,   idx) = this->v(Eigen::all,   topIdx);
    this->a(Eigen::all,   idx) = this->a(Eigen::all,   topIdx);

    // Assign: idx to topID, topID to idx, null to id, null to topIdx
    this->id2idx[topID] = idx;
    this->idx2id[idx] = topID;
    this->id2idx[id] = RIGIDBODY_IDX_NULL;
    this->idx2id[topIdx] = RIGIDBODY_ID_NULL;

    // Push id to used available IDs
    this->availableUsedIDs.push(id);

    // Decrement nextIdx
    this->nextIdx--;
}

bool Simulator::rb_exists(Rigidbody id) {
    if (id < 0 || id >= this->maxObjects || this->id2idx[id] == RIGIDBODY_IDX_NULL) {
        // ID is out of range or does not point to existing object
        return false;
    } else {
        return true;
    }
}

Eigen::Ref<const Eigen::Vector3d> Simulator::rb_pos(Rigidbody id) {
    return this->pos.col(this->id2idx[id]);
}

Eigen::Ref<const Eigen::Vector3d> Simulator::rb_v(Rigidbody id) {
    return this->v.col(this->id2idx[id]);
}

Eigen::Ref<const Eigen::Vector3d> Simulator::rb_a(Rigidbody id) {
    return this->a.col(this->id2idx[id]);
}

double Simulator::rb_m(Rigidbody id) {
    return this->m(this->id2idx[id]);
}

double Simulator::rb_r(Rigidbody id) {
    return this->r(this->id2idx[id]);
}

Eigen::Ref<const Eigen::Matrix3Xd> Simulator::activePos() {
    return this->active(this->pos);
}

Eigen::Ref<const Eigen::Matrix3Xd> Simulator::activeV() {
    return this->active(this->v);
}

Eigen::Ref<const Eigen::Matrix3Xd> Simulator::activeA() {
    return this->active(this->a);
}

Eigen::Ref<const Eigen::RowVectorXd> Simulator::activeM() {
    return this->active(this->m);
}

Eigen::Ref<const Eigen::RowVectorXd> Simulator::activeR() {
    return this->active(this->r);
}

void Simulator::updateAccelerations() {
    this->dynamicsEngine->updateAccelerations(
        this->active(this->a),
        this->active(this->pos),
        this->active(this->m)
    );
}

void Simulator::step() {
    this->updateAccelerations();
    this->integrator->integrate(
        this->timeStep,
        this->active(this->a),
        this->active(this->v),
        this->active(this->pos)
    );
}


/*
 Class header file
 */
#include "aq_base.h"

/*
 STL header files
 */
#include <fstream>
#include <iomanip>
/*
 deal.II header files
 */
#include <deal.II/base/numbers.h>
#include <deal.II/base/quadrature_lib.h>

#include "lsgc.h"

template <int dim>
AQBase<dim>::AQBase (const dealii::ParameterHandler &prm)
    : k_pi(dealii::numbers::PI),
      have_reflective_bc_(prm.get_bool("have reflective boundary")),
      transport_model_name_(prm.get("transport model")),
      n_azi_(prm.get_integer("angular quadrature order")),
      n_group_(prm.get_integer("number of groups")),
      aq_name_(prm.get("angular quadrature name")) {}

template <int dim>
std::unique_ptr<AQBase<dim>> AQBase<dim>::CreateAQ(
    const dealii::ParameterHandler &prm) {

  std::unique_ptr<AQBase<dim>> return_ptr;

  if (dim == 1) {
    return_ptr.reset(new AQBase<dim>(prm));
  } else {
    std::unordered_map<std::string, AQType>
        aq_name_map = {{"lsgc", AQType::kLSGC}};

    switch(aq_name_map[prm.get("angular quadrature name")]) {
      case AQType::kLSGC: {
        return_ptr.reset(new LSGC<dim>(prm));
        break;
      }
      default: {
        assert(false);
      }
    }
  }

  return std::move(return_ptr);
}

template <int dim>
void AQBase<dim>::MakeAQ () {
  ProduceAQ ();
  InitCompInd ();
  InitRefBCInd ();
}

template <int dim>
void AQBase<dim>::InitRefBCInd () {
  // Note: here we assume square domain and assume user either
  // uses deal.II generated mesh or mesh from gmsh with proper
  // boundary IDs setup: {0,1,2,3,4,5} for {xmin,xmax,ymin,ymax,zmin,zmax}
  if (have_reflective_bc_) {
    std::vector<dealii::Tensor<1, dim>> bnv;
    bnv.resize (2*dim);
    // All boundary normal vectors are assume to be parallel to axes
    // Then, only one component in each normal vector is nonzero
    bnv[0][0] = -1.0;
    bnv[1][0] = 1.0;
    if (dim>=2) {
      bnv[2][1] = -1.0;
      bnv[3][1] = 1.0;
      if (dim>=3) {
        bnv[4][2] = -1.0;
        bnv[5][2] = 1.0;
      }
    }
    for (int i=0; i<2*dim; ++i)
      for (int i_dir=0; i_dir<n_dir_; ++i_dir) {
        dealii::Tensor<1, dim> out_angle =
            (omega_i_[i_dir] - 2.0 * (bnv[i]*omega_i_[i_dir]) * bnv[i]);
        //(omega_i_[i_dir] *
        // (1.0 - 2.0 * (bnv[i] * omega_i_[i_dir])));
        for (int r_dir=0; r_dir<n_dir_; ++r_dir) {
          dealii::Tensor<1, dim> d_dir = out_angle;
          dealii::Tensor<1, dim> d_minus_dir = out_angle;
          d_dir -= omega_i_[r_dir];
          d_minus_dir += omega_i_[r_dir];
          // Use caution about even parity using this std::map
          if ((transport_model_name_=="ep" &&
               (d_dir.norm ()<1.0e-13 || d_minus_dir.norm ()<1.0e-13)) ||
              (transport_model_name_!="ep" && d_dir.norm ()<1.0e-13))
            reflective_direction_index_[std::make_pair (i, i_dir)] = r_dir;
        }
      }
  }
}

template <int dim>
void AQBase<dim>::ProduceAQ () {
  // Only provide 1D by default
  AssertThrow (dim==1,
               dealii::ExcMessage("Only 1D is implemented in AQBase<dim>"));
  n_dir_ = n_azi_ / (transport_model_name_=="ep" ? 2 : 1);
  total_angle_ = transport_model_name_=="ep" ? 4.0 : 2.0;
  dealii::QGauss<1> mu_quad (n_azi_);
  for (int i=0; i<n_dir_; ++i) {
    dealii::Tensor <1,dim> omega;
    omega[0] = mu_quad.point(i)[0]*2.0-1.0;
    omega_i_.push_back (omega);
    wi_.push_back (mu_quad.weight(i)*total_angle_);
  }
  n_total_ho_vars_ = n_dir_ * n_group_;
}

template <int dim>
void AQBase<dim>::InitCompInd () {
  // initialize the map from (group, direction) to component indices
  int ind = 0;
  for (int g=0; g<n_group_; ++g)
    for (int i_dir=0; i_dir<n_dir_; ++i_dir)
    {
      std::pair<int, int> key (g, i_dir);
      component_index_[key] = ind;
      inverse_component_index_[ind] = key;
      ind += 1;
    }
}

template <int dim>
void AQBase<dim>::PrintAQ () {
  std::ofstream quadr;
  quadr.open("aq.txt");
  PrintAQ(&quadr);
  quadr.close ();
}

template <int dim>
void AQBase<dim>::PrintAQ (std::ostream *output_stream) {
  *output_stream << "transport model: " << transport_model_name_
        << "; output_streamature name: " << ProduceAQName () << std::endl;
  *output_stream << "Dim = " << dim << ", SN order = " << n_azi_ << std::endl;
  *output_stream << "Weights | Omega_x | Omega_y | mu" << std::endl;
  for (size_t i=0; i<omega_i_.size(); ++i) {
    *output_stream << std::fixed << std::setprecision (15)
          << wi_[i] << "  " << omega_i_[i][0] << "  ";
    if (dim>1) {
      *output_stream << omega_i_[i][1] << "  ";
      double mu = dim>2 ? omega_i_[i][2] :
          std::sqrt(1.-(std::pow(omega_i_[i][0],2.)
                        +std::pow(omega_i_[i][1],2)));
      *output_stream << mu;
    }
    *output_stream << std::endl;
  }
}

template <int dim>
std::string AQBase<dim>::ProduceAQName () const {
  AssertThrow(aq_name_.size()>0,
              dealii::ExcMessage("aq name has to be assigned"));
  // ToDo: more quadrature name producers
  if (aq_name_ == "lsgc")
    return "Level Symmetric Gauss Chebyshev";

  return "None";
}

//public member functions to retrieve private and protected variables
template <int dim>
std::map<std::pair<int, int>, int> AQBase<dim>::GetCompInd () const {
  return component_index_;
}

template <int dim>
std::unordered_map<int, std::pair<int, int>> AQBase<dim>::GetInvCompInd () const {
  return inverse_component_index_;
}

template <int dim>
std::map<std::pair<int, int>, int> AQBase<dim>::GetRefDirInd () const {
  return reflective_direction_index_;
}

template <int dim>
int AQBase<dim>::GetSnOrder () const {
  return n_azi_;
}

template <int dim>
int AQBase<dim>::GetNDir () const {
  return n_dir_;
}

template <int dim>
int AQBase<dim>::GetNTotalHOVars () const {
  return n_total_ho_vars_;
}

template <int dim>
std::vector<double> AQBase<dim>::GetAQWeights () const {
  return wi_;
}

template <int dim>
std::vector<dealii::Tensor<1, dim>> AQBase<dim>::GetAQDirs () const {
  return omega_i_;
}

template class AQBase<1>;
template class AQBase<2>;
template class AQBase<3>;


// Three-state boolean logic library

// Copyright Douglas Gregor 2002-2004. 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)

// For more information, see http://www.boost.org
#ifndef BOOST_LOGIC_TRIBOOL_HPP
#define BOOST_LOGIC_TRIBOOL_HPP

#include <boost/logic/tribool_fwd.hpp>
#include <boost/config.hpp>
#include <boost/detail/workaround.hpp>

#ifdef BOOST_HAS_PRAGMA_ONCE
#  pragma once
#endif

namespace boost { namespace logic {

/// INTERNAL ONLY
namespace detail {
/**
 * INTERNAL ONLY
 *
 * \brief A type used only to uniquely identify the 'indeterminate'
 * function/keyword.
 */
struct indeterminate_t
{
#if BOOST_WORKAROUND(BOOST_BORLANDC, < 0x0600)
  char dummy_; // BCB would use 8 bytes by default
#endif
};

} // end namespace detail

/**
 * INTERNAL ONLY
 * The type of the 'indeterminate' keyword. This has the same type as the
 * function 'indeterminate' so that we can recognize when the keyword is
 * used.
 */
typedef bool (*indeterminate_keyword_t)(tribool, detail::indeterminate_t);

/**
 * \brief Keyword and test function for the indeterminate tribool value
 *
 * The \c indeterminate function has a dual role. It's first role is
 * as a unary function that tells whether the tribool value is in the
 * "indeterminate" state. It's second role is as a keyword
 * representing the indeterminate (just like "true" and "false"
 * represent the true and false states). If you do not like the name
 * "indeterminate", and would prefer to use a different name, see the
 * macro \c BOOST_TRIBOOL_THIRD_STATE.
 *
 * \returns <tt>x.value == tribool::indeterminate_value</tt>
 * \throws nothrow
 */
BOOST_CONSTEXPR inline bool
indeterminate(tribool x,
              detail::indeterminate_t dummy = detail::indeterminate_t()) BOOST_NOEXCEPT;

/**
 * \brief A 3-state boolean type.
 *
 * 3-state boolean values are either true, false, or
 * indeterminate.
 */
class tribool
{
#if defined( BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS )
private:
  /// INTERNAL ONLY
  struct dummy {
    void nonnull() {};
  };

  typedef void (dummy::*safe_bool)();
#endif

public:
  /**
   * Construct a new 3-state boolean value with the value 'false'.
   *
   * \throws nothrow
   */
  BOOST_CONSTEXPR tribool() BOOST_NOEXCEPT : value(false_value) {}

  /**
   * Construct a new 3-state boolean value with the given boolean
   * value, which may be \c true or \c false.
   *
   * \throws nothrow
   */
  BOOST_CONSTEXPR tribool(bool initial_value) BOOST_NOEXCEPT : value(initial_value? true_value : false_value) {}

  /**
   * Construct a new 3-state boolean value with an indeterminate value.
   *
   * \throws nothrow
   */
  BOOST_CONSTEXPR tribool(indeterminate_keyword_t) BOOST_NOEXCEPT : value(indeterminate_value) {}

  /**
   * Use a 3-state boolean in a boolean context. Will evaluate true in a
   * boolean context only when the 3-state boolean is definitely true.
   *
   * \returns true if the 3-state boolean is true, false otherwise
   * \throws nothrow
   */
#if !defined( BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS )

  BOOST_CONSTEXPR explicit operator bool () const BOOST_NOEXCEPT
  {
    return value == true_value;
  }

#else

  BOOST_CONSTEXPR operator safe_bool() const BOOST_NOEXCEPT
  {
    return value == true_value? &dummy::nonnull : 0;
  }

#endif

  /**
   * The actual stored value in this 3-state boolean, which may be false, true,
   * or indeterminate.
   */
  enum value_t { false_value, true_value, indeterminate_value } value;
};

// Check if the given tribool has an indeterminate value. Also doubles as a
// keyword for the 'indeterminate' value
BOOST_CONSTEXPR inline bool indeterminate(tribool x, detail::indeterminate_t) BOOST_NOEXCEPT
{
  return x.value == tribool::indeterminate_value;
}

/** @defgroup logical Logical operations
 */
//@{
/**
 * \brief Computes the logical negation of a tribool
 *
 * \returns the logical negation of the tribool, according to the
 * table:
 *

 *

 *      !
 *
 *

 *

 *      false
 *      true
 *

 *

 *      true
 *      false
 *

 *

 *      indeterminate
 *      indeterminate
 *

 *

 * \throws nothrow
 */
BOOST_CONSTEXPR inline tribool operator!(tribool x) BOOST_NOEXCEPT
{
  return x.value == tribool::false_value? tribool(true)
        :x.value == tribool::true_value? tribool(false)
        :tribool(indeterminate);
}

/**
 * \brief Computes the logical conjunction of two tribools
 *
 * \returns the result of logically ANDing the two tribool values,
 * according to the following table:
 *

 *

 *             &&
 *             false
 *             true
 *             indeterminate
 *

 *

 *             false
 *             false
 *             false
 *             false
 *

 *

 *             true
 *             false
 *             true
 *             indeterminate
 *

 *

 *             indeterminate
 *             false
 *             indeterminate
 *             indeterminate
 *

 *

 * \throws nothrow
 */
BOOST_CONSTEXPR inline tribool operator&&(tribool x, tribool y) BOOST_NOEXCEPT
{
  return (static_cast<bool>(!x) || static_cast<bool>(!y))
    ? tribool(false)
    : ((static_cast<bool>(x) && static_cast<bool>(y)) ? tribool(true) : indeterminate)
  ;
}

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator&&(tribool x, bool y) BOOST_NOEXCEPT
{ return y? x : tribool(false); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator&&(bool x, tribool y) BOOST_NOEXCEPT
{ return x? y : tribool(false); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator&&(indeterminate_keyword_t, tribool x) BOOST_NOEXCEPT
{ return !x? tribool(false) : tribool(indeterminate); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator&&(tribool x, indeterminate_keyword_t) BOOST_NOEXCEPT
{ return !x? tribool(false) : tribool(indeterminate); }

/**
 * \brief Computes the logical disjunction of two tribools
 *
 * \returns the result of logically ORing the two tribool values,
 * according to the following table:
 *

 *

 *             ||
 *             false
 *             true
 *             indeterminate
 *

 *

 *             false
 *             false
 *             true
 *             indeterminate
 *

 *

 *             true
 *             true
 *             true
 *             true
 *

 *

 *             indeterminate
 *             indeterminate
 *             true
 *             indeterminate
 *

 *

 *  \throws nothrow
 */
BOOST_CONSTEXPR inline tribool operator||(tribool x, tribool y) BOOST_NOEXCEPT
{
  return (static_cast<bool>(!x) && static_cast<bool>(!y))
    ? tribool(false)
    : ((static_cast<bool>(x) || static_cast<bool>(y)) ? tribool(true) : tribool(indeterminate))
  ;
}

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator||(tribool x, bool y) BOOST_NOEXCEPT
{ return y? tribool(true) : x; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator||(bool x, tribool y) BOOST_NOEXCEPT
{ return x? tribool(true) : y; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator||(indeterminate_keyword_t, tribool x) BOOST_NOEXCEPT
{ return x? tribool(true) : tribool(indeterminate); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator||(tribool x, indeterminate_keyword_t) BOOST_NOEXCEPT
{ return x? tribool(true) : tribool(indeterminate); }
//@}

/**
 * \brief Compare tribools for equality
 *
 * \returns the result of comparing two tribool values, according to
 * the following table:
 *

 *

 *            ==
 *            false
 *            true
 *            indeterminate
 *

 *

 *            false
 *            true
 *            false
 *            indeterminate
 *

 *

 *            true
 *            false
 *            true
 *            indeterminate
 *

 *

 *            indeterminate
 *            indeterminate
 *            indeterminate
 *            indeterminate
 *

 *

 * \throws nothrow
 */
BOOST_CONSTEXPR inline tribool operator==(tribool x, tribool y) BOOST_NOEXCEPT
{
  return (indeterminate(x) || indeterminate(y))
    ? indeterminate
    : ((x && y) || (!x && !y))
  ;
}

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator==(tribool x, bool y) BOOST_NOEXCEPT { return x == tribool(y); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator==(bool x, tribool y) BOOST_NOEXCEPT { return tribool(x) == y; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator==(indeterminate_keyword_t, tribool x) BOOST_NOEXCEPT
{ return tribool(indeterminate) == x; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator==(tribool x, indeterminate_keyword_t) BOOST_NOEXCEPT
{ return tribool(indeterminate) == x; }

/**
 * \brief Compare tribools for inequality
 *
 * \returns the result of comparing two tribool values for inequality,
 * according to the following table:
 *

 *

 *             !=
 *             false
 *             true
 *             indeterminate
 *

 *

 *             false
 *             false
 *             true
 *             indeterminate
 *

 *

 *             true
 *             true
 *             false
 *             indeterminate
 *

 *

 *             indeterminate
 *             indeterminate
 *             indeterminate
 *             indeterminate
 *

 *

 * \throws nothrow
 */
BOOST_CONSTEXPR inline tribool operator!=(tribool x, tribool y) BOOST_NOEXCEPT
{
  return (indeterminate(x) || indeterminate(y))
    ? indeterminate
    : !((x && y) || (!x && !y))
  ;
}

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator!=(tribool x, bool y) BOOST_NOEXCEPT { return x != tribool(y); }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator!=(bool x, tribool y) BOOST_NOEXCEPT { return tribool(x) != y; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator!=(indeterminate_keyword_t, tribool x) BOOST_NOEXCEPT
{ return tribool(indeterminate) != x; }

/**
 * \overload
 */
BOOST_CONSTEXPR inline tribool operator!=(tribool x, indeterminate_keyword_t) BOOST_NOEXCEPT
{ return x != tribool(indeterminate); }

} } // end namespace boost::logic

// Pull tribool and indeterminate into namespace "boost"
namespace boost {
  using logic::tribool;
  using logic::indeterminate;
}

/**
 * \brief Declare a new name for the third state of a tribool
 *
 * Use this macro to declare a new name for the third state of a
 * tribool. This state can have any number of new names (in addition
 * to \c indeterminate), all of which will be equivalent. The new name will be
 * placed in the namespace in which the macro is expanded.
 *
 * Example:
 *   BOOST_TRIBOOL_THIRD_STATE(true_or_false)
 *
 *   tribool x(true_or_false);
 *   // potentially set x
 *   if (true_or_false(x)) {
 *     // don't know what x is
 *   }
 */
#define BOOST_TRIBOOL_THIRD_STATE(Name)                                 \
inline bool                                                             \
Name(boost::logic::tribool x,                                           \
     boost::logic::detail::indeterminate_t =                            \
       boost::logic::detail::indeterminate_t())                         \
{ return x.value == boost::logic::tribool::indeterminate_value; }

#endif // BOOST_LOGIC_TRIBOOL_HPP


// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_EXAMPLES_LINEARELLIPTIC_MRS2016__5_1_HH
#define DUNE_HDD_EXAMPLES_LINEARELLIPTIC_MRS2016__5_1_HH

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/parallel/mpihelper.hh>

#if HAVE_ALUGRID
# include <dune/grid/alugrid.hh>
#endif

#if HAVE_DUNE_FEM
# include <dune/fem/misc/mpimanager.hh>
#endif

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/timedlogging.hh>
#include <dune/stuff/common/memory.hh>
#include <dune/stuff/common/ranges.hh>
#include <dune/stuff/grid/boundaryinfo.hh>
#include <dune/stuff/grid/provider/cube.hh>

#include <dune/hdd/linearelliptic/problems/thermalblock.hh>
#include <dune/hdd/linearelliptic/discretizations/cg.hh>

namespace internal {

template< class GridType >
class Initializer
{
  typedef Dune::Stuff::Grid::Providers::Cube< GridType > GridProviderType;

public:
  Initializer(const std::string& num_grid_elements,
              const DUNE_STUFF_SSIZE_T info_log_levels,
              const DUNE_STUFF_SSIZE_T debug_log_levels,
              const bool enable_warnings,
              const bool enable_colors,
              const std::string info_color,
              const std::string debug_color,
              const std::string warn_color)
  {
    try {
      int argc = 0;
      char** argv = new char* [0];
#if HAVE_DUNE_FEM
      Dune::Fem::MPIManager::initialize(argc, argv);
#else
      Dune::MPIHelper::instance(argc, argv);
#endif
    } catch (...) {}
    try {
      DSC::TimedLogger().create(info_log_levels,
                                debug_log_levels,
                                enable_warnings,
                                enable_colors,
                                info_color,
                                debug_color,
                                warn_color);
    } catch (Dune::Stuff::Exceptions::you_are_using_this_wrong&) {}
    auto logger = DSC::TimedLogger().get("cg.thermalblock.example");
    logger.info() << "creating grid... " << std::flush;
    grid_provider_ = GridProviderType::create(GridProviderType::default_config().add(DSC::Configuration("num_elements",
                                                                                num_grid_elements),
                                                                                "",
                                                                                true));
#if HAVE_ALUGRID
    if (std::is_same< GridType, Dune::ALUGrid< 2, 2, Dune::simplex, Dune::conforming > >::value)
      grid_provider_->grid().globalRefine(1);
#endif // HAVE_ALUGRID
    logger.info() << "done (has " << grid_provider_->grid().size(0) << " elements)" << std::endl;
  } // Initializer(...)

protected:
  std::unique_ptr< GridProviderType > grid_provider_;
}; // class Initializer

} // namespace internal

template< class GridImp, Dune::GDT::ChooseSpaceBackend space_backend, Dune::Stuff::LA::ChooseBackend la_backend >
class CgExample
  : internal::Initializer< GridImp >
{
  typedef internal::Initializer< GridImp >               BaseType;
  typedef GridImp                                        GridType;
  typedef typename GridType::template Codim< 0 >::Entity EntityType;
  static const size_t                                    dimDomain = GridType::dimension;
  typedef typename GridType::ctype                       DomainFieldType;
  typedef double                                         RangeFieldType;
  static const size_t                                    dimRange = 1;
  typedef Dune::HDD::LinearElliptic::Problems::Thermalblock
      < EntityType, DomainFieldType, dimDomain, RangeFieldType, 1 > ProblemType;
public:
  typedef Dune::HDD::LinearElliptic::Discretizations::CG< GridType, Dune::Stuff::Grid::ChooseLayer::leaf,
                                                          RangeFieldType, dimRange, 1,
                                                          space_backend, la_backend > DiscretizationType;

  CgExample(const std::string& num_blocks        = "[2 2 2]",
            const std::string& num_grid_elements = "[32 32 32]",
            const DUNE_STUFF_SSIZE_T info_log_levels  = 0,
            const DUNE_STUFF_SSIZE_T debug_log_levels = -1,
            const bool enable_warnings = true,
            const bool enable_colors   = true,
            const std::string info_color  = DSC::TimedLogging::default_info_color(),
            const std::string debug_color = DSC::TimedLogging::default_debug_color(),
            const std::string warn_color  = DSC::TimedLogging::default_warning_color())
    : BaseType(num_grid_elements,
               info_log_levels,
               debug_log_levels,
               enable_warnings,
               enable_colors,
               info_color,
               debug_color,
               warn_color)
    , problem_(ProblemType::create(ProblemType::default_config().add(DSC::Configuration("diffusion_factor.num_elements",
                                                                                num_blocks),
                                                                     "",
                                                                     true)))
    , discretization_(*grid_provider_, Dune::Stuff::Grid::BoundaryInfoConfigs::AllDirichlet::default_config(), *problem_)
  {
    auto logger = DSC::TimedLogger().get("cg.thermalblock.example");
    logger.info() << "initializing discretization... " << std::flush;
    discretization_.init();
    logger.info() << "done (has " << discretization_.ansatz_space().mapper().size() << " DoFs)" << std::endl;
  } // ... CgExample(...)

  DiscretizationType& discretization()
  {
    return discretization_;
  }

private:
  using BaseType::grid_provider_;
  std::unique_ptr< ProblemType > problem_;
  DiscretizationType discretization_;
}; // class CgExample

/**
    Orthonormalize a |VectorArray| using the stabilized Gram-Schmidt algorithm.

    Parameters
    ----------
    A
        The |VectorArray| which is to be orthonormalized.
    product
        The scalar product w.r.t. which to orthonormalize, given as a linear
        |Operator|. If `None` the Euclidean product is used.
    atol
        Vectors of norm smaller than `atol` are removed from the array.
    rtol
        Relative tolerance used to detect linear dependent vectors
        (which are then removed from the array).
    offset
        Assume that the first `offset` vectors are already orthogonal and start the
        algorithm at the `offset + 1`-th vector.
    find_duplicates
        If `True`, eliminate duplicate vectors before the main loop.
    reiterate
        If `True`, orthonormalize again if the norm of the orthogonalized vector is
        much smaller than the norm of the original vector.
    reiteration_threshold
        If `reiterate` is `True`, re-orthonormalize if the ratio between the norms of
        the orthogonalized vector and the original vector is smaller than this value.
    check
        If `True`, check if the resulting VectorArray is really orthonormal.
    check_tol
        Tolerance for the check.
    copy
        If `True`, create a copy of `A` instead of modifying `A` itself.

    Returns
    -------
    The orthonormalized |VectorArray|.
  **/
template< class VectorType >
std::vector< VectorType > gram_schmidt(const std::vector< VectorType >& A,
                                       const bool reiterate = true,
                                       const bool check = true,
                                       const double atol = 1e-13,
                                       const double rtol = 1e-13,
                                       const DUNE_STUFF_SSIZE_T offset = 0,
                                       const double reiteration_threshold = 1e-1,
                                       const double check_tol = 1e-3)
{
  if (offset < 0)
    DUNE_THROW(Dune::Stuff::Exceptions::index_out_of_range, offset);
  auto logger = DSC::TimedLogger().get("gram_schmidt");

  auto ret = A;

  // main loop
  std::set< size_t > remove;
  for (size_t i = offset; i < ret.size(); ++i) {
    // first calculate norm
    const double initial_norm = ret[i].l2_norm();

    if (initial_norm < atol) {
      logger.info() << "Removing vector " << i << " of norm " << initial_norm << std::endl;
      remove.insert(i);
    }

    if (i == 0)
      ret[0].scal(1./initial_norm);
    else {
      bool first_iteration = true;
      double norm = initial_norm;
      double old_norm = initial_norm;
      // If reiterate is true, reiterate as long as the norm of the vector changes
      // strongly during orthonormalization (due to Andreas Buhr).
      while (first_iteration || reiterate && norm/old_norm < reiteration_threshold) {

        if (first_iteration)
          first_iteration = false;
        else
          logger.info() << "Orthonormalizing vector " << i << " again" << std::endl;

        // orthogonalize to all vectors left
        for (size_t j = 0; j < i; ++j) {
          if (remove.find(j) != remove.end())
            continue;
          const double p = ret[i].dot(ret[j]);
          ret[i].axpy(-p, ret[j]);
        }

        // calculate new norm
        old_norm = norm;
        norm = ret[i].l2_norm();

        // remove vector if it got too small:
        if (norm/initial_norm < rtol) {
          logger.info() << "Removing linear dependent vector " << i << std::endl;
          remove.insert(i);
          break;
        }
      }
      ret[i].scal(1./norm);
    }
  }

  for (const size_t& i : remove)
    ret.erase(ret.begin() + i);

  if (check) {
    for (size_t i = offset; i < ret.size(); ++i)
      for (size_t j = i; j < ret.size(); ++j)
        if (std::abs(ret[i].dot(ret[j])) - (i == j ? 1. : 0.) > check_tol)
          DUNE_THROW(Dune::MathError,
                     "result not orthogonal: \n"
                     << "  A[i]*A[j] = " << ret[i].dot(ret[j]) << "\n"
                     << "  i = " << i << "\n"
                     << "  j = " << j);
  }
  return ret;
} // ... gram_schmidt(...)

template< class VectorType, class MatrixType >
std::vector< VectorType > gram_schmidt(const std::vector< VectorType >& A,
                                       const MatrixType& product,
                                       const bool reiterate = true,
                                       const bool check = true,
                                       const double atol = 1e-13,
                                       const double rtol = 1e-13,
                                       const DUNE_STUFF_SSIZE_T offset = 0,
                                       const double reiteration_threshold = 1e-1,
                                       const double check_tol = 1e-3)
{
  if (offset < 0)
    DUNE_THROW(Dune::Stuff::Exceptions::index_out_of_range, offset);
  auto logger = DSC::TimedLogger().get("gram_schmidt");

  auto ret = A;
  VectorType tmp(product.rows());

  // main loop
  std::set< size_t > remove;
  for (size_t i = offset; i < ret.size(); ++i) {
    // first calculate norm
    product.mv(ret[i], tmp);
    const double initial_norm = std::sqrt(tmp*ret[i]);

    if (initial_norm < atol) {
      logger.info() << "Removing vector " << i << " of norm " << initial_norm << std::endl;
      remove.insert(i);
    }

    if (i == 0)
      ret[0].scal(1./initial_norm);
    else {
      bool first_iteration = true;
      double norm = initial_norm;
      double old_norm = initial_norm;
      // If reiterate is true, reiterate as long as the norm of the vector changes
      // strongly during orthonormalization (due to Andreas Buhr).
      while (first_iteration || reiterate && norm/old_norm < reiteration_threshold) {

        if (first_iteration)
          first_iteration = false;
        else
          logger.info() << "Orthonormalizing vector " << i << " again" << std::endl;

        // orthogonalize to all vectors left
        for (size_t j = 0; j < i; ++j) {
          if (remove.find(j) != remove.end())
            continue;
          product.mv(ret[i], tmp);
          const double p = tmp*ret[j];
          ret[i].axpy(-p, ret[j]);
        }

        // calculate new norm
        old_norm = norm;
        product.mv(ret[i], tmp);
        norm = std::sqrt(tmp*ret[i]);

        // remove vector if it got too small:
        if (norm/initial_norm < rtol) {
          logger.info() << "Removing linear dependent vector " << i << std::endl;
          remove.insert(i);
          break;
        }
      }
      ret[i].scal(1./norm);
    }
  }

  for (const size_t& i : remove)
    ret.erase(ret.begin() + i);

  if (check) {
    for (size_t i = offset; i < ret.size(); ++i) {
      product.mv(ret[i], tmp);
      for (size_t j = i; j < ret.size(); ++j) {
        if (tmp*ret[j] - (i == j ? 1. : 0.) > check_tol)
          DUNE_THROW(Dune::MathError,
                     "result not orthogonal: \n"
                     << "  product.apply2(A[i], A[j]) = " << tmp*ret[j] << "\n"
                     << "  i = " << i << "\n"
                     << "  j = " << j);
      }
    }
  }
  return ret;
} // ... gram_schmidt(...)

#endif // DUNE_HDD_EXAMPLES_LINEARELLIPTIC_MRS2016__5_1_HH


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#ifndef   CPPQEDCORE_QUANTUMTRAJECTORY_MCWF_TRAJECTORY_TCC_INCLUDED
#define   CPPQEDCORE_QUANTUMTRAJECTORY_MCWF_TRAJECTORY_TCC_INCLUDED

#include "MCWF_Trajectory.h"

#include "StateVector.h"

#include "EvolvedGSL.tcc"
#include "QuantumTrajectory.h"

#include "FormDouble.h"

#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/find_if.hpp>

namespace quantumtrajectory {

//////////////////
//
// Implementations
//
//////////////////

template<int RANK, typename RandomEngine>
void MCWF_Trajectory<RANK,RandomEngine>::derivs(double t, const StateVectorLow& psi, StateVectorLow& dpsidt) const
{
  if (const auto ha=this->getHa()) {
    dpsidt=0;

    ha->addContribution(t,psi,dpsidt,this->getT0());
  }
}

template<int RANK, typename RandomEngine>
MCWF_Trajectory<RANK,RandomEngine>::MCWF_Trajectory(SV_Ptr psi,
                                                    typename structure::QuantumSystem<RANK>::Ptr sys,
                                                    const mcwf::Pars<RandomEngine>& p,
                                                    const StateVectorLow& scaleAbs)
  : QuantumTrajectory(sys,p.noise,
         psi->getArray(),
         bind(&MCWF_Trajectory::derivs,this,_1,_2,_3),
         initialTimeStep<RANK>(sys),
         scaleAbs,
         p,
         evolved::MakerGSL<StateVectorLow>(p.sf,p.nextDtTryCorrectionFactor)),
    psi_(psi),
    dpLimit_(p.dpLimit), overshootTolerance_(p.overshootTolerance),
    logger_(p.logLevel,this->template nAvr<structure::LA_Li>())
{
  QuantumTrajectory::checkDimension(psi);
  if (!getTime()) if(const auto li=this->getLi()) { // On startup, dpLimit should not be overshot, either.
    Rates rates(li->rates(0.,*psi_)); calculateSpecialRates(&rates,0.);
    manageTimeStep(rates,getEvolved().get(),false);
  }
}

template<int RANK, typename RandomEngine>
double MCWF_Trajectory<RANK,RandomEngine>::coherentTimeDevelopment(double Dt)
{
  if (this->getHa()) {
    getEvolved()->step(Dt);
  }
  else {
    double stepToDo=this->getLi() ? std::min(getDtTry(),Dt) : Dt; // Cf. tracker #3482771
    getEvolved()->update(getTime()+stepToDo,getDtTry());
  }
  // This defines three levels:
  // 1. System is Hamiltonian -> internal timestep control is used with dtTry possibly modified by Liouvillean needs (cf. manageTimeStep)
  // 2. System is not Hamiltonian, but it is Liouvillean -> dtTry is used, which is governed solely by Liouvillean in this case (cf. manageTimeStep)
  // 3. System is neither Hamiltonian nor Liouvillean (might be of Exact) -> as step of Dt is taken

  double t=getTime();

  if (const auto ex=this->getEx()) {
    ex->actWithU(getTime(),psi_->getArray(),this->getT0());
    QuantumTrajectory::setT0(t);
  }

  logger_.processNorm(psi_->renorm());

  return t;
}

template<int RANK, typename RandomEngine>
auto MCWF_Trajectory<RANK,RandomEngine>::calculateSpecialRates(Rates* rates, double t) const -> const IndexSVL_tuples
{
  IndexSVL_tuples res;
  for (int i=0; i<rates->size(); i++)
    if ((*rates)(i)<0) {
      StateVector psiTemp(*psi_);
      this->actWithJ(t,psiTemp.getArray(),i);
      res.push_back(IndexSVL_tuple(i,psiTemp.getArray()));
      (*rates)(i)=mathutils::sqr(psiTemp.renorm());
    } // psiTemp disappears here, but its storage does not, because the ownership is taken over by the StateVectorLow in the tuple
  return res;
}

template<int RANK, typename RandomEngine>
bool MCWF_Trajectory<RANK,RandomEngine>::manageTimeStep(const Rates& rates, evolved::TimeStepBookkeeper* evolvedCache, bool logControl)
{
  const double totalRate=boost::accumulate(rates,0.);
  const double dtDid=this->getDtDid(), dtTry=getDtTry();

  const double liouvilleanSuggestedDtTry=dpLimit_/totalRate;

  // Assumption: overshootTolerance_>=1 (equality is the limiting case of no tolerance)
  if (totalRate*dtDid>overshootTolerance_*dpLimit_) {
    evolvedCache->setDtTry(liouvilleanSuggestedDtTry);
    (*getEvolved())=*evolvedCache;
    logger_.stepBack(this->getLogStreamDuringRun(),totalRate*dtDid,dtDid,getDtTry(),getTime(),logControl);
    return true; // Step-back required.
  }
  else if ( totalRate*dtTry>dpLimit_) {
    logger_.overshot(this->getLogStreamDuringRun(),totalRate*dtTry,dtTry,liouvilleanSuggestedDtTry,logControl);
  }

  // dtTry-adjustment for next step:
  getEvolved()->setDtTry(this->getHa() ? std::min(getDtTry(),liouvilleanSuggestedDtTry) : liouvilleanSuggestedDtTry);

  return false; // Step-back not required.
}

template<int RANK, typename RandomEngine>
void MCWF_Trajectory<RANK,RandomEngine>::performJump(const Rates& rates, const IndexSVL_tuples& specialRates, double t)
{
  double random=std::uniform_real_distribution()(this->getRandomEngine())/this->getDtDid();

  int lindbladNo=0; // TODO: this could be expressed with an iterator into rates
  for (; random>0 && lindbladNo!=rates.size(); random-=rates(lindbladNo++))
    ;

  if(random<0) { // Jump corresponding to Lindblad no. lindbladNo-1 occurs
    --lindbladNo; auto i=boost::find_if(specialRates,[=](const IndexSVL_tuple& j){return lindbladNo==std::get<0>(j);});
    if (i!=specialRates.end())
      // special jump
      *psi_=std::get<1>(*i); // RHS already normalized above
    else {
      // normal  jump
      this->actWithJ(t,psi_->getArray(),lindbladNo);
      double normFactor=sqrt(rates(lindbladNo));
      if (!boost::math::isfinite(normFactor)) throw std::runtime_error("Infinite detected in MCWF_Trajectory::performJump");
      *psi_/=normFactor;
    }

    logger_.jumpOccured(this->getLogStreamDuringRun(),t,lindbladNo);
  }
}

template<int RANK, typename RandomEngine>
void MCWF_Trajectory<RANK,RandomEngine>::step_v(double Dt)
{
  const StateVector psiCache(*psi_);
  evolved::TimeStepBookkeeper evolvedCache(*getEvolved()); // This cannot be const since dtTry might change.

  double t=coherentTimeDevelopment(Dt);

  if (const auto li=this->getLi()) {

    Rates rates(li->rates(t,*psi_));
    IndexSVL_tuples specialRates=calculateSpecialRates(&rates,t);

    while (manageTimeStep(rates,&evolvedCache)) {
      *psi_=psiCache;
      t=coherentTimeDevelopment(Dt); // the next try
      rates=li->rates(t,*psi_);
      specialRates=calculateSpecialRates(&rates,t);
    }

    // Jump
    performJump(rates,specialRates,t);

  }

  logger_.step();

}

template<int RANK, typename RandomEngine>
std::ostream& MCWF_Trajectory<RANK,RandomEngine>::streamParameters_v(std::ostream& os) const
{
  using namespace std;

  this->streamCharacteristics(this->getQS()->streamParameters(Base::streamParameters_v(os)<<"MCWF Trajectory Parameters: dpLimit="<<dpLimit_<<" (overshoot tolerance factor)="<<overshootTolerance_<<endl<<endl))<<endl;

  if (const auto li=this->getLi()) {
    os<<"Decay channels:\n";
    {
      size_t i=0;
      li->streamKey(os,i);
    }
    os<<"Alternative Lindblads: ";
    {
      const Rates rates(li->rates(0,*psi_));
      int n=0;
      for (int i=0; i<rates.size(); ++i) if (rates(i)<0) {os<<i<<' '; ++n;}
      if (!n) os<<"none";
    }
    os<<endl;
  }

  return os;

}

template<int RANK, typename RandomEngine>
std::ostream& MCWF_Trajectory<RANK,RandomEngine>::streamKey_v(std::ostream& os, size_t& i) const
{
  return this->template streamKey<structure::LA_Av>(os,i);
}

} // quantumtrajectory

#endif // CPPQEDCORE_QUANTUMTRAJECTORY_MCWF_TRAJECTORY_TCC_INCLUDED