#pragma once

#include <boost/hana/contains.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>

#include <array>
#include <memory>

#include "base_encoded_column.hpp"
#include "storage/vector_compression/base_compressed_vector.hpp"
#include "types.hpp"

namespace opossum {

class BaseCompressedVector;

/**
 * @brief Column implementing frame-of-reference encoding
 *
 * Frame-of-Reference encoding divides the values of column into
 * fixed-size blocks. The values of each block are encoded
 * as an offset from the block’s minimum value. These offsets,
 * which can ideally be represented by fewer bits, are then
 * compressed using vector compression (null suppression).
 * FOR encoding on its own without vector compression does not
 * add any benefit.
 */
template <typename T, typename = std::enable_if_t<encoding_supports_data_type(
                          enum_c<EncodingType, EncodingType::FrameOfReference>, hana::type_c<T>)>>
class FrameOfReferenceColumn : public BaseEncodedColumn {
 public:
  /**
   * The column is divided into fixed-size blocks.
   * Each block has its own minimum from which the
   * offsets are calculated. Theoretically, it would
   * possible to make the block size dependent on the
   * data’s properties. Determining the optimal size
   * is however not trivial.
   */
  static constexpr auto block_size = 2048u;

  explicit FrameOfReferenceColumn(const pmr_vector<T> reference_frames, const pmr_vector<bool> null_values,
                                  std::unique_ptr<const BaseCompressedVector> offset_values);

  const pmr_vector<T>& block_minima() const;
  const pmr_vector<bool>& null_values() const;
  const BaseCompressedVector& offset_values() const;

  /**
   * @defgroup BaseColumn interface
   * @{
   */

  const AllTypeVariant operator[](const ChunkOffset chunk_offset) const final;

  size_t size() const final;

  std::shared_ptr<BaseColumn> copy_using_allocator(const PolymorphicAllocator<size_t>& alloc) const final;

  size_t estimate_memory_usage() const final;

  /**@}*/

  /**
   * @defgroup BaseEncodedColumn interface
   * @{
   */

  EncodingType encoding_type() const final;
  CompressedVectorType compressed_vector_type() const final;

  /**@}*/

 private:
  const pmr_vector<T> _block_minima;
  const pmr_vector<bool> _null_values;
  const std::unique_ptr<const BaseCompressedVector> _offset_values;
  std::unique_ptr<BaseVectorDecompressor> _decoder;
};

}  // namespace opossum


#include <libasd/header.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

BOOST_TEST_CASE_TEMPLATE_FUNCTION(to_voltage, T)
{
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::unipolar_1_0V);
        BOOST_CHECK_EQUAL(minmax.first,  0.0);
        BOOST_CHECK_EQUAL(minmax.second, 1.0);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::unipolar_2_5V);
        BOOST_CHECK_EQUAL(minmax.first,  0.0);
        BOOST_CHECK_EQUAL(minmax.second, 2.5);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::unipolar_5_0V);
        BOOST_CHECK_EQUAL(minmax.first,  0.0);
        BOOST_CHECK_EQUAL(minmax.second, 5.0);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::bipolar_1_0V);
        BOOST_CHECK_EQUAL(minmax.first, -1.0);
        BOOST_CHECK_EQUAL(minmax.second, 1.0);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::bipolar_2_5V);
        BOOST_CHECK_EQUAL(minmax.first, -2.5);
        BOOST_CHECK_EQUAL(minmax.second, 2.5);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::bipolar_5_0V);
        BOOST_CHECK_EQUAL(minmax.first, -5.0);
        BOOST_CHECK_EQUAL(minmax.second, 5.0);
    }
    {
        const auto minmax = asd::to_voltage<T>(asd::AD_range::dummy_value);
        BOOST_CHECK_EQUAL(minmax.first, -80.0);
        BOOST_CHECK_EQUAL(minmax.second, 80.0);
    }
}

boost::unit_test::test_suite*
init_unit_test_suite(int, char**)
{
    typedef boost::mpl::list<float, double> real_types;

    boost::unit_test::framework::master_test_suite()
        .add(BOOST_TEST_CASE_TEMPLATE(to_voltage, real_types));

    return 0;
}


#ifndef FVMSCALAR1D_CFL_CONDITION_HPP
#define FVMSCALAR1D_CFL_CONDITION_HPP

#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>

/// Interface for computing CFL restricted timesteps.
class CFLCondition {
  public:
    virtual ~CFLCondition() = default;

    /// Compute the largest time-step satisfying the CFL condition.
    virtual double operator()(const Eigen::VectorXd &u) const = 0;
};

std::shared_ptr<CFLCondition> make_cfl_condition(const Grid &grid, const Model &model, double cfl_number);

#endif // FVMSCALAR1D_CFL_CONDITION_HPP


/*
 * 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 "galois/Reduction.h"
#include "galois/Bag.h"
#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/graphs/LCGraph.h"
#include "llvm/Support/CommandLine.h"

#include "Lonestar/BoilerPlate.h"

#include <boost/iterator/iterator_adaptor.hpp>

#include <fstream>
#include <iostream>

namespace cll = llvm::cl;

const char* name = "Preflow Push";
const char* desc =
    "Finds the maximum flow in a network using the preflow push technique";
const char* url = "preflow_push";

enum DetAlgo { nondet = 0, detBase, detDisjoint };

static cll::opt<std::string> filename(cll::Positional,
                                      cll::desc("<input file>"), cll::Required);
static cll::opt<uint32_t> sourceId(cll::Positional, cll::desc("sourceID"),
                                   cll::Required);
static cll::opt<uint32_t> sinkId(cll::Positional, cll::desc("sinkID"),
                                 cll::Required);
static cll::opt<bool> useHLOrder("useHLOrder",
                                 cll::desc("Use HL ordering heuristic"),
                                 cll::init(false));
static cll::opt<bool>
    useUnitCapacity("useUnitCapacity",
                    cll::desc("Assume all capacities are unit"),
                    cll::init(false));
static cll::opt<bool> useSymmetricDirectly(
    "useSymmetricDirectly",
    cll::desc("Assume input graph is symmetric and has unit capacities"),
    cll::init(false));
static cll::opt<int>
    relabelInt("relabel",
               cll::desc("relabel interval X: relabel every X iterations "
                         "(default 0 uses default interval)"),
               cll::init(0));
static cll::opt<DetAlgo> detAlgo(
    cll::desc("Deterministic algorithm:"),
    cll::values(clEnumVal(nondet, "Non-deterministic (default)"),
                clEnumVal(detBase, "Base execution"),
                clEnumVal(detDisjoint, "Disjoint execution")),
    cll::init(nondet));

/**
 * Alpha parameter the original Goldberg algorithm to control when global
 * relabeling occurs. For comparison purposes, we keep them the same as
 * before, but it is possible to achieve much better performance by adjusting
 * the global relabel frequency.
 */
static const int ALPHA = 6;

/**
 * Beta parameter the original Goldberg algorithm to control when global
 * relabeling occurs. For comparison purposes, we keep them the same as
 * before, but it is possible to achieve much better performance by adjusting
 * the global relabel frequency.
 */
static const int BETA = 12;

struct Node {
  uint32_t id;
  int64_t excess;
  int height;
  int current;

  Node() : excess(0), height(1), current(0) {}
};

std::ostream& operator<<(std::ostream& os, const Node& n) {
  os << "("
     << "id: " << n.id << ", excess: " << n.excess << ", height: " << n.height
     << ", current: " << n.current << ")";
  return os;
}

using Graph =
    galois::graphs::LC_CSR_Graph<Node, int32_t>::with_numa_alloc<false>::type;
using GNode   = Graph::GraphNode;
using Counter = galois::GAccumulator<int>;

struct PreflowPush {

  Graph graph;
  GNode sink;
  GNode source;
  int global_relabel_interval;
  bool should_global_relabel = false;
  galois::LargeArray<Graph::edge_iterator>
      reverseDirectionEdgeIterator; // ideally should be on the graph as
                                    // graph.getReverseEdgeIterator()

  void reduceCapacity(const Graph::edge_iterator& ii, const GNode& src,
                      const GNode& dst, int64_t amount) {
    Graph::edge_data_type& cap1 = graph.getEdgeData(ii);
    Graph::edge_data_type& cap2 =
        graph.getEdgeData(reverseDirectionEdgeIterator[*ii]);
    cap1 -= amount;
    cap2 += amount;
  }

  Graph::edge_iterator findEdge(GNode src, GNode dst) {

    auto i     = graph.edge_begin(src, galois::MethodFlag::UNPROTECTED);
    auto end_i = graph.edge_end(src, galois::MethodFlag::UNPROTECTED);

    if ((end_i - i) < 32) {
      return findEdgeLinear(dst, i, end_i);

    } else {
      return findEdgeLog2(dst, i, end_i);
    }
  }

  Graph::edge_iterator findEdgeLinear(GNode dst, Graph::edge_iterator beg_e,
                                      Graph::edge_iterator end_e) {

    auto ii = beg_e;
    for (; ii != end_e; ++ii) {
      if (graph.getEdgeDst(ii) == dst)
        break;
    }
    assert(ii != end_e); // Never return the end iterator
    return ii;
  }

  Graph::edge_iterator findEdgeLog2(GNode dst, Graph::edge_iterator i,
                                    Graph::edge_iterator end_i) {

    struct EdgeDstIter
        : public boost::iterator_facade<
              EdgeDstIter, GNode, boost::random_access_traversal_tag, GNode> {
      Graph* g;
      Graph::edge_iterator ei;

      EdgeDstIter(void) : g(nullptr) {}

      EdgeDstIter(Graph* g, Graph::edge_iterator ei) : g(g), ei(ei) {}

    private:
      friend boost::iterator_core_access;

      GNode dereference(void) const { return g->getEdgeDst(ei); }

      void increment(void) { ++ei; }

      void decrement(void) { --ei; }

      bool equal(const EdgeDstIter& that) const {
        assert(this->g == that.g);
        return this->ei == that.ei;
      }

      void advance(ptrdiff_t n) { ei += n; }

      ptrdiff_t distance_to(const EdgeDstIter& that) const {
        assert(this->g == that.g);

        return that.ei - this->ei;
      }
    };

    EdgeDstIter ai(&graph, i);
    EdgeDstIter end_ai(&graph, end_i);

    auto ret = std::lower_bound(ai, end_ai, dst);

    assert(ret != end_ai);
    assert(*ret == dst);

    return ret.ei;
  }

  void acquire(const GNode& src) {
    // LC Graphs have a different idea of locking
    for (auto ii : graph.edges(src, galois::MethodFlag::WRITE)) {
      GNode dst = graph.getEdgeDst(ii);
      graph.getData(dst, galois::MethodFlag::WRITE);
    }
  }

  void relabel(const GNode& src) {
    int minHeight = std::numeric_limits<int>::max();
    int minEdge   = 0;

    int current = 0;
    for (auto ii : graph.edges(src, galois::MethodFlag::UNPROTECTED)) {
      GNode dst   = graph.getEdgeDst(ii);
      int64_t cap = graph.getEdgeData(ii);
      if (cap > 0) {
        const Node& dnode = graph.getData(dst, galois::MethodFlag::UNPROTECTED);
        if (dnode.height < minHeight) {
          minHeight = dnode.height;
          minEdge   = current;
        }
      }
      ++current;
    }

    assert(minHeight != std::numeric_limits<int>::max());
    ++minHeight;

    Node& node = graph.getData(src, galois::MethodFlag::UNPROTECTED);
    if (minHeight < (int)graph.size()) {
      node.height  = minHeight;
      node.current = minEdge;
    } else {
      node.height = graph.size();
    }
  }

  template <typename C>
  bool discharge(const GNode& src, C& ctx) {
    // Node& node = graph.getData(src, galois::MethodFlag::WRITE);
    Node& node = graph.getData(src, galois::MethodFlag::UNPROTECTED);
    // int prevHeight = node.height;
    bool relabeled = false;

    if (node.excess == 0 || node.height >= (int)graph.size()) {
      return false;
    }

    while (true) {
      // galois::MethodFlag flag = relabeled ? galois::MethodFlag::UNPROTECTED :
      // galois::MethodFlag::WRITE;
      galois::MethodFlag flag = galois::MethodFlag::UNPROTECTED;
      bool finished           = false;
      int current             = node.current;

      auto ii = graph.edge_begin(src, flag);
      auto ee = graph.edge_end(src, flag);

      std::advance(ii, node.current);

      for (; ii != ee; ++ii, ++current) {
        GNode dst   = graph.getEdgeDst(ii);
        int64_t cap = graph.getEdgeData(ii);
        if (cap == 0) // || current < node.current)
          continue;

        Node& dnode = graph.getData(dst, galois::MethodFlag::UNPROTECTED);
        if (node.height - 1 != dnode.height)
          continue;

        // Push flow
        int64_t amount = std::min(node.excess, cap);
        reduceCapacity(ii, src, dst, amount);

        // Only add once
        if (dst != sink && dst != source && dnode.excess == 0)
          ctx.push(dst);

        assert(node.excess >= amount);
        node.excess -= amount;
        dnode.excess += amount;

        if (node.excess == 0) {
          finished     = true;
          node.current = current;
          break;
        }
      }

      if (finished)
        break;

      relabel(src);
      relabeled = true;

      if (node.height == (int)graph.size())
        break;

      // prevHeight = node.height;
    }

    return relabeled;
  }

  template <DetAlgo version>
  void detDischarge(galois::InsertBag<GNode>& initial, Counter& counter) {
    typedef galois::worklists::Deterministic<> DWL;

    auto detIDfn = [this](const GNode& item) -> uint32_t {
      return graph.getData(item, galois::MethodFlag::UNPROTECTED).id;
    };

    const int relabel_interval =
        global_relabel_interval / galois::getActiveThreads();

    auto detBreakFn = [&, this](void) -> bool {
      if (this->global_relabel_interval > 0 &&
          counter.peekLocal() >= relabel_interval) {
        this->should_global_relabel = true;
        return true;
      } else {
        return false;
      }
    };

    galois::for_each(
        galois::iterate(initial),
        [&, this](GNode& src, auto& ctx) {
          if (version != nondet) {
            if (ctx.isFirstPass()) {
              this->acquire(src);
            }
            if (version == detDisjoint && ctx.isFirstPass()) {
              return;
            } else {
              this->graph.getData(src, galois::MethodFlag::WRITE);
              ctx.cautiousPoint();
            }
          }

          int increment = 1;
          if (this->discharge(src, ctx)) {
            increment += BETA;
          }

          counter += increment;
        },
        galois::loopname("detDischarge"), galois::wl<DWL>(),
        galois::per_iter_alloc(), galois::det_id<decltype(detIDfn)>(detIDfn),
        galois::det_parallel_break<decltype(detBreakFn)>(detBreakFn));
  }

  template <typename W>
  void nonDetDischarge(galois::InsertBag<GNode>& initial, Counter& counter,
                       const W& wl_opt) {

    // per thread
    const int relabel_interval =
        global_relabel_interval / galois::getActiveThreads();

    galois::for_each(
        galois::iterate(initial),
        [&counter, relabel_interval, this](GNode& src, auto& ctx) {
          int increment = 1;
          this->acquire(src);
          if (this->discharge(src, ctx)) {
            increment += BETA;
          }

          counter += increment;
          if (this->global_relabel_interval > 0 &&
              counter.peekLocal() >= relabel_interval) { // local check

            this->should_global_relabel = true;
            ctx.breakLoop();
            return;
          }
        },
        galois::loopname("nonDetDischarge"), galois::parallel_break(), wl_opt);
  }

  /**
   * Do reverse BFS on residual graph.
   */
  template <DetAlgo version, typename WL, bool useCAS = true>
  void updateHeights() {

    galois::for_each(
        galois::iterate({sink}),
        [&, this](const GNode& src, auto& ctx) {
          if (version != nondet) {

            if (ctx.isFirstPass()) {
              for (auto ii :
                   this->graph.edges(src, galois::MethodFlag::WRITE)) {
                GNode dst = this->graph.getEdgeDst(ii);
                int64_t rdata =
                    this->graph.getEdgeData(reverseDirectionEdgeIterator[*ii]);
                if (rdata > 0) {
                  this->graph.getData(dst, galois::MethodFlag::WRITE);
                }
              }
            }

            if (version == detDisjoint && ctx.isFirstPass()) {
              return;
            } else {
              this->graph.getData(src, galois::MethodFlag::WRITE);
              ctx.cautiousPoint();
            }
          }

          for (auto ii :
               this->graph.edges(src, useCAS ? galois::MethodFlag::UNPROTECTED
                                             : galois::MethodFlag::WRITE)) {
            GNode dst = this->graph.getEdgeDst(ii);
            int64_t rdata =
                this->graph.getEdgeData(reverseDirectionEdgeIterator[*ii]);
            if (rdata > 0) {
              Node& node =
                  this->graph.getData(dst, galois::MethodFlag::UNPROTECTED);
              int newHeight =
                  this->graph.getData(src, galois::MethodFlag::UNPROTECTED)
                      .height +
                  1;
              if (useCAS) {
                int oldHeight;
                while (newHeight < (oldHeight = node.height)) {
                  if (__sync_bool_compare_and_swap(&node.height, oldHeight,
                                                   newHeight)) {
                    ctx.push(dst);
                    break;
                  }
                }
              } else {
                if (newHeight < node.height) {
                  node.height = newHeight;
                  ctx.push(dst);
                }
              }
            }
          } // end for
        },
        galois::wl<WL>(), galois::no_conflicts(),
        galois::loopname("updateHeights"));
  }

  template <typename IncomingWL>
  void globalRelabel(IncomingWL& incoming) {

    galois::do_all(galois::iterate(graph),
                   [&](const GNode& src) {
                     Node& node =
                         graph.getData(src, galois::MethodFlag::UNPROTECTED);
                     node.height  = graph.size();
                     node.current = 0;
                     if (src == sink)
                       node.height = 0;
                   },
                   galois::loopname("ResetHeights"));

    using BSWL = galois::worklists::BulkSynchronous<>;
    using DWL  = galois::worklists::Deterministic<>;
    switch (detAlgo) {
    case nondet:
      updateHeights<nondet, BSWL>();
      break;
    case detBase:
      updateHeights<detBase, DWL>();
      break;
    case detDisjoint:
      updateHeights<detDisjoint, DWL>();
      break;
    default:
      std::cerr << "Unknown algorithm" << detAlgo << "\n";
      abort();
    }

    galois::do_all(galois::iterate(graph),
                   [&incoming, this](const GNode& src) {
                     Node& node = this->graph.getData(
                         src, galois::MethodFlag::UNPROTECTED);
                     if (src == this->sink || src == this->source ||
                         node.height >= (int)this->graph.size())
                       return;
                     if (node.excess > 0)
                       incoming.push_back(src);
                   },
                   galois::loopname("FindWork"));
  }

  template <typename C>
  void initializePreflow(C& initial) {
    for (auto ii : graph.edges(source)) {
      GNode dst   = graph.getEdgeDst(ii);
      int64_t cap = graph.getEdgeData(ii);
      reduceCapacity(ii, source, dst, cap);
      Node& node = graph.getData(dst);
      node.excess += cap;
      if (cap > 0)
        initial.push_back(dst);
    }
  }

  void run() {
    Graph *captured_graph = &graph;
    auto obimIndexer = [=](const GNode& n) {
      return -captured_graph->getData(n, galois::MethodFlag::UNPROTECTED).height;
    };

    typedef galois::worklists::PerSocketChunkFIFO<16> Chunk;
    typedef galois::worklists::OrderedByIntegerMetric<decltype(obimIndexer),
                                                      Chunk>
        OBIM;

    galois::InsertBag<GNode> initial;
    initializePreflow(initial);

    while (initial.begin() != initial.end()) {
      galois::StatTimer T_discharge("DischargeTime");
      T_discharge.start();
      Counter counter;
      switch (detAlgo) {
      case nondet:
        if (useHLOrder) {
          nonDetDischarge(initial, counter, galois::wl<OBIM>(obimIndexer));
        } else {
          nonDetDischarge(initial, counter, galois::wl<Chunk>());
        }
        break;
      case detBase:
        detDischarge<detBase>(initial, counter);
        break;
      case detDisjoint:
        detDischarge<detDisjoint>(initial, counter);
        break;
      default:
        std::cerr << "Unknown algorithm" << detAlgo << "\n";
        abort();
      }
      T_discharge.stop();

      if (should_global_relabel) {
        galois::StatTimer T_global_relabel("GlobalRelabelTime");
        T_global_relabel.start();
        initial.clear();
        globalRelabel(initial);
        should_global_relabel = false;
        std::cout << " Flow after global relabel: "
                  << graph.getData(sink).excess << "\n";
        T_global_relabel.stop();
      } else {
        break;
      }
    }
  }

  template <typename EdgeTy>
  static void writePfpGraph(const std::string& inputFile,
                            const std::string& outputFile) {
    typedef galois::graphs::FileGraph ReaderGraph;
    typedef ReaderGraph::GraphNode ReaderGNode;

    ReaderGraph reader;
    reader.fromFile(inputFile);

    typedef galois::graphs::FileGraphWriter Writer;
    typedef galois::LargeArray<EdgeTy> EdgeData;
    typedef typename EdgeData::value_type edge_value_type;

    Writer p;
    EdgeData edgeData;

    // Count edges
    size_t numEdges = 0;
    for (ReaderGraph::iterator ii = reader.begin(), ei = reader.end(); ii != ei;
         ++ii) {
      ReaderGNode rsrc = *ii;
      for (auto jj : reader.edges(rsrc)) {
        ReaderGNode rdst = reader.getEdgeDst(jj);
        if (rsrc == rdst)
          continue;
        if (!reader.hasNeighbor(rdst, rsrc))
          ++numEdges;
        ++numEdges;
      }
    }

    p.setNumNodes(reader.size());
    p.setNumEdges(numEdges);
    p.setSizeofEdgeData(sizeof(edge_value_type));

    p.phase1();
    for (ReaderGraph::iterator ii = reader.begin(), ei = reader.end(); ii != ei;
         ++ii) {
      ReaderGNode rsrc = *ii;
      for (auto jj : reader.edges(rsrc)) {
        ReaderGNode rdst = reader.getEdgeDst(jj);
        if (rsrc == rdst)
          continue;
        if (!reader.hasNeighbor(rdst, rsrc))
          p.incrementDegree(rdst);
        p.incrementDegree(rsrc);
      }
    }

    EdgeTy one = 1;
    static_assert(sizeof(one) == sizeof(uint32_t), "Unexpected edge data size");
    one = galois::convert_le32toh(one);

    p.phase2();
    edgeData.create(numEdges);
    for (ReaderGraph::iterator ii = reader.begin(), ei = reader.end(); ii != ei;
         ++ii) {
      ReaderGNode rsrc = *ii;
      for (auto jj : reader.edges(rsrc)) {
        ReaderGNode rdst = reader.getEdgeDst(jj);
        if (rsrc == rdst)
          continue;
        if (!reader.hasNeighbor(rdst, rsrc))
          edgeData.set(p.addNeighbor(rdst, rsrc), 0);
        EdgeTy cap = useUnitCapacity ? one : reader.getEdgeData<EdgeTy>(jj);
        edgeData.set(p.addNeighbor(rsrc, rdst), cap);
      }
    }

    edge_value_type* rawEdgeData = p.finish<edge_value_type>();
    std::uninitialized_copy(std::make_move_iterator(edgeData.begin()),
                            std::make_move_iterator(edgeData.end()),
                            rawEdgeData);

    using Wnode = Writer::GraphNode;

    struct IdLess {
      bool operator()(
          const galois::graphs::EdgeSortValue<Wnode, edge_value_type>& e1,
          const galois::graphs::EdgeSortValue<Wnode, edge_value_type>& e2)
          const {
        return e1.dst < e2.dst;
      }
    };

    for (Writer::iterator i = p.begin(), end_i = p.end(); i != end_i; ++i) {
      p.sortEdges<edge_value_type>(*i, IdLess());
    }

    p.toFile(outputFile);
  }

  void initializeGraph(std::string inputFile, uint32_t sourceId,
                       uint32_t sinkId) {
    if (useSymmetricDirectly) {
      galois::graphs::readGraph(graph, inputFile);
      for (auto ss : graph)
        for (auto ii : graph.edges(ss))
          graph.getEdgeData(ii) = 1;
    } else {
      if (inputFile.find(".gr.pfp") != inputFile.size() - strlen(".gr.pfp")) {
        std::string pfpName = inputFile + ".pfp";
        std::ifstream pfpFile(pfpName.c_str());
        if (!pfpFile.good()) {
          galois::gPrint("Writing new input file: ", pfpName, "\n");
          writePfpGraph<Graph::edge_data_type>(inputFile, pfpName);
        }
        inputFile = pfpName;
      }
      galois::gPrint("Reading graph: ", inputFile, "\n");
      galois::graphs::readGraph(graph, inputFile);

      // Assume that input edge data has already been converted instead
#if 0 // def HAVE_BIG_ENDIAN
      // Convert edge data to host ordering
      for (auto ss : newApp->graph) {
        for (auto ii : newApp->graph.edges(ss)) {
          Graph::edge_data_type& cap = newApp->graph.getEdgeData(ii);
          static_assert(sizeof(cap) == sizeof(uint32_t), "Unexpected edge data size");
          cap = galois::convert_le32toh(cap);
        }
      }
#endif
    }

    if (sourceId == sinkId || sourceId >= graph.size() ||
        sinkId >= graph.size()) {
      std::cerr << "invalid source or sink id\n";
      abort();
    }

    uint32_t id = 0;
    for (Graph::iterator ii = graph.begin(), ei = graph.end(); ii != ei;
         ++ii, ++id) {
      if (id == sourceId) {
        source                       = *ii;
        graph.getData(source).height = graph.size();
      } else if (id == sinkId) {
        sink = *ii;
      }
      graph.getData(*ii).id = id;
    }

    reverseDirectionEdgeIterator.allocateInterleaved(graph.sizeEdges());
    // memoize the reverse direction edge-iterators
    galois::do_all(galois::iterate(graph.begin(), graph.end()),
                   [&, this](const GNode& src) {
                     for (auto ii : this->graph.edges(
                              src, galois::MethodFlag::UNPROTECTED)) {
                       GNode dst = this->graph.getEdgeDst(ii);
                       reverseDirectionEdgeIterator[*ii] =
                           this->findEdge(dst, src);
                     }
                   },
                   galois::loopname("FindReverseDirectionEdges"));
  }

  void checkSorting(void) {
    for (auto n : graph) {
      galois::optional<GNode> prevDst;
      for (auto e : graph.edges(n, galois::MethodFlag::UNPROTECTED)) {
        GNode dst = graph.getEdgeDst(e);
        if (prevDst.is_initialized()) {
          Node& prevNode =
              graph.getData(*prevDst, galois::MethodFlag::UNPROTECTED);
          Node& currNode = graph.getData(dst, galois::MethodFlag::UNPROTECTED);
          GALOIS_ASSERT(prevNode.id != currNode.id,
                        "Adjacency list cannot have duplicates");
          GALOIS_ASSERT(prevNode.id <= currNode.id, "Adjacency list unsorted");
        }
        prevDst = dst;
      }
    }
  }

  void checkAugmentingPath() {
    // Use id field as visited flag
    for (Graph::iterator ii = graph.begin(), ee = graph.end(); ii != ee; ++ii) {
      GNode src             = *ii;
      graph.getData(src).id = 0;
    }

    std::deque<GNode> queue;

    graph.getData(source).id = 1;
    queue.push_back(source);

    while (!queue.empty()) {
      GNode& src = queue.front();
      queue.pop_front();
      for (auto ii : graph.edges(src)) {
        GNode dst = graph.getEdgeDst(ii);
        if (graph.getData(dst).id == 0 && graph.getEdgeData(ii) > 0) {
          graph.getData(dst).id = 1;
          queue.push_back(dst);
        }
      }
    }

    if (graph.getData(sink).id != 0) {
      assert(false && "Augmenting path exisits");
      abort();
    }
  }

  void checkHeights() {
    for (Graph::iterator ii = graph.begin(), ei = graph.end(); ii != ei; ++ii) {
      GNode src = *ii;
      int sh    = graph.getData(src).height;
      for (auto jj : graph.edges(src)) {
        GNode dst   = graph.getEdgeDst(jj);
        int64_t cap = graph.getEdgeData(jj);
        int dh      = graph.getData(dst).height;
        if (cap > 0 && sh > dh + 1) {
          std::cerr << "height violated at " << graph.getData(src) << "\n";
          abort();
        }
      }
    }
  }

  void checkConservation(PreflowPush& orig) {
    std::vector<GNode> map;
    map.resize(graph.size());

    // Setup ids assuming same iteration order in both graphs
    uint32_t id = 0;
    for (Graph::iterator ii = graph.begin(), ei = graph.end(); ii != ei;
         ++ii, ++id) {
      graph.getData(*ii).id = id;
    }
    id = 0;
    for (Graph::iterator ii = orig.graph.begin(), ei = orig.graph.end();
         ii != ei; ++ii, ++id) {
      orig.graph.getData(*ii).id = id;
      map[id]                    = *ii;
    }

    // Now do some checking
    for (Graph::iterator ii = graph.begin(), ei = graph.end(); ii != ei; ++ii) {
      GNode src        = *ii;
      const Node& node = graph.getData(src);
      uint32_t srcId   = node.id;

      if (src == source || src == sink)
        continue;

      if (node.excess != 0 && node.height != (int)graph.size()) {
        std::cerr << "Non-zero excess at " << node << "\n";
        abort();
      }

      int64_t sum = 0;
      for (auto jj : graph.edges(src)) {
        GNode dst      = graph.getEdgeDst(jj);
        uint32_t dstId = graph.getData(dst).id;
        int64_t ocap =
            orig.graph.getEdgeData(orig.findEdge(map[srcId], map[dstId]));
        int64_t delta = 0;
        if (ocap > 0)
          delta -= (ocap - graph.getEdgeData(jj));
        else
          delta += graph.getEdgeData(jj);
        sum += delta;
      }

      if (node.excess != sum) {
        std::cerr << "Not pseudoflow: " << node.excess << " != " << sum
                  << " at " << node << "\n";
        abort();
      }
    }
  }

  void verify(PreflowPush& orig) {
    // FIXME: doesn't fully check result
    checkHeights();
    checkConservation(orig);
    checkAugmentingPath();
  }
};

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  PreflowPush app;
  app.initializeGraph(filename, sourceId, sinkId);

  app.checkSorting();

  if (relabelInt == 0) {
    app.global_relabel_interval =
        app.graph.size() * ALPHA + app.graph.sizeEdges() / 3;
  } else {
    app.global_relabel_interval = relabelInt;
  }
  std::cout << "Number of nodes: " << app.graph.size() << "\n";
  std::cout << "Global relabel interval: " << app.global_relabel_interval
            << "\n";

  galois::StatTimer T;
  galois::preAlloc(numThreads * app.graph.size() /
                   galois::runtime::pagePoolSize());
  galois::reportPageAlloc("MeminfoPre");
  T.start();
  app.run();
  T.stop();
  galois::reportPageAlloc("MeminfoPost");

  std::cout << "Flow is " << app.graph.getData(app.sink).excess << "\n";

  if (!skipVerify) {
    PreflowPush orig;
    orig.initializeGraph(filename, sourceId, sinkId);
    app.verify(orig);
    std::cout << "(Partially) Verified\n";
  }

  return 0;
}


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

// For problem
#include <cstdio>
#include <fstream>
#include <iostream>
#include <sstream>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "dart/common/Console.hpp"
#include "dart/config.hpp"
#include "dart/dynamics/FreeJoint.hpp"
#include "dart/dynamics/InverseKinematics.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/optimizer/Function.hpp"
#include "dart/optimizer/GradientDescentSolver.hpp"
#include "dart/optimizer/Problem.hpp"

#include "TestHelpers.hpp"
#if HAVE_NLOPT
  #include "dart/optimizer/nlopt/NloptSolver.hpp"
#endif
#if HAVE_IPOPT
  #include "dart/optimizer/ipopt/IpoptSolver.hpp"
#endif
#if HAVE_SNOPT
  #include "dart/optimizer/snopt/SnoptSolver.hpp"
#endif

using namespace std;
using namespace Eigen;
using namespace dart::optimizer;
using namespace dart::dynamics;

//==============================================================================
/// \brief class SampleObjFunc
class SampleObjFunc : public Function
{
public:
  /// \brief Constructor
  SampleObjFunc() : Function() {}

  /// \brief Destructor
  virtual ~SampleObjFunc() {}

  /// \copydoc Function::eval
  double eval(const Eigen::VectorXd& _x) override
  {
    return std::sqrt(_x[1]);
  }

  /// \copydoc Function::evalGradient
  void evalGradient(
      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
  {
    _grad[0] = 0.0;
    _grad[1] = 0.5 / std::sqrt(_x[1]);
  }
};

//==============================================================================
class SampleConstFunc : public Function
{
public:
  /// \brief Constructor
  SampleConstFunc(double _a, double _b) : Function(), mA(_a), mB(_b) {}

  /// \brief Destructor
  virtual ~SampleConstFunc() {}

  /// \copydoc Function::eval
  double eval(const Eigen::VectorXd& _x) override
  {
    return ((mA * _x[0] + mB) * (mA * _x[0] + mB) * (mA * _x[0] + mB) - _x[1]);
  }

  /// \copydoc Function::evalGradient
  void evalGradient(
      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
  {
    _grad[0] = 3 * mA * (mA * _x[0] + mB) * (mA * _x[0] + mB);
    _grad[1] = -1.0;
  }

private:
  /// \brief Data
  double mA;

  /// \brief Data
  double mB;
};

//==============================================================================
TEST(Optimizer, GradientDescent)
{
  std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);

  prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
  prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));

  FunctionPtr obj = std::make_shared<SampleObjFunc>();
  prob->setObjective(obj);

  GradientDescentSolver solver(prob);
  EXPECT_TRUE(solver.solve());

  double minF = prob->getOptimumValue();
  Eigen::VectorXd optX = prob->getOptimalSolution();

  EXPECT_NEAR(minF, 0, 1e-6);
  EXPECT_EQ(optX.size(), static_cast<int>(prob->getDimension()));
  EXPECT_NEAR(optX[0], 1.234, 0.0);
  EXPECT_NEAR(optX[1], 0.0, solver.getTolerance());
}

//==============================================================================
#if HAVE_NLOPT
TEST(Optimizer, BasicNlopt)
{
  // Problem reference: http://ab-initio.mit.edu/wiki/index.php/NLopt_Tutorial

  std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);

  prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
  prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));

  FunctionPtr obj = std::make_shared<SampleObjFunc>();
  prob->setObjective(obj);

  FunctionPtr const1 = std::make_shared<SampleConstFunc>(2, 0);
  FunctionPtr const2 = std::make_shared<SampleConstFunc>(-1, 1);
  prob->addIneqConstraint(const1);
  prob->addIneqConstraint(const2);

  NloptSolver solver(prob, NloptSolver::LD_MMA);
  EXPECT_TRUE(solver.solve());

  double minF = prob->getOptimumValue();
  Eigen::VectorXd optX = prob->getOptimalSolution();

  EXPECT_NEAR(minF, 0.544330847, 1e-6);
  EXPECT_EQ(static_cast<std::size_t>(optX.size()), prob->getDimension());
  EXPECT_NEAR(optX[0], 0.333334, 1e-6);
  EXPECT_NEAR(optX[1], 0.296296, 1e-6);
}
#endif

//==============================================================================
#if HAVE_IPOPT
TEST(Optimizer, BasicIpopt)
{
  std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);

  prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
  prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));

  FunctionPtr obj = std::make_shared<SampleObjFunc>();
  prob->setObjective(obj);

  FunctionPtr const1 = std::make_shared<SampleConstFunc>(2, 0);
  FunctionPtr const2 = std::make_shared<SampleConstFunc>(-1, 1);
  prob->addIneqConstraint(const1);
  prob->addIneqConstraint(const2);

  IpoptSolver solver(prob);
  solver.solve();

  double minF = prob->getOptimumValue();
  Eigen::VectorXd optX = prob->getOptimalSolution();

  EXPECT_NEAR(minF, 0.544330847, 1e-6);
  EXPECT_EQ(static_cast<std::size_t>(optX.size()), prob->getDimension());
  EXPECT_NEAR(optX[0], 0.333334, 1e-6);
  EXPECT_NEAR(optX[1], 0.296296, 1e-6);
}
#endif

//==============================================================================
#if HAVE_SNOPT
TEST(Optimizer, BasicSnopt)
{
  dterr << "SNOPT is not implemented yet.\n";
  return;
}
#endif

//==============================================================================
bool compareStringAndFile(
    const std::string& content, const std::string& fileName)
{
  std::ifstream ifs(fileName, std::ifstream::in);
  EXPECT_TRUE(ifs.is_open());

  auto itr = content.begin();

  char c = ifs.get();
  while (ifs.good())
  {
    if (*itr != c)
      return false;

    c = ifs.get();
    itr++;
  }

  ifs.close();

  return true;
}

//==============================================================================
TEST(Optimizer, OutStream)
{
  std::shared_ptr<Problem> prob = std::make_shared<Problem>(2);

  prob->setLowerBounds(Eigen::Vector2d(-HUGE_VAL, 0));
  prob->setInitialGuess(Eigen::Vector2d(1.234, 5.678));

  FunctionPtr obj = std::make_shared<SampleObjFunc>();
  prob->setObjective(obj);

  GradientDescentSolver solver(prob);
  solver.setIterationsPerPrint(50);

  // Print the progess to a std::string
  std::stringstream ss;
  solver.setOutStream(&ss);
  EXPECT_TRUE(solver.solve());
  std::string outputString = ss.str();

  // Print the progress to a file
  std::string outputFile = "test_optimizer_outstream.txt";
  std::ofstream ofs(outputFile);
  EXPECT_TRUE(ofs.is_open());
  solver.setOutStream(&ofs);
  EXPECT_TRUE(solver.solve());
  ofs.close();

  // Compare them
  EXPECT_TRUE(compareStringAndFile(outputString, outputFile));

  std::remove(outputFile.c_str());
}


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

// This example shows how a partial shape can be reconstructed using a statistical shape model.
//
// WARNING: This example assumes that the closest point of the partial shape to the model mean is also its
// corresponding point. This is only true if the partial shape is close to the model mean. If this is not the
// case, a more sophisticated method for establishing correspondence needs to be used.
//
#include <iostream>
#include <boost/scoped_ptr.hpp>

#include <vtkPolyData.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkVersion.h>

#include "DataManager.h"
#include "PosteriorModelBuilder.h"
#include "StatisticalModel.h"
#include "StatismoIO.h"

#include "vtkStandardMeshRepresenter.h"

typedef statismo::VectorType VectorType;
typedef statismo::MatrixType MatrixType;
typedef statismo::vtkStandardMeshRepresenter RepresenterType;
typedef statismo::StatisticalModel<vtkPolyData> StatisticalModelType;
typedef statismo::PosteriorModelBuilder<vtkPolyData> PosteriorModelBuilderType;
typedef StatisticalModelType::DomainType DomainType;
typedef DomainType::DomainPointsListType::const_iterator DomainPointsConstIterator;

vtkPolyData* loadVTKPolyData(const std::string& filename) {
    vtkPolyDataReader* reader = vtkPolyDataReader::New();
    reader->SetFileName(filename.c_str());
    reader->Update();
    vtkPolyData* pd = vtkPolyData::New();
    pd->DeepCopy(reader->GetOutput());
    reader->Delete();
    return pd;
}

/**
  * Computes the mahalanobis distance of the targetPt, to the model point with the given pointId.
  */
double mahalanobisDistance(const StatisticalModelType* model, unsigned ptId, const statismo::vtkPoint& targetPt) {
    statismo::MatrixType cov = model->GetCovarianceAtPoint(ptId, ptId);
    statismo::vtkPoint meanPt = model->DrawMeanAtPoint(ptId);
    unsigned pointDim = model->GetRepresenter()->GetDimensions();
    assert(pointDim <= 3);

    VectorType x = VectorType::Zero(pointDim);
    for (unsigned d = 0; d < pointDim; d++) {
        x(d) = targetPt[d] - meanPt[d];
    }
    return x.transpose() * cov.inverse() * x;
}

/**
  * Given the inputModel and a partialMesh, the program outputs the posterior model (a statistical model) and
  * its mean, which is the best reconstruction of the partial shape given the model.
 */
int main(int argc, char** argv) {

    if (argc < 5) {
        std::cout << "Usage " << argv[0] << " inputModel  partialShapeMesh posteriorModel reconstructedShape" << std::endl;
        exit(-1);
    }

    std::string inputModelName(argv[1]);
    std::string partialShapeMeshName(argv[2]);
    std::string posteriorModelName(argv[3]);
    std::string reconstructedShapeName(argv[4]);

    try {

        vtkPolyData* partialShape = loadVTKPolyData(partialShapeMeshName);

        RepresenterType* representer = RepresenterType::Create();
        boost::scoped_ptr<StatisticalModelType> inputModel(
                statismo::IO<vtkPolyData>::LoadStatisticalModel(representer, inputModelName));
        vtkPolyData* refPd = const_cast<vtkPolyData*>(inputModel->GetRepresenter()->GetReference());

        StatisticalModelType::PointValueListType constraints;

        // for each point we get the closest point and check how far it is away (in mahalanobis distance).
        // If it is close, we add it as a constraint, otherwise we ignore the remaining points.
        const DomainType::DomainPointsListType& domainPoints = inputModel->GetDomain().GetDomainPoints();
        for (unsigned ptId = 0; ptId < domainPoints.size(); ptId++) {
            statismo::vtkPoint domainPoint = domainPoints[ptId];

            unsigned closestPointId = ptId;
            statismo::vtkPoint closestPointOnPartialShape = partialShape->GetPoint(closestPointId);
            double mhdist = mahalanobisDistance(inputModel.get(), ptId, closestPointOnPartialShape);
            if (mhdist < 5) {
                StatisticalModelType::PointValuePairType ptWithTargetPt(domainPoint, closestPointOnPartialShape);
                constraints.push_back(ptWithTargetPt);
            }
        }

        // build the new model. In addition to the input model and the constraints, we also specify
        // the inaccuracy of our value (variance of the error).

        boost::scoped_ptr<PosteriorModelBuilderType> posteriorModelBuilder(PosteriorModelBuilderType::Create());
        boost::scoped_ptr<StatisticalModelType> constraintModel(posteriorModelBuilder->BuildNewModelFromModel(inputModel.get(), constraints, 0.5));

        // The resulting model is a normal statistical model, from which we could for example sample examples.
        // Here we simply  save it to disk for later use.
        statismo::IO<vtkPolyData>::SaveStatisticalModel(constraintModel.get(), posteriorModelName);
        std::cout << "successfully saved the model to " << posteriorModelName << std::endl;

        // The mean of the constraint model is the optimal reconstruction
        vtkPolyData* pmean = constraintModel->DrawMean();
        vtkPolyDataWriter* writer = vtkPolyDataWriter::New();
#if (VTK_MAJOR_VERSION == 5 )
        writer->SetInput(pmean);
#else
        writer->SetInputData(pmean);
#endif
        writer->SetFileName(reconstructedShapeName.c_str());
        writer->Update();
    } catch (statismo::StatisticalModelException& e) {
        std::cout << "Exception occured while building the intenisity model" << std::endl;
        std::cout << e.what() << std::endl;
    }
}


#pragma once

#include <armadillo>

#include "heattransfer/radial.hpp"

class Pipeline;
class PipeWall;
class BurialMedium;
class AmbientFluid;

/*!
 * \brief Class that implements steady state heat transfer between gas and
 * pipeline surroundings.
 *
 * This is well documented in Jan Fredrik Helgaker's PhD thesis.
 */
class SteadyStateHeatTransfer : public RadialHeatTransfer
{
public:
    /*!
     * \brief Construct from full description of pipeline.
     * \param diameter Inner diameter [m]
     * \param pipeWall PipeWall instance
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     * \param burialMedium BurialMedium instace
     * \param ambientFluid AmbientFluid instance
     */
    SteadyStateHeatTransfer(
            const double diameter,
            const PipeWall& pipeWall,
            const double burialDepth,
            const BurialMedium& burialMedium,
            const AmbientFluid& ambientFluid);

    /*!
     * \brief Constructor with default pipe wall, burial medium and ambient medium.
     * \param diameter Inner diameter [m]
     * \param burialDepth Distance from top of pipe to top of burial medium [m]
     */
    SteadyStateHeatTransfer(
            const double diameter = 1.0,
            const double burialDepth = 1.0);

    /*!
     * \brief Evaluate 1d radial steady state heat transfer.
     *
     * Operates on a HeatTransferState and returns a new HeatTransferState,
     * but does not require discretization temperature.
     *
     * \param current Current HeatTransferState
     * \param timeStep Time step [s]
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    virtual HeatTransferState evaluate(
            const HeatTransferState& current,
            const double timeStep,
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const override;

    /*!
     * \brief Internal method used for evaluating steady state heat transfer.
     *
     * This is exposed for testing purposes. We typically use pointers anyway,
     * so this is not accessible without casting to SteadyStateHeatTransfer.
     *
     * \param ambientTemperature Ambient temperature [K]
     * \param gasPressure Gas pressure [Pa]
     * \param gasTemperature Gas temperature [K]
     * \param gasReynoldsNumber Reynolds number of gas [-]
     * \param gasHeatCapacity Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return HeatTransferState with new heat flux.
     */
    HeatTransferState evaluateInternal(
            const double ambientTemperature,
            const double gasPressure,
            const double gasTemperature,
            const double gasReynoldsNumber,
            const double gasHeatCapacity,
            const double gasViscosity) const;

    /*!
     * \brief Calculate the total heat transfer coefficient U.
     *
     * This is exposed for testing purposes. We typically use pointers anyway,
     * so this is not accessible without casting to SteadyStateHeatTransfer.
     *
     * \param gasPressure Gas pressure [Pa]
     * \param gasReynoldsNumber Gas Reynolds number [-]
     * \param gasHeatCapacityConstantPressure Gas heat capacity (\f$c_p\f$) [J/(kg K)]
     * \param gasViscosity Gas dynamic viscosity [Pa s] = [kg/m*s]
     * \return Total heat transfer coefficient U [W/(m2 K)]
     */
    double calculateHeatTransferCoefficient(
            const double gasPressure,
            const double gasReynoldsNumber,
            const double gasHeatCapacityConstantPressure,
            const double gasViscosity) const;

    //! Get the total heat transfer coefficient of all radial discretization shells.
    //! Does not include the inner film coefficient.
    double getOverallHeatTransferCoefficient() const { return m_overallHeatTransferCoefficient; }
    //! Get the total thermal resistance of all radial discretization shells.
    //! Does not include the inner film coefficient.
    double getOverallThermalResistance() const { return m_overallThermalResistance; }

private:
    //! Total heat transfer coefficient of all radial discretization shells.
    //! Does not include the inner film coefficient.
    double m_overallHeatTransferCoefficient;

    //! Total thermal resistance of all radial discretization shells.
    //! Does not include the inner film coefficient.
    double m_overallThermalResistance;
};


#ifndef __CTS_HPP__
#define __CTS_HPP__

/****************************************************************
      Author: Joel Veness
        Date: 2011
        Info: A Context Tree Switching (CTS) implementation.
*****************************************************************/

#include "common.hpp"

// boost includes
#include <boost/utility.hpp>
#include <boost/pool/pool.hpp>

// context tree node
class SNode {

    friend class SwitchingTree;
    friend std::ostream &operator<<(std::ostream &o, const SNode &);

    public:

        SNode(int depth);
        explicit SNode(const SNode &rhs, int pindx);

        /// process a new binary symbol, with switching rate alpha, and blend 1-2*alpha
        void update(bit_t b, double log_alpha, double log_blend, double log_split_mul);

        /// log weighted blocked probability
        weight_t logProbWeighted() const;

        /// log KT estimated probability
        weight_t logProbEstimated() const;

        /// child corresponding to a particular symbol
        const SNode *child(bit_t b) const;

        /// the number of times this context been visited
        count_t visits() const;

        /// number of descendents
        size_t size() const;

    private:

        // compute the result of an update call non-destructively
        double updateNonDestructive(bit_t b, double c_weighted, double c_weighted_old) const;

        // is the current node a leaf node?
        bool isLeaf() const;

        // compute the logarithm of the KT-estimator update multiplier
        double logKTMul(bit_t b) const;

        weight_t m_log_prob_est;
        weight_t m_log_prob_weighted;

        // switching weights
        weight_t m_log_b;
        weight_t m_log_s;

        // one slot for each binary value
        count_t m_count[2];
        SNode *m_child[2];

        // m_backidx >= 0 when a previous symbol's context was pruned
        int m_pruned_idx;
};

extern std::ostream &operator<<(std::ostream &o, const SNode &sn);

// a context tree used for CTW mixing
class SwitchingTree : public Compressor, boost::noncopyable {

    typedef std::pair<SNode *, SNode> ctpair_t;

    public:

        /// create a context tree of specified maximum depth and size
        SwitchingTree(history_t &history, size_t depth, int phase=-1);

        /// delete the context tree
        ~SwitchingTree();

        /// file extension
        const char *fileExtension() const { return "cts"; }

        /// the logarithm of the probability of all processed experience
        double logBlockProbability() const;

        /// the probability of seeing a particular symbol next
        double prob(bit_t b);

        /// process a new piece of sensory experience
        void update(bit_t b);

        /// the depth of the context tree
        size_t depth() const;

        /// number of nodes in the context tree
        size_t size() const;

    private:

        // compute the switching rate for a given time t
        double switchRate(size_t t) const;

        // recover the memory used by a node
        void reclaimMemory(SNode *n);

        // computes the context, creates relevant nodes and determine the path to update
        void makeContextAndPath();

        // determine whether two contexts are identical
        static bool contextsEqual(const context_t &lhs, const context_t &rhs);

        // compute the current context
        void getContext(const history_t &h, context_t &context, int idx = -1);

        // create (if necessary) all of the nodes in the current context
        void createNodesInCurrentContext(const context_t &context);

        // recursively deletes the nodes in a context tree
        void deleteCT(SNode *root);

        boost::pool<> m_ctnode_pool;

        SNode *m_root;
        int m_phase;
        size_t m_depth;
        context_t m_context;
        context_t m_pcontext;
        std::vector<SNode *> m_created;
        std::vector<ctpair_t> m_modified;
        std::vector<SNode *> m_path;
        std::vector<weight_t> m_log_old_weights;
        history_t &m_history;
        int m_prob_cache;
};

#endif // __CTS_HPP__


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/CalculatorBasics/PropertyList.h>
#include <Utils/CalculatorBasics/ResultsAutoCompleter.h>
#include <Utils/ExternalQC/Orca/OrcaHessianOutputParser.h>
#include <Utils/Geometry/ElementTypes.h>
#include <Utils/IO/ChemicalFileFormats/XyzStreamHandler.h>
#include <Utils/Scf/LcaoUtils/ElectronicOccupation.h>
#include <gmock/gmock.h>
#include <Eigen/Eigenvalues>
#include <boost/dll/runtime_symbol_info.hpp>

using namespace testing;
namespace Scine {
namespace Utils {

class AResultsAutoCompleterTest : public Test {
 public:
  Results arbitraryResults;
  std::unique_ptr<ResultsAutoCompleter> arbitraryResultsAutoCompleter;
  HessianMatrix arbitraryHessian;
  Eigen::MatrixXd arbitraryOneElectronMatrix;
  BondOrderCollection arbitraryBondOrders;
  ElementTypeCollection arbitraryElements;
  PositionCollection randomPositions;
  AtomCollection randomAtomCollection;
  MolecularOrbitals arbitraryCoefficientMatrix;
  LcaoUtils::ElectronicOccupation arbitraryOccupation;
  Eigen::MatrixXd arbitraryOverlapMatrix;
  DensityMatrix arbitraryDensityMatrix;
  AtomsOrbitalsIndexes arbitraryAOToAtomMapping;
  Eigen::MatrixXd generateRandomOverlapMatrix(int nOrbitals);
  Eigen::MatrixXd generateNonOrthogonalCoefficientMatrix(const Eigen::MatrixXd& overlap, int nOrbitals);

 private:
  void SetUp() final {
    arbitraryElements = ElementTypeCollection{ElementType::Am, ElementType::Ce, ElementType::Ca};
    randomPositions = Eigen::MatrixX3d::Random(3, 3);
    randomAtomCollection.resize(arbitraryElements.size());
    randomAtomCollection.setElements(arbitraryElements);
    randomAtomCollection.setPositions(randomPositions);
    const int nOrbitals = 15;
    arbitraryOccupation.fillLowestRestrictedOrbitalsWithElectrons(4);
    arbitraryOverlapMatrix = generateRandomOverlapMatrix(nOrbitals);
    // Generate coefficient matrix as Eigen type
    auto arbitraryCoefficients = generateNonOrthogonalCoefficientMatrix(arbitraryOverlapMatrix, nOrbitals);
    // Deduce coefficients as MolecularOrbitals type
    arbitraryCoefficientMatrix = MolecularOrbitals::createFromRestrictedCoefficients(arbitraryCoefficients);
    // Set up AO to Atom Mapping
    arbitraryAOToAtomMapping.clear();
    arbitraryAOToAtomMapping.setSize(3);
    arbitraryAOToAtomMapping.addAtom(7);
    arbitraryAOToAtomMapping.addAtom(3);
    arbitraryAOToAtomMapping.addAtom(5);
  }
};

// Adapted from HFWaveFunctionOverlapTest to get arbitrary overlap and MO coefficients
Eigen::MatrixXd AResultsAutoCompleterTest::generateRandomOverlapMatrix(int nOrbitals) {
  Eigen::MatrixXd random = Eigen::MatrixXd::Random(nOrbitals, nOrbitals);
  random /= 10;
  for (int i = 0; i < nOrbitals; ++i)
    random(i, i) = 1;
  return .5 * (random + random.transpose());
}
Eigen::MatrixXd AResultsAutoCompleterTest::generateNonOrthogonalCoefficientMatrix(const Eigen::MatrixXd& overlap, int nOrbitals) {
  Eigen::MatrixXd matrixToDiagonalize = 20 * Eigen::MatrixXd::Random(nOrbitals, nOrbitals);
  Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es;
  es.compute(matrixToDiagonalize, overlap);
  return es.eigenvectors();
}

TEST_F(AResultsAutoCompleterTest, CanConstructResultAutoCompleter) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
}

TEST_F(AResultsAutoCompleterTest, CanDetermineDensityMatrixGenerability) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::ElectronicOccupation>(arbitraryOccupation);
  // Just one more random property
  arbitraryResults.set<Property::OneElectronMatrix>(arbitraryOneElectronMatrix);

  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::DensityMatrix));

  arbitraryResults.set<Property::CoefficientMatrix>(arbitraryCoefficientMatrix);

  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::DensityMatrix));
}

TEST_F(AResultsAutoCompleterTest, CanDetermineThermochemistryGenerability) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::ElectronicOccupation>(arbitraryOccupation);
  // Just one more random property
  arbitraryResults.set<Property::OneElectronMatrix>(arbitraryOneElectronMatrix);

  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::Thermochemistry));

  arbitraryResults.set<Property::Hessian>(arbitraryHessian);

  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::Thermochemistry));
}

TEST_F(AResultsAutoCompleterTest, CanDetermineAtomicChargesGenerability) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::AOtoAtomMapping>(arbitraryAOToAtomMapping);
  arbitraryResults.set<Property::DensityMatrix>(arbitraryDensityMatrix);

  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::AtomicCharges));

  arbitraryResults.set<Property::OverlapMatrix>(arbitraryOverlapMatrix);

  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::AtomicCharges));
}

TEST_F(AResultsAutoCompleterTest, CanDetermineBondOrderGenerability) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::AOtoAtomMapping>(arbitraryAOToAtomMapping);
  arbitraryResults.set<Property::DensityMatrix>(arbitraryDensityMatrix);

  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::BondOrderMatrix));

  arbitraryResults.set<Property::OverlapMatrix>(arbitraryOverlapMatrix);

  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::BondOrderMatrix));
}

TEST_F(AResultsAutoCompleterTest, CanDetermineEnergyUngenerability) {
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::Energy));
}

TEST_F(AResultsAutoCompleterTest, CanComputeThermochemistry) {
  // Generate AtomCollection corresponding to H2O+ as given in ORCA input
  std::stringstream stream("3\n\n"
                           "H      0.7493682000    0.0000000000    0.4424329000\n"
                           "O      0.0000000000    0.0000000000   -0.1653507000\n"
                           "H     -0.7493682000    0.0000000000    0.4424329000\n");
  auto structure = XyzStreamHandler::read(stream);
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(structure);
  // C2v symmetry
  arbitraryResultsAutoCompleter->setMolecularSymmetryNumber(2);

  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  // Set electronic occupation
  // Only thing that is really needed from occupation is multiplicity so make sure it is correct
  LcaoUtils::ElectronicOccupation unrestrictedOccupation;
  unrestrictedOccupation.fillLowestUnrestrictedOrbitals(5, 4);
  arbitraryResults.set<Property::ElectronicOccupation>(unrestrictedOccupation);
  // Electronic energy from Resources/orca_test_calc.out
  arbitraryResults.set<Property::Energy>(-75.818269296087);

  // Read in Hessian from ORCA output given in ExternalQC test
  auto pathToResources = boost::dll::program_location().parent_path();
  pathToResources /= "Resources";
  const auto hessianFile = (pathToResources / "orca_test_calc.hess").string();
  HessianMatrix hessian = ExternalQC::OrcaHessianOutputParser::getHessian(hessianFile);
  arbitraryResults.set<Property::Hessian>(hessian);

  // Should be able to compute Thermochemistry
  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::Thermochemistry));
  // Launch calculation
  arbitraryResultsAutoCompleter->generateProperties(arbitraryResults, structure);

  // Should have computed thermochemical properties
  ASSERT_TRUE(arbitraryResults.has<Property::Thermochemistry>());
  // Insufficient information for calculating Atomic Charges so they should not be there
  ASSERT_FALSE(arbitraryResults.has<Property::AtomicCharges>());

  // Check whether computed Thermochemical properties match orca_test_calc.out
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().vibrationalComponent.zeroPointVibrationalEnergy,
              DoubleNear(0.02101209, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.enthalpy, DoubleNear(-75.79347127, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.entropy * 298.15, DoubleNear(0.02210368, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.gibbsFreeEnergy, DoubleNear(-75.81557495, 1e-5));
}

TEST_F(AResultsAutoCompleterTest, CanComputeThermochemistryReverseOccupation) {
  // Generate AtomCollection corresponding to H2O+ as given in ORCA input
  std::stringstream stream("3\n\n"
                           "H      0.7493682000    0.0000000000    0.4424329000\n"
                           "O      0.0000000000    0.0000000000   -0.1653507000\n"
                           "H     -0.7493682000    0.0000000000    0.4424329000\n");
  auto structure = XyzStreamHandler::read(stream);
  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(structure);
  // C2v symmetry
  arbitraryResultsAutoCompleter->setMolecularSymmetryNumber(2);

  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  // Set electronic occupation
  // Only thing that is really needed from occupation is multiplicity so make sure it is correct
  LcaoUtils::ElectronicOccupation unrestrictedOccupation;
  // Reverse occupation w.r.t. CanComputeThermochemistry
  unrestrictedOccupation.fillLowestUnrestrictedOrbitals(4, 5);
  arbitraryResults.set<Property::ElectronicOccupation>(unrestrictedOccupation);
  // Electronic energy from Resources/orca_test_calc.out
  arbitraryResults.set<Property::Energy>(-75.818269296087);

  // Read in Hessian from ORCA output given in ExternalQC test
  auto pathToResources = boost::dll::program_location().parent_path();
  pathToResources /= "Resources";
  const auto hessianFile = (pathToResources / "orca_test_calc.hess").string();
  HessianMatrix hessian = ExternalQC::OrcaHessianOutputParser::getHessian(hessianFile);
  arbitraryResults.set<Property::Hessian>(hessian);

  // Should be able to compute Thermochemistry
  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::Thermochemistry));
  // Launch calculation
  arbitraryResultsAutoCompleter->generateProperties(arbitraryResults, structure);

  // Should have computed thermochemical properties
  ASSERT_TRUE(arbitraryResults.has<Property::Thermochemistry>());
  // Insufficient information for calculating Atomic Charges so they should not be there
  ASSERT_FALSE(arbitraryResults.has<Property::AtomicCharges>());

  // Check whether computed Thermochemical properties match orca_test_calc.out
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().vibrationalComponent.zeroPointVibrationalEnergy,
              DoubleNear(0.02101209, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.enthalpy, DoubleNear(-75.79347127, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.entropy * 298.15, DoubleNear(0.02210368, 1e-5));
  ASSERT_THAT(arbitraryResults.get<Property::Thermochemistry>().overall.gibbsFreeEnergy, DoubleNear(-75.81557495, 1e-5));
}

TEST_F(AResultsAutoCompleterTest, CanComputeDensityMatrix) {
  // Set required properties
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::CoefficientMatrix>(arbitraryCoefficientMatrix);
  arbitraryResults.set<Property::ElectronicOccupation>(arbitraryOccupation);

  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);

  // Should be able to compute DensityMatrix
  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::DensityMatrix));
  // Launch calculation
  arbitraryResultsAutoCompleter->generateProperties(arbitraryResults, randomAtomCollection);
  // Should have computed density matrix
  ASSERT_TRUE(arbitraryResults.has<Property::DensityMatrix>());
}

TEST_F(AResultsAutoCompleterTest, CanComputeAtomicChargesAndBondOrderMatrix) {
  // Set required properties
  arbitraryResults.set<Property::SuccessfulCalculation>(true);
  arbitraryResults.set<Property::CoefficientMatrix>(arbitraryCoefficientMatrix); // used for DensityMatrix
  arbitraryResults.set<Property::ElectronicOccupation>(arbitraryOccupation);     // used for DensityMatrix
  arbitraryResults.set<Property::OverlapMatrix>(arbitraryOverlapMatrix);
  arbitraryResults.set<Property::AOtoAtomMapping>(arbitraryAOToAtomMapping);

  arbitraryResultsAutoCompleter = std::make_unique<ResultsAutoCompleter>(randomAtomCollection);

  // Should be able to compute DensityMatrix
  ASSERT_TRUE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::DensityMatrix));
  // Should not be able to compute AtomicCharges and BondOrders right now (depend on DensityMatrix)
  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::AtomicCharges));
  ASSERT_FALSE(arbitraryResultsAutoCompleter->propertyGeneratable(arbitraryResults, Property::BondOrderMatrix));
  // Launch calculation
  arbitraryResultsAutoCompleter->generateProperties(arbitraryResults, randomAtomCollection);

  // Should have computed density matrix, atomic charges and bond order matrix
  ASSERT_TRUE(arbitraryResults.has<Property::DensityMatrix>());
  ASSERT_TRUE(arbitraryResults.has<Property::AtomicCharges>());
  ASSERT_TRUE(arbitraryResults.has<Property::BondOrderMatrix>());
}

} // namespace Utils
} // namespace Scine


/*
use to control uav fly to track target
*/

#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <thread>
#include <geometry_msgs/PointStamped.h>
#include <std_srvs/Empty.h>
#include <Eigen/Core>
#include <vector>

#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <std_msgs/UInt16MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>

using namespace std;

ros::Publisher trajectory_pub;
geometry_msgs::PointStamped current_position;

float linear_smoothing_navigation_step = 1;
int flag_tasks_OK = 0;
vector<int> Mid(375,240);//图像中心点
int Area = 100*80;//ROI应该占有的像素面积

void flyToTarget(const std_msgs::UInt16MultiArray&);
void updateUavPosition(const geometry_msgs::PointStamped&);
double getDistanceToTarget(const Eigen::Vector3d&);
bool reachTargetPosition(const Eigen::Vector3d&, float);
bool linearSmoothingNavigationTask(const Eigen::Vector3d&);
vector<int> getBoxCenter(int, int, int, int);
Eigen::Vector3d getTarget(const vector<int>&);

int main(int argc, char** argv) {
    ROS_INFO("uav tracking is running...");
    ros::init(argc, argv, "UAV Tracking");
    ros::NodeHandle nh;
    // Create a private node handle for accessing node parameters.
    ros::NodeHandle nh_private("~");

    std::string uav_name = "";
    uav_name = "firefly";
    ros::param::get("~mav_name",uav_name);
    string topic_dest = "/uav_tracking/target_point";

    // 订阅话题
    // /odometry_sensor1/position   无人机位置信息(包含噪声)
    ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUavPosition);
    ros::Subscriber destination_sub = nh.subscribe(topic_dest, 10, &flyToTarget);

    trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        // ROS_INFO("UAV navigation is running...");

        ros::spinOnce();
        loop_rate.sleep();
    }
}

/*
得到中心点和框面积
*/
vector<int> getBoxCenter(int x, int y, int w, int h)
{
    vector<int> boxCenterNArea;
    boxCenterNArea[0] = x + w/2;
    boxCenterNArea[1] = y + h/2;
    boxCenterNArea[2] = w*h;
    return boxCenterNArea;
}

/*
得到目标点：
通过比较 boxCenter 和 Mid，得到应该往current_position的哪边走

位置更新算法：移动到物体成像在图像中心
设跟踪距离为d，即物体到相机感光器件远点的距离
焦距为ｆ
在x轴上：d/f=X/x，　X为需要向右移动的距离，x为物体图像中心像素距离整个图片中心像素的距离
y轴同x轴
z轴需要根据物体成像大小比例来计算：成像大了向后，小了向前
*/
Eigen::Vector3d getTarget(const vector<int>& boxCenterNArea)
{
    int pixDistX = boxCenterNArea[0] - Mid[0];
    int pixDistY = boxCenterNArea[1] - Mid[1];
    int areaDistZ = boxCenterNArea[2] - Area;
    float positionUpdateX = 0, positionUpdateY = 0, positionUpdateZ = 0;
    //随便取值测试
    positionUpdateX = pixDistX * 0.05;
    positionUpdateY = pixDistY * 0.05;
    positionUpdateZ = (float)areaDistZ/Area * 1.0;
    Eigen::Vector3d target3d(current_position.point.x+positionUpdateX, current_position.point.y+positionUpdateY,current_position.point.z+positionUpdateZ);
    return target3d;
}

/*
Description:
    void flyToTarget(const std_msgs::UInt16MultiArray& targetBox)
    飞到目标点

Parameters:
    targetBox 目标在图片中的位置信息
    ｘ,y,width,height

Return:
    无
*/
void flyToTarget(const std_msgs::UInt16MultiArray& targetBox)
{
    //把target转化为飞行目标点
    vector<int> boxCenterNArea = getBoxCenter(targetBox.data[0],targetBox.data[1],targetBox.data[2],targetBox.data[3]);

    Eigen::Vector3d target = getTarget(boxCenterNArea);

    bool ok = linearSmoothingNavigationTask(target);
    if(!ok)
        cout << "fail to fly to target..." << endl;
}

/*
Description:
    updateUavPosition(const geometry_msgs::PointStamped& msg)
    gps数据更新的回调函数.

Parameters:
    msg 位置信息

Return:
    无
*/
void updateUavPosition(const geometry_msgs::PointStamped& msg)
{
    current_position = msg;
    // std::cout<<"UAV current position is: "<<msg.point.x<< msg.point.y<< msg.point.z<<std::endl;
}

/*
Description:
    getDistanceToTarget(const Eigen::Vector3d& target)
    获取当前位置到指定位置位置的距离.

Parameters:
    target 需要飞达的位置点

Return:
    double 当前位置到达目标点的位置
*/
double getDistanceToTarget(const Eigen::Vector3d& target)
{
    double temp = 0;
    temp += pow((target[0] - current_position.point.x), 2);
    temp += pow((target[1] - current_position.point.y), 2);
    temp += pow((target[2] - current_position.point.z), 2);
    temp = sqrt(temp);
    return temp;
}

/*
Description:
    reachTargetPosition(const Eigen::Vector3d& target, float max_error)
    判定是否到达指定的目标点.

Parameters:
    target 需要飞达的位置点
    max_error 允许的位置误差阈值,当前位置和目标位置小于该阈值时,判定无人机到达目标点

Return:
    bool 到达目标点时返回 true
            未到达目标点时返回 false
*/
bool reachTargetPosition(const Eigen::Vector3d& target, float max_error)
{
    double temp = getDistanceToTarget(target);

    if (temp < max_error)
        return true;
    return false;
}

/*
Description:
    linearSmoothingNavigationTask(const Eigen::Vector3d& target)
    控制无人机从当前位置飞向指定位置.

Parameters:
    target 需要飞达的位置点

Return:
    bool 起飞结束后返回 true
            起飞过程中返回 false
*/
bool linearSmoothingNavigationTask(const Eigen::Vector3d& target)
{
    trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
    trajectory_msg.header.stamp = ros::Time::now();

    if (reachTargetPosition(target,0.2))
        return true;

    double dist = getDistanceToTarget(target);
    Eigen::Vector3d next_step;

    if(dist<linear_smoothing_navigation_step)
    {
        next_step = target;
    }
    else
    {
        next_step[0] = current_position.point.x+(target[0]-current_position.point.x)/dist*linear_smoothing_navigation_step;
        next_step[1] = current_position.point.y+(target[1]-current_position.point.y)/dist*linear_smoothing_navigation_step;
        next_step[2] = current_position.point.z+(target[2]-current_position.point.z)/dist*linear_smoothing_navigation_step;
    }

    double desired_yaw = 0.0;

    mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(next_step, desired_yaw, &trajectory_msg);
    trajectory_pub.publish(trajectory_msg);
    return false;
}


#include <iostream>
#include <string>
#include <armadillo>
#include <time.h>
#include "ga.h"
#include "simpleNN.h"

using namespace std;
using namespace arma;

int main(){

    vec inp = ones<vec>(5);
    mat inpWeights = ones<mat>(5,5);
    cube weights = ones<cube>(5,5,5);
    mat outWeights = ones<mat>(5,5);
    vec inpBiases = ones<vec>(5);
    mat biases = ones<mat>(5,5);
    vec outBiases = ones<vec>(5);

cout <<    simpleNN(inp, inpWeights , weights, outWeights , inpBiases, biases, outBiases) << endl;
cout << weights.n_elem<< endl;
//    cout << sum(a%b) << endl;

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_FREXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_FREXP_HPP_INCLUDED

#include <boost/simd/ieee/functions/frexp.hpp>

#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_andnot.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/shr.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/logical_notand.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/constants/maxexponentm1.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/include/constants/mask1frexp.hpp>
#include <boost/simd/include/constants/mask2frexp.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/type_traits/is_same.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/constants/twotonmb.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF         ( frexp_
                                      , tag::cpu_, (A0)(A1)(A2)(X)(Y)
                                      , ( boost::mpl::and_
                                          < boost::is_same<A0,A1>
                                          , boost::is_same
                                            < typename dispatch::meta
                                                        ::as_integer<A0>::type
                                            , A2
                                            >
                                          >
                                        )
                                      , ((simd_< floating_<A0>, X>))
                                        ((simd_< floating_<A1>, X>))
                                        ((simd_< integer_<A2>, Y>))
                                      )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 & r0, A2 & r1) const
    {
      typedef typename meta::as_logical<A0>::type bA0;
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      typedef typename meta::as_logical<int_type>::type                 bint_type;
      typedef typename meta::scalar_of<A0>::type                           s_type;

#ifndef BOOST_SIMD_NO_DENORMALS
      bA0 test = logical_and(lt(simd::abs(a0), Smallestposval<A0>()), is_nez(a0));
      A0 aa0 = if_else(test, Twotonmb<A0>()*a0, a0);
      A2 t = if_else_zero(test,Nbmantissabits<A0>());
#else
      A0 aa0 = a0;
#endif
      r1 = simd::bitwise_cast<int_type>(b_and(aa0, Mask1frexp<A0>())); //extract exp.
      A0  x   = b_andnot(aa0, Mask1frexp<A0>());
      r1  = shri(r1,Nbmantissabits<s_type>()) - Maxexponentm1<A0>();
      r0  = b_or(x,Mask2frexp<A0>());

      bA0       test0 = is_nez(aa0);
      bint_type test1 = gt(r1,Limitexponent<A0>());

      r1 = if_else_zero(logical_notand(test1, test0), r1);
#ifndef BOOST_SIMD_NO_DENORMALS
      r1 -= t ;
#endif
      r0 = if_else_zero(test0, seladd(test1,r0,aa0));
    }
  };
} } }

#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_OEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OEA_HPP

#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace oea
    {
            template <typename T>
            struct par_oea
            {
                T    theta;
                T    m, n;
                T    two_r_m, two_r_n, rm, rn, hm, hn;
                T    cp0, sp0;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_oea_spheroid
                : public base_t_fi<base_oea_spheroid<T, Parameters>, T, Parameters>
            {
                par_oea<T> m_proj_parm;

                inline base_oea_spheroid(const Parameters& par)
                    : base_t_fi<base_oea_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T Az, M, N, cp, sp, cl, shz;

                    cp = cos(lp_lat);
                    sp = sin(lp_lat);
                    cl = cos(lp_lon);
                    Az = aatan2(cp * sin(lp_lon), this->m_proj_parm.cp0 * sp - this->m_proj_parm.sp0 * cp * cl) + this->m_proj_parm.theta;
                    shz = sin(0.5 * aacos(this->m_proj_parm.sp0 * sp + this->m_proj_parm.cp0 * cp * cl));
                    M = aasin(shz * sin(Az));
                    N = aasin(shz * cos(Az) * cos(M) / cos(M * this->m_proj_parm.two_r_m));
                    xy_y = this->m_proj_parm.n * sin(N * this->m_proj_parm.two_r_n);
                    xy_x = this->m_proj_parm.m * sin(M * this->m_proj_parm.two_r_m) * cos(N) / cos(N * this->m_proj_parm.two_r_n);
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T N, M, xp, yp, z, Az, cz, sz, cAz;

                    N = this->m_proj_parm.hn * aasin(xy_y * this->m_proj_parm.rn);
                    M = this->m_proj_parm.hm * aasin(xy_x * this->m_proj_parm.rm * cos(N * this->m_proj_parm.two_r_n) / cos(N));
                    xp = 2. * sin(M);
                    yp = 2. * sin(N) * cos(M * this->m_proj_parm.two_r_m) / cos(M);
                    cAz = cos(Az = aatan2(xp, yp) - this->m_proj_parm.theta);
                    z = 2. * aasin(0.5 * boost::math::hypot(xp, yp));
                    sz = sin(z);
                    cz = cos(z);
                    lp_lat = aasin(this->m_proj_parm.sp0 * cz + this->m_proj_parm.cp0 * sz * cAz);
                    lp_lon = aatan2(sz * sin(Az),
                        this->m_proj_parm.cp0 * cz - this->m_proj_parm.sp0 * sz * cAz);
                }

                static inline std::string get_name()
                {
                    return "oea_spheroid";
                }

            };

            // Oblated Equal Area
            template <typename Params, typename Parameters, typename T>
            inline void setup_oea(Params const& params, Parameters& par, par_oea<T>& proj_parm)
            {
                if (((proj_parm.n = pj_get_param_f<T, srs::spar::n>(params, "n", srs::dpar::n)) <= 0.) ||
                    ((proj_parm.m = pj_get_param_f<T, srs::spar::m>(params, "m", srs::dpar::m)) <= 0.)) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_invalid_m_or_n) );
                } else {
                    proj_parm.theta = pj_get_param_r<T, srs::spar::theta>(params, "theta", srs::dpar::theta);
                    proj_parm.sp0 = sin(par.phi0);
                    proj_parm.cp0 = cos(par.phi0);
                    proj_parm.rn = 1./ proj_parm.n;
                    proj_parm.rm = 1./ proj_parm.m;
                    proj_parm.two_r_n = 2. * proj_parm.rn;
                    proj_parm.two_r_m = 2. * proj_parm.rm;
                    proj_parm.hm = 0.5 * proj_parm.m;
                    proj_parm.hn = 0.5 * proj_parm.n;
                    par.es = 0.;
                }
            }

    }} // namespace detail::oea
    #endif // doxygen

    /*!
        \brief Oblated Equal Area projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - n (real)
         - m (real)
         - theta: Theta (degrees)
        \par Example
        \image html ex_oea.gif
    */
    template <typename T, typename Parameters>
    struct oea_spheroid : public detail::oea::base_oea_spheroid<T, Parameters>
    {
        template <typename Params>
        inline oea_spheroid(Params const& params, Parameters const& par)
            : detail::oea::base_oea_spheroid<T, Parameters>(par)
        {
            detail::oea::setup_oea(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_oea, oea_spheroid, oea_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(oea_entry, oea_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(oea_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(oea, oea_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_OEA_HPP


// All content Copyright (C) 2018 Genomics plc
#ifndef GENOTYPE_UTILS_HPP
#define GENOTYPE_UTILS_HPP

#include <algorithm>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils/matrix.hpp"
#include "variant/genotype.hpp"

namespace wecall
{
namespace caller
{
    namespace model
    {
        template < typename Sequence >
        void rescaleLogLikelihoods( Sequence & sequence, bool hasMultipleReads )
        {
            if ( hasMultipleReads )
            {
                const auto maxLogLikelihood = *std::max_element( sequence.begin(), sequence.end() );
                for ( auto it = sequence.begin(); it != sequence.end(); ++it )
                {
                    *it = log( std::max( std::numeric_limits< double >::min(), exp( *it - maxLogLikelihood ) ) );
                }
            }
            else
            {
                for ( auto it = sequence.begin(); it != sequence.end(); ++it )
                {
                    *it = 1.0;
                }
            }
        }

        std::vector< double > computeGenotypeLikelihoods( const variant::GenotypeVector & genotypes,
                                                          const utils::matrix_t & probReadsGivenHaplotypes,
                                                          const variant::HaplotypeVector & haplotypes );
    }
}
}

#endif