// std includes
#include <cmath>

// boost includes
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

// llvm includes
#include "llvm/Support/CFG.h"
#include "llvm/Support/Debug.h"

// hydra includes
#include "hydra/Analyses/Decider.h"
#include "hydra/Analyses/FunArgInfo.h"
#include "hydra/Analyses/Profitability.h"
#include "hydra/Support/FunAlgorithms.h"
#include "hydra/Support/PrintCollection.h"
#include "hydra/Support/TargetMacros.h"

using namespace llvm;
using namespace hydra;

//------------------------------------------------------------------------------
char Decider::ID = 0;

//------------------------------------------------------------------------------
void Decider::getAnalysisUsage(llvm::AnalysisUsage &Info) const {
  Info.setPreservesAll();
  Info.addRequired<Profitability>();
  Info.addRequired<FunArgInfo>();
}

using Graph = boost::adjacency_list<
    boost::listS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_weight_t, int>>;
using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
using VSizeType = boost::graph_traits<Graph>::vertices_size_type;
using Edge = std::pair<int, int>;
using bb_citer = BasicBlock::const_iterator;

//------------------------------------------------------------------------------
static int countInstructions(bb_citer first, bb_citer last,
                             const Profitability &Profit) {
  DEBUG(dbgs() << "countInstructions()\n");
  int count = std::accumulate(first, last, 0,
                              [&](int runningTotal, const Instruction &i) {
    int extraInsts{ 1 };
    if (auto *ci = dyn_cast<CallInst>(&i)) {
      extraInsts += Profit.getFunStats(*ci->getCalledFunction())->totalCost;
    }
    return runningTotal + extraInsts;
  });
  DEBUG(dbgs() << "Count is " << count << "\n");
  return count;
}

//FIXME: this function is horrible! try and make shorter... :)
//------------------------------------------------------------------------------
static int genGraph(const CallInst *spawn, const std::set<Instruction *> &joins,
                    const Profitability &Profit, Graph &out_graph,
                    std::set<int> &out_joinVertices) {
  DEBUG(dbgs() << "Decider::genGraph()\n");

  // early exit: if we have a trivial join, tell the caller
  if (joins.size() == 1u &&
      spawn->getParent() == (*joins.begin())->getParent() &&
      inOrder(spawn, *joins.begin())) {
    return -1;
  }

  std::vector<Edge> edges;
  std::vector<int> weights;
  int numNodes{ 0 };
  // each entryInst, terminatorInst and joinInst is associated with a vertex
  std::map<const Instruction *, int> vertices;
  std::map<const BasicBlock *, const Instruction *> joinBlocks;

  auto getOrAddVertex = [&](const Instruction *i) {
    auto vertexIter = vertices.find(i);
    if (vertexIter == vertices.end()) {
      vertices.emplace(i, numNodes);
      return numNodes++;
    } else {
      return vertexIter->second;
    }
  };

  for (const auto *join : joins) {
    joinBlocks.emplace(join->getParent(), join);
    out_joinVertices.insert(getOrAddVertex(join));
  }

  auto addEdgeWithWeight = [&](int v1, int v2, int w) {
    edges.emplace_back(v1, v2);
    weights.push_back(w);
  };

  auto *const spawnBlock = spawn->getParent();
  std::deque<const BasicBlock *> blocksToExplore;
  std::set<const BasicBlock *> exploredBlocks;

  int entry = getOrAddVertex(spawn);
  int terminator = getOrAddVertex(&*spawnBlock->end());

  auto addSuccBlock = [&](const BasicBlock *successorBlock) {
    addEdgeWithWeight(terminator, getOrAddVertex(&successorBlock->front()), 0);
    blocksToExplore.push_back(successorBlock);
  };

  addEdgeWithWeight(
      entry, terminator,
      countInstructions(bb_citer{ spawn }, spawnBlock->end(), Profit));
  std::for_each(succ_begin(spawnBlock), succ_end(spawnBlock), addSuccBlock);

  while (!blocksToExplore.empty()) {
    auto *currBlock = blocksToExplore.front();
    blocksToExplore.pop_front();

    // if already seen block, discard it
    if (exploredBlocks.count(currBlock) > 0) {
      continue;
    }

    // check if there's a join in this block
    auto joinIter = joinBlocks.find(currBlock);
    const bool joinInBlock{ joinIter != joinBlocks.end() };

    int entry = getOrAddVertex(&currBlock->front());
    terminator = joinInBlock ? getOrAddVertex(joinIter->second)
                             : getOrAddVertex(currBlock->getTerminator());
    int cost = joinInBlock
                   ? countInstructions(currBlock->begin(),
                                       bb_citer{ joinIter->second }, Profit)
                   : (currBlock == spawnBlock
                          ? countInstructions(currBlock->begin(),
                                              bb_citer{ spawn }, Profit)
                          : countInstructions(currBlock->begin(),
                                              currBlock->end(), Profit));
    addEdgeWithWeight(entry, terminator, cost);

    if (!joinInBlock) {
      std::for_each(succ_begin(currBlock), succ_end(currBlock), addSuccBlock);
    }
  }

  DEBUG(printCollection(weights, dbgs(), "Weights"));

  out_graph = Graph{ edges.begin(),   edges.end(),
                     weights.begin(), static_cast<VSizeType>(numNodes) };
  return entry;
}

namespace {
enum class Aggregator { min, arithmeticMean, max };
}

//------------------------------------------------------------------------------
static unsigned getSpawnToJoinCost(const CallInst *spawn,
                                   const std::set<Instruction *> &joins,
                                   Profitability &Profit, Aggregator aggr) {
  DEBUG(dbgs() << "Decider::getSpawnToJoinCost()\n");

  Graph g;
  std::set<int> joinVertices;
  int spawnVertex = genGraph(spawn, joins, Profit, g, joinVertices);

  // if the join is trivial, return the trivial cost
  if (spawnVertex < 0) {
    return countInstructions(++bb_citer{ spawn }, bb_citer{ *joins.begin() },
                             Profit);
  }

  std::vector<int> distances(num_vertices(g));

  boost::dijkstra_shortest_paths(g, spawnVertex,
                                 boost::distance_map(distances.data()));
  DEBUG(printCollection(distances, dbgs(), "Distances"));

  std::vector<unsigned> joinDistances;
  joinDistances.reserve(joinVertices.size());
  DEBUG(dbgs() << "signed distances: ");
  for (int v : joinVertices) {
    DEBUG(dbgs() << v << " has " << distances[v] << ", ");
    joinDistances.push_back(distances[v]);
  }
  DEBUG(dbgs() << "\n");

  // aggregate the joinDistances using the inputed method
  switch (aggr) {
  case Aggregator::min:
    return *std::min_element(joinDistances.begin(), joinDistances.end());

  case Aggregator::arithmeticMean: {
    DEBUG(printCollection(joinDistances, dbgs(), "Costs"));

    const double acc =
        std::accumulate(joinDistances.begin(), joinDistances.end(), 0.0);
    DEBUG(dbgs() << "total cost is " << acc << "\n");
    const double mean = acc / joinDistances.size();
    DEBUG(dbgs() << "mean cost is " << mean << "\n");
    const double rounded = round(mean);
    DEBUG(dbgs() << "rounded mean is " << rounded << "\n");
    const unsigned casted = static_cast<unsigned>(rounded);
    DEBUG(dbgs() << "casted mean is " << casted << "\n");
    return casted;
    /*
    return static_cast<unsigned>(
        round(std::accumulate(joinDistances.begin(), joinDistances.end(), 0.0) /
              joinDistances.size()));
    */
  }

  case Aggregator::max:
    return *std::max_element(joinDistances.begin(), joinDistances.end());
  }
}

#if KERNEL_THREADS
  static constexpr unsigned spawnCost = 1000u;
  static constexpr unsigned syncCost = 0u;
#elif LIGHT_THREADS
  static constexpr unsigned spawnCost = 100u;
  static constexpr unsigned syncCost = 0u;
#endif

//------------------------------------------------------------------------------
static bool
decide(const std::pair<const CallInst *, std::set<Instruction *>> &pair,
       FunArgInfo &FAI, Profitability &Profit) {
  DEBUG(dbgs() << "decide() for:\n");
  DEBUG(pair.first->print(dbgs()));
  DEBUG(dbgs() << "\nIn " << pair.first->getCalledFunction()->getName()
               << "()\n");

  auto *funStats = Profit.getFunStats(*pair.first->getCalledFunction());

  assert(funStats);

  const unsigned calleeInsts{ funStats->totalCost };
  DEBUG(dbgs() << "calleeInsts is " << calleeInsts << "\n");

  //TODO: uses arithmeticMean for now... make configurable later?
  const unsigned callerInsts{ getSpawnToJoinCost(
      pair.first, pair.second, Profit, Aggregator::arithmeticMean) };
  DEBUG(dbgs() << "callerInsts is " << callerInsts << "\n");

  const unsigned serialCost{ calleeInsts + callerInsts };
  const unsigned parallelCost{ spawnCost + std::max(calleeInsts, callerInsts) +
                               syncCost };

  DEBUG(dbgs() << "serialCost == " << serialCost << "\n");
  DEBUG(dbgs() << "parallelCost == " << parallelCost << "\n\n");

  const bool decision{ serialCost > parallelCost };
  if (decision) {
    // update the new total cost to reflect that the fun has been parallelised
    const unsigned costReduction{ serialCost - parallelCost };

    assert(Profit.getFunStats(*pair.first->getParent()->getParent()));
    assert(Profit.getFunStats(*pair.first->getParent()->getParent())
               ->totalCost > costReduction);

    Profit.getFunStats(*pair.first->getParent()->getParent())->totalCost -=
        costReduction;
  }
  return decision;
}

//------------------------------------------------------------------------------
bool Decider::runOnModule(llvm::Module &M) {
  DEBUG(dbgs() << "Decider::runOnModule()\n");

  auto &Profit = getAnalysis<Profitability>();
  auto &FAI = getAnalysis<FunArgInfo>();

  for (auto &pair : FAI) {
    if (decide(pair, FAI, Profit)) {
      profitableJoinPoints.insert(pair);
      funsToBeSpawned.insert(pair.first->getCalledFunction());
    }
  }

  return false;
}

//------------------------------------------------------------------------------
void Decider::releaseMemory() {
  funsToBeSpawned.clear();
  profitableJoinPoints.clear();
}

//------------------------------------------------------------------------------
void Decider::print(raw_ostream &O, const llvm::Module *) const {
  if (funsToBeSpawned.size() == 0u) {
    O << "No functions should be spawned.\n";
    return;
  }

  O << "The following function(s) should be spawned:\n\n";
  for (const auto F : funsToBeSpawned) {
    O << F->getName() << "()\n";
  }

  O << "\nThey should be spawned and synced here:\n\n";
  for (const auto &pair : profitableJoinPoints) {
    O << "Spawn:\t";
    pair.first->print(O);
    O << "\nSync:\t";
    for (const auto *i : pair.second) {
      i->print(O);
      O << "\n\t";
    }
    O << "\n\n";
  }
}

static RegisterPass<Decider>
X("decider", "Decide which functions to spawn", false, true);


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// 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 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE crypto3_marshalling_r1cs_gg_ppzksnark_test

#include <boost/test/unit_test.hpp>

#include <iostream>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>
#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/pairing/mnt4.hpp>
#include <nil/crypto3/algebra/pairing/mnt6.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>
#include <nil/crypto3/zk/snark/systems/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/marshalling/status_type.hpp>
#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/primary_input.hpp>
#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/proof.hpp>
#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/verification_key.hpp>

#include "detail/r1cs_examples.hpp"

using namespace nil::crypto3;
using namespace nil::crypto3::marshalling;
using namespace nil::crypto3::zk;

template<typename CurveType, typename Endianness>
bool test_r1cs_gg_ppzksnark() {

    std::size_t num_constraints = 1000, input_size = 100;

    typedef CurveType curve_type;
    typedef typename curve_type::scalar_field_type scalar_field_type;
    typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

    std::cout << "R1CS generation started." << std::endl;

    zk::snark::r1cs_example<scalar_field_type> example =
        zk::snark::generate_r1cs_example_with_binary_input<scalar_field_type>(num_constraints, input_size);

    std::cout << "R1CS generation finished." << std::endl;

    std::cout << "Starting generator" << std::endl;

    typename scheme_type::keypair_type keypair = zk::snark::generate<scheme_type>(example.constraint_system);

    std::cout << "Starting prover" << std::endl;

    const typename scheme_type::proof_type proof =
        zk::snark::prove<scheme_type>(keypair.first, example.primary_input, example.auxiliary_input);

    using verification_key_marshalling_type =
        types::r1cs_gg_ppzksnark_verification_key<nil::marshalling::field_type<Endianness>,
                                                  typename scheme_type::verification_key_type>;

    verification_key_marshalling_type filled_verification_key_val =
        types::fill_r1cs_gg_ppzksnark_verification_key<typename scheme_type::verification_key_type, Endianness>(
            keypair.second);

    using proof_marshalling_type =
        types::r1cs_gg_ppzksnark_proof<nil::marshalling::field_type<Endianness>, typename scheme_type::proof_type>;

    proof_marshalling_type filled_proof_val =
        types::fill_r1cs_gg_ppzksnark_proof<typename scheme_type::proof_type, Endianness>(proof);

    using primary_input_marshalling_type =
        types::r1cs_gg_ppzksnark_primary_input<nil::marshalling::field_type<Endianness>,
                                               typename scheme_type::primary_input_type>;

    primary_input_marshalling_type filled_primary_input_val =
        types::fill_r1cs_gg_ppzksnark_primary_input<typename scheme_type::primary_input_type, Endianness>(
            example.primary_input);

    std::cout << "Marshalling types filled." << std::endl;

    using unit_type = unsigned char;

    std::vector<unit_type> verification_key_byteblob;
    verification_key_byteblob.resize(filled_verification_key_val.length(), 0x00);
    auto write_iter = verification_key_byteblob.begin();

    typename nil::marshalling::status_type status =
        filled_verification_key_val.write(write_iter, verification_key_byteblob.size());

    std::vector<unit_type> proof_byteblob;
    proof_byteblob.resize(filled_proof_val.length(), 0x00);
    write_iter = proof_byteblob.begin();

    status = filled_proof_val.write(write_iter, proof_byteblob.size());

    std::vector<unit_type> primary_input_byteblob;

    primary_input_byteblob.resize(filled_primary_input_val.length(), 0x00);
    auto primary_input_write_iter = primary_input_byteblob.begin();

    status = filled_primary_input_val.write(primary_input_write_iter, primary_input_byteblob.size());

    std::cout << "Byteblobs filled." << std::endl;

    verification_key_marshalling_type val_verification_key_read;

    auto read_iter = verification_key_byteblob.begin();
    status = val_verification_key_read.read(read_iter, verification_key_byteblob.size());

    typename scheme_type::verification_key_type constructed_val_verification_key_read =
        types::make_r1cs_gg_ppzksnark_verification_key<typename scheme_type::verification_key_type, Endianness>(
            val_verification_key_read);

    proof_marshalling_type val_proof_read;

    read_iter = proof_byteblob.begin();
    status = val_proof_read.read(read_iter, proof_byteblob.size());

    typename scheme_type::proof_type constructed_val_proof_read =
        types::make_r1cs_gg_ppzksnark_proof<typename scheme_type::proof_type, Endianness>(val_proof_read);

    primary_input_marshalling_type val_primary_input_read;

    read_iter = primary_input_byteblob.begin();
    status = val_primary_input_read.read(read_iter, primary_input_byteblob.size());

    typename scheme_type::primary_input_type constructed_val_primary_input_read =
        types::make_r1cs_gg_ppzksnark_primary_input<typename scheme_type::primary_input_type, Endianness>(
            val_primary_input_read);

    bool ans = zk::snark::verify<scheme_type>(constructed_val_verification_key_read, constructed_val_primary_input_read,
                                              constructed_val_proof_read);

    return ans;
}

BOOST_AUTO_TEST_SUITE(r1cs_gg_ppzksnark_test_suite)

BOOST_AUTO_TEST_CASE(r1cs_gg_ppzksnark_bls12_381_be) {
    std::cout << "BLS12-381 r1cs_gg_ppzksnark big-endian test started" << std::endl;
    bool res =
        test_r1cs_gg_ppzksnark<nil::crypto3::algebra::curves::bls12<381>, nil::marshalling::option::big_endian>();
    BOOST_CHECK(res);
    std::cout << "BLS12-381 r1cs_gg_ppzksnark big-endian test finished" << std::endl;
}

// BOOST_AUTO_TEST_CASE(proof_bls12_381_le) {
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark proof little-endian test started" << std::endl;
//     test_proof<nil::crypto3::zk::snark::r1cs_gg_ppzksnark<nil::crypto3::algebra::curves::bls12<381>>,
//         nil::marshalling::option::little_endian>();
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark proof little-endian test finished" << std::endl;
// }

BOOST_AUTO_TEST_SUITE_END()


//  test_duration.cpp  ----------------------------------------------------------//

//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#if !defined(__GNUC__)

#define min(A,B) ((A)<(B)?(A):(B))
#define max(A,B) ((A)>(B)?(A):(B))

#include <boost/chrono/chrono.hpp>

#endif


/**
 * \ file GainSwellFilter.cpp
 */

#include <ATK/Dynamic/GainSwellFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (64)

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainSwellFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_0_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainSwellFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1); // if input is zero, we still need a gain of 1 to have a progression of 1 for values < threshold
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_threshold_05_ratio_2_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainSwellFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.490775019, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_threshold_05_ratio_4_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::GainSwellFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.240860119, outdata[i], 0.1);
  }
}


#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <boost/timer.hpp>
#include <algorithm>

#include "core/Config.h"
#include "core/Feature.h"
#include "core/Featuredetection.h"
#include "core/Tracker.h"
#include "io/Zed_svo_wrapper.h"
#include "utils/SLAMTools.h"

using namespace std;

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: test_feature_tracking config_file svo_file"<<endl;
        return -1;
    }

    string path_to_config ( argv[1] );
    string svo_file ( argv[2] );

    zed_slam::Config::init ( path_to_config );
    zed_slam::Zed_svo_wrapper* svo_wrapper = new zed_slam::Zed_svo_wrapper ( svo_file );

    zed_slam::Frame::Ptr frame1 = svo_wrapper->next();
    zed_slam::FrameDrawer viewer;
    if ( frame1 == nullptr )
        return -1;

    cout<<"start to detect features"<<endl;
    zed_slam::FeatureDetection featureDetection;
    boost::timer timer;
    featureDetection.detectFAST ( frame1 );
    cout<<"Detecting fast cost time "<<timer.elapsed() <<endl;
    cout<<"total features: "<<frame1->features_.size() <<endl;

    // track these features in frame 2
    int loop=1;
    while ( 1 )
    {
        cout<<"********* loop "<<loop<<" **********"<<endl;
        zed_slam::Frame::Ptr frame2 = svo_wrapper->next();
        if ( frame2 == nullptr )
            break;
        zed_slam::Tracker tracker;

        boost::timer timer;
        tracker.trackKLT ( frame1, frame2 );
        cout<<"trackKLT uses time "<<timer.elapsed() <<endl;

        // estimate the pose of frame 2
        boost::timer timer_pose;
        tracker.poseEstimationTwoFrames ( frame1, frame2 );
        cout<<"frame 2 pose = "<<endl<<frame2->T_f_w_.matrix()<<endl;
        cout<<"pose estimation cost time="<<timer_pose.elapsed() <<endl;

        if ( frame2->features_.size() <= zed_slam::Config::get<int> ( "min_features" ) )
        {
            // add new features into frame 2
            cout<<"add new features"<<endl;
            featureDetection.detectFAST ( frame2 );
        }
        cout<<"draw features"<<endl;
        zed_slam::SLAMTools::drawFeatures ( frame2 );
        cv::waitKey ( 1 );
        frame1 = frame2;

        loop++;

        viewer.addFrame( frame2 );
    }

    viewer.stop();
    return 0;
}


#include <geometry_msgs/PoseStamped.h>
#include <octomap/octomap.h>
#include <octomap_msgs/conversions.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <stl_aeplanner_msgs/LTLStats.h>
#include <stl_aeplanner_msgs/Volume.h>
#include <tf2/utils.h>

#include <boost/filesystem.hpp>
#include <fstream>
#include <iostream>

std::ofstream stats_file;
std::ofstream pose_file;
std::ofstream map_file;

double total_volume;
octomap::point3d bbx_min;
octomap::point3d bbx_max;

ros::Publisher volume_pub;
double volume_scaler;

double volume_complete;
double time_limit;

bool stats_done;
bool pose_done;
bool map_done;

ros::Time start;

void LTLStatsCallback(const stl_aeplanner_msgs::LTLStats::ConstPtr& stats)
{
  if (stats_done || pose_done || map_done)
  {
    exit(0);
  }

  if (stats_done || pose_done || map_done)
  {
    stats_file.close();
    stats_done = true;
    return;
  }

  if (start.is_zero())
  {
    start = ros::Time(stats->header.stamp);
  }

  if ((ros::Time::now() - start).toSec() > time_limit)
  {
    stats_file.close();
    stats_done = true;
    return;
  }

  if (stats_file.is_open())
  {
    stats_file << (stats->header.stamp - start).toSec() << ", ";
    stats_file << stats->ltl_min_distance << ", ";
    stats_file << stats->ltl_max_distance << ", ";
    stats_file << stats->current_closest_distance << ", ";
    stats_file << stats->mean_closest_distance << ", ";
    stats_file << stats->closest_router_distance << ", ";
    stats_file << stats->ltl_min_altitude << ", ";
    stats_file << stats->ltl_max_altitude << ", ";
    stats_file << stats->current_closest_altitude << ", ";
    stats_file << stats->mean_closest_altitude << "\n";
    stats_file.flush();
  }
}

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
  if (stats_done || pose_done || map_done)
  {
    exit(0);
  }

  if (stats_done || pose_done || map_done)
  {
    pose_file.close();
    pose_done = true;
    return;
  }

  if (start.is_zero())
  {
    start = ros::Time(pose->header.stamp);
  }

  if ((ros::Time::now() - start).toSec() > time_limit)
  {
    pose_file.close();
    pose_done = true;
    return;
  }

  if (pose_file.is_open())
  {
    pose_file << (pose->header.stamp - start).toSec() << ", ";
    pose_file << pose->pose.position.x << ", ";
    pose_file << pose->pose.position.y << ", ";
    pose_file << pose->pose.position.z << ", ";
    pose_file << tf2::getYaw(pose->pose.orientation) << "\n";
    pose_file.flush();
  }
}

void mapCallback(const octomap_msgs::Octomap::ConstPtr& map)
{
  if (stats_done || pose_done || map_done)
  {
    exit(0);
  }

  if (stats_done || pose_done || map_done)
  {
    map_file.close();
    map_done = true;
    return;
  }

  if (start.is_zero())
  {
    start = ros::Time(map->header.stamp);
  }

  if ((ros::Time::now() - start).toSec() > time_limit)
  {
    map_file.close();
    map_done = true;
    return;
  }

  if (map_file.is_open())
  {
    octomap::OcTree* ot = (octomap::OcTree*)octomap_msgs::msgToMap(*map);

    double res = ot->getResolution();
    double cell_size = std::pow(res, 3.0);

    // ot->expand();

    // double current_volume = 0;
    // for (auto it = ot->begin_leafs_bbx(bbx_min, bbx_max), it_end = ot->end_leafs_bbx();
    // it != it_end; ++it)
    // {
    //   current_volume += cell_size;
    // }

    double volume_unmapped = 0;
    double volume_occupied = 0;
    double volume_free = 0;
    double current_volume = 0;
    for (double x = bbx_min.x(); x < bbx_max.x() - res / 2; x += res)
    {
      for (double y = bbx_min.y(); y < bbx_max.y() - res / 2; y += res)
      {
        for (double z = bbx_min.z(); z < bbx_max.z() - res / 2; z += res)
        {
          octomap::OcTreeNode* result = ot->search(x + res / 2, y + res / 2, z + res / 2);
          if (!result)
          {
            volume_unmapped += cell_size;
          }
          else if (result->getLogOdds() > 0)
          {
            current_volume += cell_size;
          }
          else
          {
            current_volume += cell_size;
          }
        }
      }
    }

    delete ot;

    map_file << (map->header.stamp - start).toSec() << ", ";
    map_file << total_volume << ", ";
    map_file << current_volume << ", ";
    map_file << (current_volume / total_volume) << "\n";
    map_file.flush();

    stl_aeplanner_msgs::Volume msg;
    msg.header.stamp = map->header.stamp;
    msg.current_volume = current_volume * volume_scaler / total_volume;
    volume_pub.publish(msg);

    if (msg.current_volume >= volume_complete)
    {
      map_done = true;
      map_file.close();
    }
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "stl_evaluation");

  ros::NodeHandle nh;
  ros::NodeHandle nh_priv("~");
  std::string name = "";
  if (!nh_priv.getParam("name", name))
  {
    ROS_WARN("No name value specified");
  }

  std::string package_path = ros::package::getPath("stl_evaluation");

  int postfix = 0;
  while (boost::filesystem::exists(package_path + "/data/" + name + "_" + std::to_string(postfix)))
  {
    ++postfix;
  }

  boost::filesystem::path p(package_path + "/data/" + name + "_" + std::to_string(postfix));
  boost::filesystem::create_directories(p);

  std::vector<double> boundary_min;
  std::vector<double> boundary_max;
  if (!nh_priv.getParam("boundary/min", boundary_min))
  {
    ROS_WARN("No boundary/min value specified");
  }
  if (!nh_priv.getParam("boundary/max", boundary_max))
  {
    ROS_WARN("No boundary/max value specified");
  }

  stats_done = false;
  pose_done = false;
  map_done = false;
  volume_complete = nh_priv.param<double>("volume_complete", 0.0);
  time_limit = nh_priv.param<double>("time_limit", 0.0);

  std::ofstream info_file;
  info_file.open(package_path + "/data/" + name + "_" + std::to_string(postfix) + "/" + "info.txt");
  info_file << "name: " << nh_priv.param<std::string>("name", "") << "\n";
  info_file << "volume_complete: " << volume_complete << "\n";
  info_file << "time_limit: " << time_limit << "\n";
  info_file << "\n";
  info_file << "camera/horizontal_fov: " << nh_priv.param<double>("camera/horizontal_fov", 0.0) << "\n";
  info_file << "camera/vertical_fov: " << nh_priv.param<double>("camera/vertical_fov", 0.0) << "\n";
  info_file << "\n";
  info_file << "raycast/dr: " << nh_priv.param<double>("raycast/dr", 0.0) << "\n";
  info_file << "raycast/dphi: " << nh_priv.param<double>("raycast/dphi", 0.0) << "\n";
  info_file << "raycast/dtheta: " << nh_priv.param<double>("raycast/dtheta", 0.0) << "\n";
  info_file << "\n";
  info_file << "system/bbx/r: " << nh_priv.param<double>("system/bbx/r", 0.0) << "\n";
  info_file << "system/bbx/overshoot: " << nh_priv.param<double>("system/bbx/overshoot", 0.0) << "\n";
  info_file << "\n";
  info_file << "aep/gain/r_min: " << nh_priv.param<double>("aep/gain/r_min", 0.0) << "\n";
  info_file << "aep/gain/r_max: " << nh_priv.param<double>("aep/gain/r_max", 0.0) << "\n";
  info_file << "aep/gain/zero: " << nh_priv.param<double>("aep/gain/zero", 0.0) << "\n";
  info_file << "aep/gain/lambda: " << nh_priv.param<double>("aep/gain/lambda", 0.0) << "\n";
  info_file << "aep/gain/sigma_thresh: " << nh_priv.param<double>("aep/gain/sigma_thresh", 0.0) << "\n";
  info_file << "aep/tree/extension_range: " << nh_priv.param<double>("aep/tree/extension_range", 0.0) << "\n";
  info_file << "aep/tree/max_sampling_radius: " << nh_priv.param<double>("aep/tree/max_sampling_radius", 0.0) << "\n";
  info_file << "aep/tree/initial_iterations: " << nh_priv.param<double>("aep/tree/initial_iterations", 0.0) << "\n";
  info_file << "aep/tree/cutoff_iterations: " << nh_priv.param<double>("aep/tree/cutoff_iterations", 0.0) << "\n";
  info_file << "\n";
  info_file << "rrt/min_nodes: " << nh_priv.param<double>("rrt/min_nodes", 0.0) << "\n";
  info_file << "rrt/max_nodes: " << nh_priv.param<double>("rrt/max_nodes", 0.0) << "\n";
  info_file << "\n";
  info_file << "visualize_rays: " << nh_priv.param<bool>("visualize_rays", false) << "\n";
  info_file << "visualize_tree: " << nh_priv.param<bool>("visualize_tree", false) << "\n";
  info_file << "visualize_exploration_area: " << nh_priv.param<bool>("visualize_exploration_area", false) << "\n";
  info_file << "\n";
  info_file << "robot_frame: " << nh_priv.param<std::string>("robot_frame", "") << "\n";
  info_file << "world_frame: " << nh_priv.param<std::string>("world_frame", "") << "\n";
  info_file << "\n";
  info_file << "boundary/min: [ " << boundary_min[0] << ", " << boundary_min[1] << ", " << boundary_min[2] << " ]\n";
  info_file << "boundary/max: [ " << boundary_max[0] << ", " << boundary_max[1] << ", " << boundary_max[2] << " ]\n";
  info_file << "\n";
  info_file << "# STL STUFF BELOW\n";
  info_file << "lambda: " << nh_priv.param<double>("lambda", 0.0) << "\n";
  info_file << "min_distance: " << nh_priv.param<double>("min_distance", 0.0) << "\n";
  info_file << "max_distance: " << nh_priv.param<double>("max_distance", 0.0) << "\n";
  info_file << "min_distance_active: " << nh_priv.param<bool>("min_distance_active", false) << "\n";
  info_file << "max_distance_active: " << nh_priv.param<bool>("max_distance_active", false) << "\n";
  info_file << "routers_active: " << nh_priv.param<bool>("routers_active", false) << "\n";
  info_file << "distance_add_path: " << nh_priv.param<double>("distance_add_path", 0.0) << "\n";
  info_file << "max_search_distance: " << nh_priv.param<double>("max_search_distance", 0.0) << "\n";
  info_file << "step_size: " << nh_priv.param<double>("step_size", 0.0) << "\n";
  info_file << "min_altitude: " << nh_priv.param<double>("min_altitude", 0.0) << "\n";
  info_file << "max_altitude: " << nh_priv.param<double>("max_altitude", 0.0) << "\n";
  info_file << "min_altitude_active: " << nh_priv.param<bool>("min_altitude_active", false) << "\n";
  info_file << "max_altitude_active: " << nh_priv.param<bool>("max_altitude_active", false) << "\n";
  info_file.close();

  stats_file.open(package_path + "/data/" + name + "_" + std::to_string(postfix) + "/" + "stats.txt");
  stats_file << "Stamp, Min, Max, Current closest distance, Mean closest distance, "
                "Current closest router distance, Min altitude, Max altitude, Current closest altitude, Mean closest "
                "altitude\n";

  pose_file.open(package_path + "/data/" + name + "_" + std::to_string(postfix) + "/" + "pose.txt");
  pose_file << "Stamp, X, Y, Z, Yaw\n";

  map_file.open(package_path + "/data/" + name + "_" + std::to_string(postfix) + "/" + "map.txt");
  map_file << "Stamp, Max volume, Current volume, Procentage\n";

  bbx_min.x() = boundary_min[0];
  bbx_min.y() = boundary_min[1];
  bbx_min.z() = boundary_min[2];

  bbx_max.x() = boundary_max[0];
  bbx_max.y() = boundary_max[1];
  bbx_max.z() = boundary_max[2];

  volume_scaler = nh_priv.param("volume_scaler", 1.0);

  octomap::point3d temp = bbx_max - bbx_min;
  total_volume = temp.x() * temp.y() * temp.z();

  start = ros::Time(0);

  ros::Subscriber ltl_sub = nh.subscribe("/stl_aeplanner/ltl_stats", 1, &LTLStatsCallback);
  ros::Subscriber pose_sub = nh.subscribe("/mavros/local_position/pose", 1, &poseCallback);
  ros::Subscriber map_sub = nh.subscribe("/stl_aeplanner/octomap_full", 1, &mapCallback);

  volume_pub = nh.advertise<stl_aeplanner_msgs::Volume>("/stl_evaluation/volume", 1);

  ros::spin();

  stats_file.close();
  pose_file.close();
  map_file.close();

  return 0;
}


/*
  Copyright ©2013 The Regents of the University of California
  (Regents). All Rights Reserved. Permission to use, copy, modify, and
  distribute this software and its documentation for educational,
  research, and not-for-profit purposes, without fee and without a
  signed licensing agreement, is hereby granted, provided that the
  above copyright notice, this paragraph and the following two
  paragraphs appear in all copies, modifications, and
  distributions. Contact The Office of Technology Licensing, UC
  Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620,
  (510) 643-7201, for commercial licensing opportunities.

  IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT,
  INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING
  LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
  DOCUMENTATION, EVEN IF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY
  OF SUCH DAMAGE.

  REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  FOR A PARTICULAR PURPOSE. THE SOFTWARE AND ACCOMPANYING
  DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS
  IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT,
  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
*/

#ifndef MESH_HPP
#define MESH_HPP
#include <utility>
#include <Eigen/Core>
#include "vectors.hpp"
#include <vector>

struct Node {
    int label;
    Vec3 x, x0; // position, old (collision-free) position,
    int index; // position in mesh.nodes
    Node () {}
    explicit Node (const Vec3 &x):
        x(x), x0(x) {}
};

struct Face {
    Node* n[3]; // verts
    int index; // position in mesh.faces
    Face () {}
    explicit Face (Node *vert0, Node *vert1, Node *vert2){
        n[0] = vert0;
        n[1] = vert1;
        n[2] = vert2;
    }
};

struct ArcsimMesh {
    std::vector<Node*> nodes;
    std::vector<Face*> faces;

    void Initialize(const std::vector<Eigen::Vector3d> &pos,
        const std::vector<Eigen::Vector3d> &pos0,
        const std::vector<Eigen::Vector3i> &fcs) {
        for (int i = 0; i < pos.size(); ++i) {
            Vec3 tmp(pos[i][0],pos[i][1],pos[i][2]);
            nodes.push_back(new Node(tmp));
            nodes.back()->x0 = Vec3(pos0[i][0],pos0[i][1],pos0[i][2]);
            nodes.back()->index = i;
        }
        for (int i = 0; i < fcs.size(); ++i) {
            const Eigen::Vector3i &face = fcs[i];
            faces.push_back(new Face(nodes[face[0]], nodes[face[1]], nodes[face[2]]));
            faces.back()->index = i;
        }
    }
};

#endif


// Copyright (c) 2016 University of Minnesota
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "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 UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef COLLISION_SHAPE_HPP
#define COLLISION_SHAPE_HPP

#include <Eigen/Dense>

namespace admm{

class CollisionShape {

	public:

		CollisionShape(Eigen::Vector3d shapeCenter) { center = shapeCenter; }
		virtual ~CollisionShape() {}

		// Negative signed distance:
		// postive if inside, zero on surface, negative (distance to surface) if outside
		virtual double isColliding(Eigen::Vector3d pos) const = 0;

		virtual Eigen::Vector3d projectOut(const Eigen::Vector3d currPos) const = 0;

		Eigen::Vector3d center;

	protected:

};

} // end of namespace admm

#endif


/*
 ============================================================================
 Name        : INSIGHTv3_mpi.cpp
 Author      : Jan Mikelson
 Version     :
 Copyright   : 	This free software is available under the Creative Commons Attribution Share Alike License.
 You are permitted to use, redistribute and adapt this software as long as appropriate credit
 is given to the original author, and all derivative works are distributed under the same
 license or a compatible one.
 For more information, visit http://creativecommons.org/licenses/by-sa/3.0/ or send a letter to
 Creative Commons, 171 2nd Street, Suite 300, San Francisco, California, 94105, USA.
 Description : 	Parallel version of the INSIGHT algorithm. Further details can be found in Lillacci & Khammash 2013
 ============================================================================
 */

#include <ctime>
#include <vector>

#include <stdio.h>  /* defines FILENAME_MAX */
#ifdef WINDOWS
#include <direct.h>
#define GetCurrentDir _getcwd
#else
#include <unistd.h>
#define GetCurrentDir getcwd
#endif

char cCurrentPath[FILENAME_MAX];

#include <boost/program_options.hpp>
#include <boost/smart_ptr/make_shared.hpp>
#include <boost/ref.hpp>
#include <Eigen/Dense>
#include <mpi.h>

#include "INSIGHTv3.h"
#include "BirthDeathModel.h"
#include "InsightAlgorithm.h"
#include "InsightParticleSamplerFactory.h"
#include "IllegalArgumentException.h"
#include "ParticleEvaluatorInsight.h"
#include "UniformSamplerFactory.h"
#include "ScaledUniformSamplerFactory.h"
#include "ProblemFileReader.h"
#include "ModelDescription.h"
#include "UniformSampler.h"
#include "InsightIterationMpi.h"
#include "InsightMpiWorker.h"
#include "PrevPopReader.h"

using namespace INSIGHTv3;

//Default values
const static std::string PROBLEM_FILE = "";
const static int S = 3;
const static double FINAL_TOLERANCE = 0.03;
const static int NUM_PARTICLES = 1000;
const static std::string OUTPUT_FILE = "";
const static std::string PREVIOUS_POPULATION_FILE = "";
const static bool IGNORE_WEIGHTS = false;
const static int PRINT = 20;
const static double BETA = 0.01;
const static double TOL_KOLMOGOROV = 0.0000001;
const static double KAPPA_KOLMOGOROV = 0.005224; // kappa value for M = 80000 and beta = 0.01
const static double TOL_KAPPA_KOLMOGOROV = 0.01;

std::string problem_file_name;
int s;
double final_tolerance;
int num_particles;
std::string output_file_name;
std::string prev_pop_file;
bool ignore_weights;
int print;
double beta;
double tolerance_kolmogorov;
double kappa_kolmogorov;
double tol_kappa_kolmovorog;

namespace po = boost::program_options;

void handleOptions(int argc, char * argv[]);
void printInfo(ModelDescription& desc);

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

	handleOptions(argc, argv);

	clock_t tic = clock();
	MPI::Init(argc, argv);
	int my_rank = MPI::COMM_WORLD.Get_rank();
	int num_tasks = MPI::COMM_WORLD.Get_size();

	if (num_tasks == 1) {
		std::cerr
				<< "When using mpi version of INSIGHT at least two processes must be started!"
				<< std::endl;
		abort();
	}

	int seed = time(NULL) + my_rank;
#ifdef DEBUG
	seed = 2;
#endif
	RngPtr rng = boost::make_shared<RandomNumberGenerator>(seed);

	ProblemFileReader reader;
	ModelDescription desc = reader.read(rng, problem_file_name);

	KolmogorovComputerPtr kolmogorov_computer = boost::make_shared<
			KolmogorovComputer>(*desc.original_data, beta, tolerance_kolmogorov,
			kappa_kolmogorov);
	evaluator_ptr evaluator = boost::make_shared<ParticleEvaluatorInsight>(
			boost::ref(*desc.model), *desc.times, boost::ref(rng),
			kolmogorov_computer); // need to use boost::ref, see boost
	sampler_ptr prior_sampler = boost::make_shared<UniformSampler>(
			boost::ref(rng), *desc.lower_bounds_parameter,
			*desc.upper_bounds_parameter);

	if (my_rank == 0) {
		printInfo(desc);
		writer_ptr writer = boost::make_shared<InsightOutputWriter>(
				output_file_name);

		sampler_factory_ptr factory = boost::make_shared<
				ScaledUniformSamplerFactory>(boost::ref(rng),
				desc.model->num_params, *desc.lower_bounds_parameter,
				*desc.upper_bounds_parameter, ignore_weights);

		InsightIterationMpi iteration(num_tasks, prior_sampler);
		ToleranceProviderPtr tolerance_provider = boost::make_shared<
				FixedSequenceToleranceProvider>(s, desc.model->model,
				kolmogorov_computer);

		sampler_ptr first_sampler;
		std::vector<double> prev_acceptance_rates;
		std::vector<double> prev_tolerances;
		std::vector<int> prev_num_simulations;
		if (prev_pop_file.length() > 0) {
			PrevPopReader prev_pop_reader;
			ParticleSet particle_set;
			prev_pop_reader.readPreviousPop(prev_pop_file,
					desc.model->num_params, &particle_set,
					&prev_acceptance_rates, &prev_tolerances,
					&prev_num_simulations);
			first_sampler = factory->createSampler(particle_set,
					kolmogorov_computer->getThresholdForS(s), s);
		} else {
			first_sampler = prior_sampler;
		}
		InsightAlgorithm algorithm(iteration, factory, evaluator, writer,
				tolerance_provider, final_tolerance, num_particles,
				desc.model->num_params, prev_acceptance_rates, prev_tolerances,
				prev_num_simulations);
		IterationLogger logger(print);

		algorithm.run(first_sampler, logger);

		MPI_INSTRUCTION instruction(DIETAG);
		for (int rank = 1; rank < num_tasks; rank++) {
			MPI::COMM_WORLD.Send(&instruction, 1, MPI::INT, rank, INSTRUCTION);
		}
	} else {
		InsightMpiWorker worker(my_rank, desc.model->num_params, evaluator,
				prior_sampler);
		worker.run();
	}

	clock_t toc = clock();
	if (my_rank == 0) {
		std::cout << "\n\nSMC inference complete." << std::endl;
		std::cout << "\nElapsed time: "
				<< ((double) (toc - tic)) / CLOCKS_PER_SEC << "sec.\n"
				<< std::endl;
	}
	MPI::Finalize();

	return 0;
}

void handleOptions(int argc, char * argv[]) {

	po::options_description desc("Allowed options");
	desc.add_options()("help", "produce help message")("problem_file",
			po::value<std::string>(&problem_file_name)->default_value(
					PROBLEM_FILE),
			"problem file. A problem file must always be provided!")(
			"number_simulations,s", po::value<int>(&s)->default_value(S),
			"starting number of simulations")("final tolerance,e",
			po::value<double>(&final_tolerance)->default_value(FINAL_TOLERANCE),
			"the final tolerance for the algorithm")("pop_size,n",
			po::value<int>(&num_particles)->default_value(NUM_PARTICLES),
			"define the population size")("output_file,O",
			po::value<std::string>(&output_file_name)->default_value(
					OUTPUT_FILE), "output file")("previous_pop_file,P",
			po::value<std::string>(&prev_pop_file)->default_value(
					PREVIOUS_POPULATION_FILE), "previous population file")(
			"ignore_weights,i",
			po::value<bool>(&ignore_weights)->default_value(IGNORE_WEIGHTS),
			"ignore weights")("print,p",
			po::value<int>(&print)->default_value(PRINT),
			"define the frequency of print outs")("beta,b",
			po::value<double>(&beta)->default_value(BETA),
			"the confidence of the kolmogorov test for the rejection rule")(
			"tol_kol,k",
			po::value<double>(&tolerance_kolmogorov)->default_value(
					TOL_KOLMOGOROV),
			"the tolerance for the algorithm to compute the inverse of the kolmogorov distribution")(
			"kappa,K",
			po::value<double>(&kappa_kolmogorov)->default_value(
					KAPPA_KOLMOGOROV),
			"Kappa for the computation of the Kolmogorov distance")(
			"tol_kol_kap,T",
			po::value<double>(&tol_kappa_kolmovorog)->default_value(
					TOL_KAPPA_KOLMOGOROV),
			"the tolerance for the algorithm to compute kappa for the computation of the Kolmogorov distance");

	po::positional_options_description p;
	p.add("problem_file", -1);

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::store(
			po::command_line_parser(argc, argv).options(desc).positional(p).run(),
			vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		abort();
	}

	if (problem_file_name.size() == 0) {
		std::cerr << "Problem file is required" << std::endl;
		abort();
	}

	if (output_file_name.size() == 0) {
		GetCurrentDir(cCurrentPath, sizeof(cCurrentPath));
		output_file_name = std::string(cCurrentPath) + "/resutls.txt";
	}

}

void printInfo(ModelDescription& desc) {

	std::cout << "\nThis is INSIGHT v3.0\n" << std::endl;
	std::cout << "Problem file path:" << std::endl;
	std::cout << problem_file_name << std::endl;
	std::cout << "\nThe new particle population will be saved in:" << std::endl;
	std::cout << output_file_name << std::endl;

	std::cout << "\nWill estimate " << desc.model->model_name << " using "
			<< num_particles << " SMC particles." << std::endl;
	std::cout << "\nThe data is assumed to have been created using "
			<< desc.model->num_outputs << " outputs," << std::endl;
	std::cout << "measured at " << desc.times->size() << " timepoints."
			<< std::endl;

	if (prev_pop_file.length() > 0) {
		std::cout << "A previous population is loaded from file: " << std::endl;
		std::cout << prev_pop_file << std::endl;
	}
	std::cout << "To estimate the density of each population "
			<< "Kernel Density Estimation with uniform Kernel" << std::endl;
	std::cout << "is used." << std::endl;
	if (ignore_weights) {
		std::cout << std::endl << "Particle weights will be ignored by sampler!"
				<< std::endl;
	}
	std::cout << "\nThe parallel version of the algorithm is used."
			<< std::endl;
}


// --------------------------------------------------------------
//
// Utilities for hashing nodes.
//
// --------------------------------------------------------------
#ifndef __AST_HASH_HPP_DEFINED__
#define __AST_HASH_HPP_DEFINED__

#include <boost/functional/hash.hpp>

#include <iostream>
#include <string>
#include <vector>
#include <memory>

#include <z3++.h>
#include <assert.h>
#include <type.hpp>

#include <boost/shared_ptr.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/python.hpp>

#include <ast.hpp>

namespace ila
{
    std::size_t hash_value(const Node& n);
    std::size_t hash_value(const NodeType & ntype);
    std::size_t compute_hash_value(const NodeType & ntype);

    template<typename T>
    void choice_hash_combine(std::size_t& seed, const ChoiceExpr<T>* c)
    {
        const ReadSlice* rs = dynamic_cast<const ReadSlice*>(c);
        const WriteSlice* ws = dynamic_cast<const WriteSlice*>(c);

        if (rs != NULL) {
            boost::hash_combine(seed, 103002410);
            boost::hash_combine(seed, rs->bitvec.get());
            boost::hash_combine(seed, rs->width);
            boost::hash_combine(seed, rs->increment);
        } else if (ws != NULL) {
            boost::hash_combine(seed, 1505753267);
            boost::hash_combine(seed, ws->bitvec.get());
            boost::hash_combine(seed, ws->data.get());
            boost::hash_combine(seed, ws->increment);
        }
        boost::hash_combine(seed, c->getName());
    }

    template<class T> struct NodeHash;
    template<> struct NodeHash<const Node*>
    {
        size_t operator()(const Node* n) const
        {
            return hash_value(*n);
        }
    };

    template<class T> struct NodeEqual;
    template<> struct NodeEqual<const Node*>
    {
        bool operator()(const Node* l, const Node* r) const
        {
            return l->equal(r);
        }
    };
}

#endif


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "DoseMapHelper.h"

#include <boost/make_shared.hpp>

#include "mapRegistrationFileReader.h"

#include "rttbITKImageAccessorConverter.h"
#include "rttbSimpleMappableDoseAccessor.h"
#include "rttbMatchPointTransformation.h"
#include "rttbLinearInterpolation.h"
#include "rttbNearestNeighborInterpolation.h"
#include "rttbRosuMappableDoseAccessor.h"
#include "rttbArithmetic.h"
#include "rttbBinaryFunctorAccessor.h"
#include "rttbExceptionMacros.h"
#include "rttbImageWriter.h"

rttb::apps::doseMap::ApplicationData::RegistrationType::Pointer
rttb::apps::doseMap::loadRegistration(const std::string& fileName)
{
	map::io::RegistrationFileReader::Pointer spRegReader = map::io::RegistrationFileReader::New();

	map::io::RegistrationFileReader::LoadedRegistrationPointer spReg;

	std::cout << std::endl << "read registration file... ";
	spReg = spRegReader->read(fileName);
	std::cout << "done." << std::endl;

	ApplicationData::RegistrationType::Pointer resultPtr =
	    dynamic_cast<ApplicationData::RegistrationType*>(spReg.GetPointer());

	if (resultPtr.IsNull())
	{
		mapDefaultExceptionStaticMacro( <<
		                                "Loaded registration cannot be used. Only 3D 3D registrations are allowed.");
	}

	return resultPtr;
};

/**Private helper function for processData(). Generates a suitable output accessor
 * (depending on the configuration in appData a suitable accessor pipeline is established)
 * which performs the accumulation of the doses and returns the output.to */
rttb::core::DoseAccessorInterface::Pointer
assembleOutputAccessor(rttb::apps::doseMap::ApplicationData& appData)
{

	rttb::core::DoseAccessorInterface::Pointer outputAccessor = appData._inputDose;

  auto transform = boost::make_shared<rttb::interpolation::MatchPointTransformation>(appData._spReg);

	if (appData._interpolatorName == "rosu")
	{
		outputAccessor = boost::make_shared<rttb::interpolation::RosuMappableDoseAccessor>(appData._refDose->getGeometricInfo(),
		                             appData._inputDose, transform);
	}
	else
	{
    rttb::interpolation::InterpolationBase::Pointer interpolate = boost::make_shared<rttb::interpolation::LinearInterpolation>();

		if (appData._interpolatorName == "nn")
		{
			interpolate = boost::make_shared<rttb::interpolation::NearestNeighborInterpolation>();
		}
		else if (appData._interpolatorName != "linear")
		{
			mapDefaultExceptionStaticMacro( <<
			                                "Unkown interpolation type selected. Cannot map dose. Interpolation type: " <<
			                                appData._interpolatorName);
		}

    outputAccessor = boost::make_shared<rttb::interpolation::SimpleMappableDoseAccessor>(appData._refDose->getGeometricInfo(),
      appData._inputDose, transform, interpolate);
	}

	return outputAccessor;
}

void
rttb::apps::doseMap::processData(rttb::apps::doseMap::ApplicationData& appData)
{
	rttb::core::DoseAccessorInterface::Pointer outputAccessor = assembleOutputAccessor(
	            appData);

	std::cout << std::endl << "generate output image... ";
	io::itk::ITKImageAccessorConverter converter(outputAccessor);
	converter.setFailOnInvalidIDs(true);
	converter.process();
	io::itk::ITKImageAccessorConverter::ITKImageType::Pointer itkImage = converter.getITKImage();
	std::cout << "done." << std::endl;

	std::cout << std::endl << "write output image... ";
	io::itk::ImageWriter writer(appData._outputFileName, itkImage.GetPointer());
	writer.writeFile();

	std::cout << "done." << std::endl;
};


#include "Poisson.h"
#include "config.h"
#include "kernels/poisson/init.h"
#include "kernels/poisson/kernel.h"
#include "kernels/poisson/tensor.h"

#include "basis/WarpAndBlend.h"
#include "form/BC.h"
#include "form/DGCurvilinearCommon.h"
#include "form/InverseInequality.h"
#include "form/RefElement.h"
#include "geometry/Curvilinear.h"
#include "quadrules/SimplexQuadratureRule.h"
#include "tensor/EigenMap.h"
#include "util/LinearAllocator.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <cassert>

namespace tensor = tndm::poisson::tensor;
namespace init = tndm::poisson::init;
namespace kernel = tndm::poisson::kernel;

namespace tndm {

Poisson::Poisson(std::shared_ptr<Curvilinear<DomainDimension>> cl, functional_t<1> K,
                 DGMethod method)
    : DGCurvilinearCommon<DomainDimension>(std::move(cl), MinQuadOrder()), method_(method),
      space_(PolynomialDegree, ALIGNMENT),
      materialSpace_(PolynomialDegree, WarpAndBlendFactory<DomainDimension>(), ALIGNMENT),
      fun_K(make_volume_functional(std::move(K))), fun_force(zero_volume_function),
      fun_dirichlet(zero_facet_function), fun_slip(zero_facet_function) {

    Minv_ = space_.inverseMassMatrix();
    E_Q = space_.evaluateBasisAt(volRule.points());
    E_Q_T = space_.evaluateBasisAt(volRule.points(), {1, 0});
    Dxi_Q = space_.evaluateGradientAt(volRule.points());

    negative_E_Q_T = Managed<Matrix<double>>(E_Q_T.shape(), std::size_t{ALIGNMENT});
    EigenMap(negative_E_Q_T) = -EigenMap(E_Q_T);

    for (std::size_t f = 0; f < DomainDimension + 1u; ++f) {
        auto points = cl_->facetParam(f, fctRule.points());
        E_q.emplace_back(space_.evaluateBasisAt(points));
        E_q_T.emplace_back(space_.evaluateBasisAt(points, {1, 0}));
        Dxi_q.emplace_back(space_.evaluateGradientAt(points));
        Dxi_q_120.emplace_back(space_.evaluateGradientAt(points, {1, 2, 0}));
        matE_q_T.emplace_back(materialSpace_.evaluateBasisAt(points, {1, 0}));

        negative_E_q_T.emplace_back(space_.evaluateBasisAt(points, {1, 0}));
        auto E = EigenMap(negative_E_q_T.back());
        E = -E;
    }

    matE_Q_T = materialSpace_.evaluateBasisAt(volRule.points(), {1, 0});
    matDxi_Q = materialSpace_.evaluateGradientAt(volRule.points());
}

void Poisson::compute_mass_matrix(std::size_t elNo, double* M) const {
    kernel::massMatrix mm;
    mm.E_Q = E_Q.data();
    mm.J_Q = vol[elNo].get<AbsDetJ>().data();
    mm.M = M;
    mm.W = volRule.weights().data();
    mm.execute();
}

void Poisson::compute_inverse_mass_matrix(std::size_t elNo, double* Minv) const {
    compute_mass_matrix(elNo, Minv);

    auto J_Q = vol[elNo].get<AbsDetJ>();
    alignas(ALIGNMENT) double Jinv_Q[tensor::Jinv_Q::size()] = {};
    for (unsigned q = 0; q < tensor::Jinv_Q::Shape[0]; ++q) {
        Jinv_Q[q] = 1.0 / J_Q[q];
    }

    kernel::MinvWA wa;
    wa.E_Q = E_Q.data();
    wa.Jinv_Q = Jinv_Q;
    wa.MinvRef = Minv_.data();
    wa.MinvWA = Minv;
    wa.W = volRule.weights().data();
    wa.execute();
}

void Poisson::compute_K_Dx_q(std::size_t fctNo, FacetInfo const& info,
                             std::array<double*, 2> K_Dx_q) const {
    kernel::K_Dx_q dx;
    for (int i = 0; i < 2; ++i) {
        if (K_Dx_q[i]) {
            auto JInv = (i == 1) ? fct[fctNo].get<JInv1>() : fct[fctNo].get<JInv0>();
            dx.G_q = JInv.data()->data();
            dx.matE_q_T = matE_q_T[info.localNo[i]].data();
            dx.K = material[info.up[i]].get<K>().data();
            dx.K_Dx_q(0) = K_Dx_q[i];
            dx.Dxi_q(0) = Dxi_q[info.localNo[i]].data();
            dx.execute();
        }
    }
}

void Poisson::compute_K_q(std::size_t fctNo, FacetInfo const& info,
                          std::array<double*, 2> K_q) const {
    kernel::K_q kw;
    for (int i = 0; i < 2; ++i) {
        if (K_q[i]) {
            kw.matE_q_T = matE_q_T[info.localNo[i]].data();
            kw.K = material[info.up[i]].get<K>().data();
            kw.K_q(0) = K_q[i];
            kw.execute();
        }
    }
}

void Poisson::begin_preparation(std::size_t numElements, std::size_t numLocalElements,
                                std::size_t numLocalFacets) {
    base::begin_preparation(numElements, numLocalElements, numLocalFacets);

    material.setStorage(
        std::make_shared<material_vol_t>(numElements * materialSpace_.numBasisFunctions()), 0u,
        numElements, materialSpace_.numBasisFunctions());

    volPre.setStorage(std::make_shared<vol_pre_t>(numElements * volRule.size()), 0u,
                      numLocalElements, volRule.size());

    fctPre.setStorage(std::make_shared<fct_pre_t>(numLocalFacets * fctRule.size()), 0u,
                      numLocalFacets, fctRule.size());

    penalty_.resize(numLocalFacets);
}

void Poisson::prepare_volume(std::size_t elNo, LinearAllocator<double>& scratch) {
    base::prepare_volume(elNo, scratch);

    auto Kfield = material[elNo].get<K>().data();
    alignas(ALIGNMENT) double K_Q_raw[tensor::K_Q::size()];
    auto K_Q = Matrix<double>(K_Q_raw, 1, volRule.size());
    fun_K(elNo, K_Q);

    alignas(ALIGNMENT) double Mmem[tensor::matM::size()];
    kernel::project_K_lhs krnl_lhs;
    krnl_lhs.matE_Q_T = matE_Q_T.data();
    krnl_lhs.J_Q = vol[elNo].get<AbsDetJ>().data();
    krnl_lhs.matM = Mmem;
    krnl_lhs.W = volRule.weights().data();
    krnl_lhs.execute();

    kernel::project_K_rhs krnl_rhs;
    krnl_rhs.matE_Q_T = matE_Q_T.data();
    krnl_rhs.J_Q = vol[elNo].get<AbsDetJ>().data();
    krnl_rhs.K = Kfield;
    krnl_rhs.K_Q = K_Q_raw;
    krnl_rhs.W = volRule.weights().data();
    krnl_rhs.execute();

    using MMap = Eigen::Map<Eigen::Matrix<double, tensor::matM::Shape[0], tensor::matM::Shape[1]>,
                            Eigen::Unaligned,
                            Eigen::OuterStride<init::matM::Stop[0] - init::matM::Start[0]>>;
    using KMap = Eigen::Map<Eigen::Matrix<double, tensor::K::Shape[0], 1>, Eigen::Unaligned,
                            Eigen::InnerStride<1>>;

    auto K_eigen = KMap(Kfield);
    K_eigen = MMap(Mmem).fullPivLu().solve(K_eigen);
}

void Poisson::prepare_skeleton(std::size_t fctNo, FacetInfo const& info,
                               LinearAllocator<double>& scratch) {
    base::prepare_skeleton(fctNo, info, scratch);

    for (int side = 0; side < 2; ++side) {
        kernel::K_G_q k;
        k.G_q = side == 1 ? fct[fctNo].get<JInv1>().data()->data()
                          : fct[fctNo].get<JInv0>().data()->data();
        k.K = material[info.up[side]].get<K>().data();
        k.K_G_q(0) = side == 1 ? fctPre[fctNo].get<KJInv1>().data()->data()
                               : fctPre[fctNo].get<KJInv0>().data()->data();
        k.matE_q_T = matE_q_T[info.localNo[side]].data();
        k.execute();
    }
}

void Poisson::prepare_boundary(std::size_t fctNo, FacetInfo const& info,
                               LinearAllocator<double>& scratch) {
    base::prepare_boundary(fctNo, info, scratch);

    kernel::K_G_q k;
    k.G_q = fct[fctNo].get<JInv0>().data()->data();
    k.K = material[info.up[0]].get<K>().data();
    k.K_G_q(0) = fctPre[fctNo].get<KJInv0>().data()->data();
    k.matE_q_T = matE_q_T[info.localNo[0]].data();
    k.execute();
}

void Poisson::prepare_volume_post_skeleton(std::size_t elNo, LinearAllocator<double>& scratch) {
    base::prepare_volume_post_skeleton(elNo, scratch);

    auto Kfield = material[elNo].get<K>().data();

    kernel::J_W_K_Q krnl;
    krnl.J_W_K_Q = volPre[elNo].get<AbsDetJWK>().data()->data();
    krnl.J_Q = vol[elNo].get<AbsDetJ>().data();
    krnl.K = Kfield;
    krnl.matE_Q_T = matE_Q_T.data();
    krnl.W = volRule.weights().data();
    krnl.execute();
}

void Poisson::prepare_penalty(std::size_t fctNo, FacetInfo const& info, LinearAllocator<double>&) {
    auto const p = [&](int side) {
        auto Kfield = material[info.up[side]].get<K>().data();
        auto k0 = *std::min_element(Kfield, Kfield + materialSpace_.numBasisFunctions());
        auto k1 = *std::max_element(Kfield, Kfield + materialSpace_.numBasisFunctions());
        constexpr double c_N_1 = InverseInequality<Dim>::trace_constant(PolynomialDegree - 1);
        return (Dim + 1) * c_N_1 * (area_[fctNo] / volume_[info.up[side]]) * (k1 * k1 / k0);
    };

    if (info.up[0] != info.up[1]) {
        penalty_[fctNo] = (p(0) + p(1)) / 4.0;
    } else {
        penalty_[fctNo] = p(0);
    }
}

bool Poisson::assemble_volume(std::size_t elNo, Matrix<double>& A00,
                              LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double Dx_Q[tensor::Dx_Q::size()];

    assert(volRule.size() == tensor::W::Shape[0]);
    assert(Dxi_Q.shape(0) == tensor::Dxi_Q::Shape[0]);
    assert(Dxi_Q.shape(1) == tensor::Dxi_Q::Shape[1]);
    assert(Dxi_Q.shape(2) == tensor::Dxi_Q::Shape[2]);

    kernel::Dx_Q dx;
    dx.Dx_Q = Dx_Q;
    dx.Dxi_Q = Dxi_Q.data();
    dx.G_Q = vol[elNo].get<JInv>().data()->data();
    dx.execute();

    kernel::assembleVolume krnl;
    krnl.A = A00.data();
    krnl.Dx_Q = Dx_Q;
    krnl.K = material[elNo].get<K>().data();
    krnl.matE_Q_T = matE_Q_T.data();
    krnl.J_Q = vol[elNo].get<AbsDetJ>().data();
    krnl.W = volRule.weights().data();
    krnl.execute();
    return true;
}

bool Poisson::assemble_skeleton(std::size_t fctNo, FacetInfo const& info, Matrix<double>& A00,
                                Matrix<double>& A01, Matrix<double>& A10, Matrix<double>& A11,
                                LinearAllocator<double>& scratch) const {
    assert(fctRule.size() == tensor::w::Shape[0]);
    assert(E_q[0].shape(0) == tensor::E_q::Shape[0][0]);
    assert(E_q[0].shape(1) == tensor::E_q::Shape[0][1]);
    assert(Dxi_q[0].shape(0) == tensor::Dxi_q::Shape[0][0]);
    assert(Dxi_q[0].shape(1) == tensor::Dxi_q::Shape[0][1]);
    assert(Dxi_q[0].shape(2) == tensor::Dxi_q::Shape[0][2]);

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    alignas(ALIGNMENT) double K_Dx_q1[tensor::K_Dx_q::size(1)];
    auto K_Dx_q = std::array<double*, 2>{K_Dx_q0, K_Dx_q1};
    compute_K_Dx_q(fctNo, info, K_Dx_q);

    alignas(ALIGNMENT) double L_q[2][std::max(tensor::L_q::size(0), tensor::L_q::size(1))];

    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double Lift0[tensor::Lift::size(0)];
        alignas(ALIGNMENT) double Lift1[tensor::Lift::size(1)];
        alignas(ALIGNMENT) double Minv[2][tensor::M::size()];
        for (int i = 0; i < 2; ++i) {
            compute_inverse_mass_matrix(info.up[i], Minv[i]);
        }

        alignas(ALIGNMENT) double K_q0[tensor::K_q::size(0)];
        alignas(ALIGNMENT) double K_q1[tensor::K_q::size(1)];
        auto K_q = std::array<double*, 2>{K_q0, K_q1};
        compute_K_q(fctNo, info, K_q);

        kernel::lift_skeleton lift;
        lift.Lift(0) = Lift0;
        lift.Lift(1) = Lift1;
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        for (int i = 0; i < 2; ++i) {
            lift.K_q(i) = K_q[i];
            lift.L_q(i) = L_q[i];
            lift.Minv(i) = Minv[i];
            lift.E_q(i) = E_q[info.localNo[i]].data();
        }
        lift.execute(0);
        lift.execute(1);
    } else { // IP
        kernel::lift_ip lift;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        for (int i = 0; i < 2; ++i) {
            lift.L_q(i) = L_q[i];
            lift.E_q(i) = E_q[info.localNo[i]].data();
        }
        lift.execute(0);
        lift.execute(1);
    }

    kernel::assembleSurface assemble;
    assemble.c00 = -0.5;
    assemble.c01 = -assemble.c00;
    assemble.c10 = epsilon * 0.5;
    assemble.c11 = -assemble.c10;
    assemble.c20 = penalty(fctNo);
    assemble.c21 = -assemble.c20;
    assemble.a(0, 0) = A00.data();
    assemble.a(0, 1) = A01.data();
    assemble.a(1, 0) = A10.data();
    assemble.a(1, 1) = A11.data();
    for (int i = 0; i < 2; ++i) {
        assemble.K_Dx_q(i) = K_Dx_q[i];
        assemble.E_q(i) = E_q[info.localNo[i]].data();
        assemble.L_q(i) = L_q[i];
    }
    assemble.n_q = fct[fctNo].get<Normal>().data()->data();
    assemble.w = fctRule.weights().data();
    assemble.execute(0, 0);
    assemble.execute(0, 1);
    assemble.execute(1, 0);
    assemble.execute(1, 1);

    return true;
}

bool Poisson::assemble_boundary(std::size_t fctNo, FacetInfo const& info, Matrix<double>& A00,
                                LinearAllocator<double>& scratch) const {
    if (info.bc == BC::Natural) {
        return false;
    }

    assert(fctRule.size() == tensor::w::Shape[0]);
    assert(E_q[0].shape(0) == tensor::E_q::Shape[0][0]);
    assert(E_q[0].shape(1) == tensor::E_q::Shape[0][1]);
    assert(Dxi_q[0].shape(0) == tensor::Dxi_q::Shape[0][0]);
    assert(Dxi_q[0].shape(1) == tensor::Dxi_q::Shape[0][1]);
    assert(Dxi_q[0].shape(2) == tensor::Dxi_q::Shape[0][2]);

    alignas(ALIGNMENT) double L0[tensor::L_q::size(0)];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double Lift0[tensor::Lift::size(0)];
        alignas(ALIGNMENT) double Minv0[tensor::M::size()];
        compute_inverse_mass_matrix(info.up[0], Minv0);

        alignas(ALIGNMENT) double K_q[tensor::K_q::size(0)];
        compute_K_q(fctNo, info, {K_q, nullptr});

        kernel::lift_boundary lift;
        lift.Lift(0) = Lift0;
        lift.K_q(0) = K_q;
        lift.L_q(0) = L0;
        lift.Minv(0) = Minv0;
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.w = fctRule.weights().data();
        lift.execute();
    } else { // IP
        kernel::lift_ip lift;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.L_q(0) = L0;
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.execute(0);
    }

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    compute_K_Dx_q(fctNo, info, {K_Dx_q0, nullptr});

    kernel::assembleSurface assemble;
    assemble.c00 = -1.0;
    assemble.c10 = epsilon;
    assemble.c20 = penalty(fctNo);
    assemble.a(0, 0) = A00.data();
    assemble.K_Dx_q(0) = K_Dx_q0;
    assemble.E_q(0) = E_q[info.localNo[0]].data();
    assemble.L_q(0) = L0;
    assemble.n_q = fct[fctNo].get<Normal>().data()->data();
    assemble.w = fctRule.weights().data();
    assemble.execute(0, 0);
    return true;
}

bool Poisson::rhs_volume(std::size_t elNo, Vector<double>& B,
                         LinearAllocator<double>& scratch) const {
    assert(tensor::b::Shape[0] == tensor::A::Shape[0]);

    alignas(ALIGNMENT) double F_Q_raw[tensor::F_Q::size()];
    assert(tensor::F_Q::size() == volRule.size());
    auto F_Q = Matrix<double>(F_Q_raw, 1, tensor::F_Q::Shape[0]);
    fun_force(elNo, F_Q);

    kernel::rhsVolume rhs;
    rhs.E_Q = E_Q.data();
    rhs.F_Q = F_Q_raw;
    rhs.J_Q = vol[elNo].get<AbsDetJ>().data();
    rhs.W = volRule.weights().data();
    rhs.b = B.data();
    rhs.execute();
    return true;
}

bool Poisson::bc_skeleton(std::size_t fctNo, BC bc, double f_q_raw[]) const {
    assert(tensor::f_q::size() == fctRule.size());
    auto f_q = Matrix<double>(f_q_raw, 1, tensor::f_q::Shape[0]);
    if (bc == BC::Fault) {
        fun_slip(fctNo, f_q, false);
    } else if (bc == BC::Dirichlet) {
        fun_dirichlet(fctNo, f_q, false);
    } else {
        return false;
    }
    return true;
}
bool Poisson::bc_boundary(std::size_t fctNo, BC bc, double f_q_raw[]) const {
    assert(tensor::f_q::size() == fctRule.size());
    auto f_q = Matrix<double>(f_q_raw, 1, tensor::f_q::Shape[0]);
    if (bc == BC::Fault) {
        fun_slip(fctNo, f_q, true);
        for (std::size_t q = 0; q < tensor::f_q::Shape[0]; ++q) {
            f_q(0, q) *= 0.5;
        }
    } else if (bc == BC::Dirichlet) {
        fun_dirichlet(fctNo, f_q, true);
    } else {
        return false;
    }
    return true;
}

bool Poisson::rhs_skeleton(std::size_t fctNo, FacetInfo const& info, Vector<double>& B0,
                           Vector<double>& B1, LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    if (!bc_skeleton(fctNo, info.bc, f_q_raw)) {
        return false;
    }

    alignas(ALIGNMENT) double f_lifted_q[tensor::f_lifted_q::size()];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double f_lifted0[tensor::f_lifted::size(0)];
        alignas(ALIGNMENT) double f_lifted1[tensor::f_lifted::size(1)];
        alignas(ALIGNMENT) double Minv[2][tensor::M::size()];
        compute_inverse_mass_matrix(info.up[0], Minv[0]);
        compute_inverse_mass_matrix(info.up[1], Minv[1]);

        alignas(ALIGNMENT) double K_q0[tensor::K_q::size(0)];
        alignas(ALIGNMENT) double K_q1[tensor::K_q::size(1)];
        auto K_q = std::array<double*, 2>{K_q0, K_q1};
        compute_K_q(fctNo, info, K_q);

        kernel::rhs_lift_skeleton lift;
        for (int i = 0; i < 2; ++i) {
            lift.E_q(i) = E_q[info.localNo[i]].data();
            lift.K_q(i) = K_q[i];
            lift.Minv(i) = Minv[i];
        }
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.f_q = f_q_raw;
        lift.f_lifted(0) = f_lifted0;
        lift.f_lifted(1) = f_lifted1;
        lift.f_lifted_q = f_lifted_q;
        lift.w = fctRule.weights().data();
        lift.execute();
    } else { // IP
        kernel::rhs_lift_ip lift;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.f_q = f_q_raw;
        lift.f_lifted_q = f_lifted_q;
        lift.execute();
    }

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    alignas(ALIGNMENT) double K_Dx_q1[tensor::K_Dx_q::size(1)];
    compute_K_Dx_q(fctNo, info, {K_Dx_q0, K_Dx_q1});

    kernel::rhsFacet rhs;
    rhs.b = B0.data();
    rhs.c10 = 0.5 * epsilon;
    rhs.c20 = penalty(fctNo);
    rhs.f_q = f_q_raw;
    rhs.f_lifted_q = f_lifted_q;
    rhs.n_q = fct[fctNo].get<Normal>().data()->data();
    rhs.w = fctRule.weights().data();
    rhs.K_Dx_q(0) = K_Dx_q0;
    rhs.E_q(0) = E_q[info.localNo[0]].data();
    rhs.execute();

    rhs.b = B1.data();
    rhs.c20 *= -1.0;
    rhs.K_Dx_q(0) = K_Dx_q1;
    rhs.E_q(0) = E_q[info.localNo[1]].data();
    rhs.execute();

    return true;
}

bool Poisson::rhs_boundary(std::size_t fctNo, FacetInfo const& info, Vector<double>& B0,
                           LinearAllocator<double>& scratch) const {
    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    if (!bc_boundary(fctNo, info.bc, f_q_raw)) {
        return false;
    }

    alignas(ALIGNMENT) double f_lifted_q[tensor::f_lifted_q::size()];
    if (method_ == DGMethod::BR2) {
        alignas(ALIGNMENT) double f_lifted0[tensor::f_lifted::size(0)];
        alignas(ALIGNMENT) double M0[tensor::M::size()];
        compute_inverse_mass_matrix(info.up[0], M0);

        alignas(ALIGNMENT) double K_q[tensor::K_q::size(0)];
        compute_K_q(fctNo, info, {K_q, nullptr});

        kernel::rhs_lift_boundary lift;
        lift.E_q(0) = E_q[info.localNo[0]].data();
        lift.n_q = fct[fctNo].get<Normal>().data()->data();
        lift.K_q(0) = K_q;
        lift.Minv(0) = M0;
        lift.f_q = f_q_raw;
        lift.f_lifted(0) = f_lifted0;
        lift.f_lifted_q = f_lifted_q;
        lift.w = fctRule.weights().data();
        lift.execute();
    } else { // IP
        kernel::rhs_lift_ip lift;
        lift.nl_q = fct[fctNo].get<NormalLength>().data();
        lift.f_q = f_q_raw;
        lift.f_lifted_q = f_lifted_q;
        lift.execute();
    }

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    compute_K_Dx_q(fctNo, info, {K_Dx_q0, nullptr});

    kernel::rhsFacet rhs;
    rhs.b = B0.data();
    rhs.c10 = epsilon;
    rhs.c20 = penalty(fctNo);
    rhs.f_q = f_q_raw;
    rhs.f_lifted_q = f_lifted_q;
    rhs.n_q = fct[fctNo].get<Normal>().data()->data();
    rhs.w = fctRule.weights().data();
    rhs.K_Dx_q(0) = K_Dx_q0;
    rhs.E_q(0) = E_q[info.localNo[0]].data();
    rhs.execute();
    return true;
}

void Poisson::apply(std::size_t elNo, mneme::span<SideInfo> info, Vector<double const> const& x_0,
                    std::array<Vector<double const>, NumFacets> const& x_n,
                    Vector<double>& y_0) const {

    alignas(ALIGNMENT) double Dx_Q[tensor::Dx_Q::size()];
    kernel::apply_volume av;
    av.Dx_Q = Dx_Q;
    av.Dxi_Q = Dxi_Q.data();
    av.G_Q = vol[elNo].get<JInv>().data()->data();
    av.J_W_K_Q = volPre[elNo].get<AbsDetJWK>().data()->data();
    av.U = x_0.data();
    av.U_new = y_0.data();
    av.execute();

    alignas(ALIGNMENT) double n_q_flipped[tensor::n_q::size()];
    alignas(ALIGNMENT) double n_unit_q_flipped[tensor::n_unit_q::size()];
    for (std::size_t f = 0; f < NumFacets; ++f) {
        bool is_skeleton_face = elNo != info[f].lid;
        bool is_fault_or_dirichlet = info[f].bc == BC::Fault || info[f].bc == BC::Dirichlet;

        auto fctNo = info[f].fctNo;
        double const* n_q = fct[fctNo].get<Normal>().data()->data();
        double const* n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
        double const* K_G_q0 = fctPre[fctNo].get<KJInv0>().data()->data();
        double const* K_G_q1 = fctPre[fctNo].get<KJInv1>().data()->data();
        if (is_skeleton_face && info[f].side == 1) {
            std::swap(K_G_q0, K_G_q1);

            for (int i = 0; i < tensor::n_q::size(); ++i) {
                n_q_flipped[i] = -n_q[i];
            }
            n_q = n_q_flipped;
            for (int i = 0; i < tensor::n_unit_q::size(); ++i) {
                n_unit_q_flipped[i] = -n_unit_q[i];
            }
            n_unit_q = n_unit_q_flipped;
        }

        alignas(ALIGNMENT) double u_hat_q[tensor::u_hat_q::size()] = {};
        alignas(ALIGNMENT) double sigma_hat_q[tensor::sigma_hat_q::size()] = {};
        if (info[f].bc == BC::None || (is_skeleton_face && is_fault_or_dirichlet)) {
            kernel::flux_u_skeleton fu;
            fu.negative_E_q_T(0) = negative_E_q_T[f].data();
            fu.E_q_T(1) = E_q_T[info[f].localNo].data();
            fu.U = x_0.data();
            fu.U_ext = x_n[f].data();
            fu.u_hat_q = u_hat_q;
            fu.execute();

            kernel::flux_sigma_skeleton fs;
            fs.c00 = -penalty(fctNo);
            fs.Dxi_q_120(0) = Dxi_q_120[f].data();
            fs.Dxi_q_120(1) = Dxi_q_120[info[f].localNo].data();
            fs.E_q_T(0) = E_q_T[f].data();
            fs.negative_E_q_T(1) = negative_E_q_T[info[f].localNo].data();
            fs.K_G_q(0) = K_G_q0;
            fs.K_G_q(1) = K_G_q1;
            fs.U = x_0.data();
            fs.U_ext = x_n[f].data();
            fs.n_unit_q = n_unit_q;
            fs.sigma_hat_q = sigma_hat_q;
            fs.execute();
        } else if (is_fault_or_dirichlet) {
            kernel::flux_u_boundary fu;
            fu.U = x_0.data();
            fu.u_hat_q = u_hat_q;
            fu.negative_E_q_T(0) = negative_E_q_T[f].data();
            fu.execute();

            kernel::flux_sigma_boundary fs;
            fs.c00 = -penalty(fctNo);
            fs.Dxi_q_120(0) = Dxi_q_120[f].data();
            fs.E_q_T(0) = E_q_T[f].data();
            fs.K_G_q(0) = K_G_q0;
            fs.U = x_0.data();
            fs.n_unit_q = n_unit_q;
            fs.sigma_hat_q = sigma_hat_q;
            fs.execute();
        } else {
            continue;
        }

        kernel::apply_facet af;
        af.Dxi_q(0) = Dxi_q[f].data();
        af.E_q(0) = E_q[f].data();
        af.K_G_q(0) = K_G_q0;
        af.n_q = n_q;
        af.sigma_hat_q = sigma_hat_q;
        af.u_hat_q = u_hat_q;
        af.U_new = y_0.data();
        af.w = fctRule.weights().data();
        af.execute();
    }
}

std::size_t Poisson::flops_apply(std::size_t elNo, mneme::span<SideInfo> info) const {
    std::size_t flops = kernel::apply_volume::HardwareFlops;
    for (std::size_t f = 0; f < NumFacets; ++f) {
        bool is_skeleton_face = elNo != info[f].lid;
        bool is_fault_or_dirichlet = info[f].bc == BC::Fault || info[f].bc == BC::Dirichlet;
        if (info[f].bc == BC::None || (is_skeleton_face && is_fault_or_dirichlet)) {
            flops += kernel::flux_u_skeleton::HardwareFlops;
            flops += kernel::flux_sigma_skeleton::HardwareFlops;
        } else if (is_fault_or_dirichlet) {
            flops += kernel::flux_u_boundary::HardwareFlops;
            flops += kernel::flux_sigma_boundary::HardwareFlops;
        } else {
            continue;
        }
        flops += kernel::apply_facet::HardwareFlops;
    }
    return flops;
}

void Poisson::coefficients_volume(std::size_t elNo, Matrix<double>& C,
                                  LinearAllocator<double>&) const {
    auto const coeff_K = material[elNo].get<K>();
    assert(coeff_K.size() == C.shape(0));
    for (std::size_t i = 0; i < coeff_K.size(); ++i) {
        C(i, 0) = coeff_K[i];
    }
}

TensorBase<Matrix<double>> Poisson::tractionResultInfo() const {
    return TensorBase<Matrix<double>>(tensor::grad_u::Shape[0], tensor::grad_u::Shape[1]);
}

void Poisson::traction_skeleton(std::size_t fctNo, FacetInfo const& info, Vector<double const>& u0,
                                Vector<double const>& u1, Matrix<double>& result) const {
    assert(result.size() == tensor::grad_u::size());

    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    bc_skeleton(fctNo, info.bc, f_q_raw);

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    alignas(ALIGNMENT) double K_Dx_q1[tensor::K_Dx_q::size(1)];
    compute_K_Dx_q(fctNo, info, {K_Dx_q0, K_Dx_q1});

    kernel::grad_u krnl;
    krnl.c00 = -penalty(fctNo);
    krnl.K_Dx_q(0) = K_Dx_q0;
    krnl.K_Dx_q(1) = K_Dx_q1;
    krnl.E_q(0) = E_q[info.localNo[0]].data();
    krnl.E_q(1) = E_q[info.localNo[1]].data();
    krnl.f_q = f_q_raw;
    krnl.grad_u = result.data();
    krnl.n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
    krnl.u(0) = u0.data();
    krnl.u(1) = u1.data();
    krnl.execute();
}

void Poisson::traction_boundary(std::size_t fctNo, FacetInfo const& info, Vector<double const>& u0,
                                Matrix<double>& result) const {
    assert(result.size() == tensor::grad_u::size());

    alignas(ALIGNMENT) double f_q_raw[tensor::f_q::size()];
    bc_boundary(fctNo, info.bc, f_q_raw);

    alignas(ALIGNMENT) double K_Dx_q0[tensor::K_Dx_q::size(0)];
    compute_K_Dx_q(fctNo, info, {K_Dx_q0, nullptr});

    kernel::grad_u_bnd krnl;
    krnl.c00 = -penalty(fctNo);
    krnl.K_Dx_q(0) = K_Dx_q0;
    krnl.E_q(0) = E_q[info.localNo[0]].data();
    krnl.f_q = f_q_raw;
    krnl.grad_u = result.data();
    krnl.n_unit_q = fct[fctNo].get<UnitNormal>().data()->data();
    krnl.u(0) = u0.data();
    krnl.execute();
}

} // namespace tndm


#include <array>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <random>
#include <vector>

#include <boost/container/static_vector.hpp>
#include <boost/random.hpp>

template <typename Type = std::uint64_t> class randomizer_with_sentinel_shift {
  public:
    template <typename Generator> bool operator()(Generator &rng) {
        if (1 == m_rand)
            m_rand = std::uniform_int_distribution<Type> {} (rng) | s_mask_left1;
        const bool ret = m_rand & 1;
        m_rand >>= 1;
        return ret;
    }
  private:
    static constexpr Type s_mask_left1 = Type(1) << (sizeof(Type) * 8 - 1);
    Type m_rand = 1;
};

namespace {

    constexpr auto FIELD_SIZE = 10;
    enum class CellType: std::uint8_t {
      EMPTY,
      UNAVAILABLE,
      OCCUPIED
    };
    using Coordinates = struct { std::int32_t x; std::int32_t y; };
    using Directions = std::array<Coordinates, 9>;

    constexpr auto DIRECTIONS = Directions {{{  0,  0 },
                                             {  0,  1 },
                                             {  0, -1 },
                                             { -1,  0 },
                                             {  1,  0 },
                                             { -1,  1 },
                                             {  1, -1 },
                                             { -1, -1 },
                                             {  1,  1 }}};

    boost::taus88 generator {std::random_device {}()};
    randomizer_with_sentinel_shift<std::uint64_t> bool_generator;
}

struct Field {
  std::array<CellType, FIELD_SIZE * FIELD_SIZE> field = {CellType::EMPTY };
  friend std::ostream& operator<<(std::ostream& o, const Field& field) {
    for (size_t index = 0; index < FIELD_SIZE * FIELD_SIZE;) {
      switch (field.field[index]) {
        case CellType::EMPTY:
          o << ".";
          break;
        case CellType::UNAVAILABLE:
          o << " ";
          break;
        case CellType::OCCUPIED:
          o << "X";
          break;
      }
      if (++index % FIELD_SIZE == 0)
        o << "\n";
    }
    return o;
  }
};

bool
is_valid_formation(const Field&       field,
                   std::size_t        x,
                   std::size_t        y,
                   const std::int32_t dx,
                   const std::int32_t dy,
                   const std::size_t  ship_size) noexcept
{

  static const auto within_bounds = [](const auto x,
                                       const auto y,
                                       Coordinates direction = Coordinates {0, 0})
  noexcept {
    return ((x + direction.x  < FIELD_SIZE) && (x + direction.x >= 0)) &&
           ((y + direction.y  < FIELD_SIZE) && (y + direction.y >= 0));
  };

  for (std::size_t iteration = 0; iteration < ship_size; ++iteration) {
    const std::int32_t nx = x + (dx * iteration);
    const std::int32_t ny = y + (dy * iteration);
    for (const auto& direction: DIRECTIONS) {
      [[likely]] if (!within_bounds(nx, ny, direction))
        continue;
      const auto& bounding_box_cell = field.field[(nx + direction.x) + ((ny + direction.y) * FIELD_SIZE)];
      if (CellType::OCCUPIED == bounding_box_cell)
        return false;
    }
  }

  for (std::size_t iteration = 0; iteration < ship_size; ++iteration) {
    if (!within_bounds(x, y))
      return false;
    if (CellType::EMPTY != field.field[(y * FIELD_SIZE) + x])
      return false;
    x += dx;
    y += dy;
  }
  return true;
}

template <typename Buffer = boost::container::static_vector<Coordinates, FIELD_SIZE * FIELD_SIZE>>
void
get_available_cells(const Field&       field,
                    const std::int32_t dx,
                    const std::int32_t dy,
                    const std::size_t  ship_size,
                    Buffer& buffer) noexcept
{
  buffer.clear();
  for (auto x = 0; x < FIELD_SIZE; ++x) {
    for (auto y = 0; y < FIELD_SIZE; ++y) {
      if (is_valid_formation(field, x, y, dx, dy, ship_size))
        buffer.emplace_back(x, y);
    }
  }
}

template <typename Buffer = boost::container::static_vector<Coordinates, FIELD_SIZE * FIELD_SIZE>>
auto emplace_ships(Field&            field,
                   const std::size_t ship_size,
                   Buffer&           buffer) noexcept
{
  static const auto get_alignment = [] {
    return bool_generator(generator)
           ? Coordinates {1, 0}
           : Coordinates {0, 1};
  };

  auto [dx, dy] = get_alignment();
  get_available_cells(field, dx, dy, ship_size, buffer);
  auto [x, y] = buffer[generator() % buffer.size()];
  for (std::size_t iteration = 0; iteration < ship_size; ++iteration) {
    field.field[x + (FIELD_SIZE * y)] = CellType::OCCUPIED;
    x += dx;
    y += dy;
  }
}

int
main()
{
  boost::container::static_vector<Coordinates, FIELD_SIZE * FIELD_SIZE> buffer;
  buffer.resize(FIELD_SIZE * FIELD_SIZE);
  Field field;
  for (auto&& ship_size: { 4, 3, 3, 2, 2, 2, 1, 1, 1, 1 }) {
    emplace_ships(field, ship_size, buffer);
  }
}