// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE Reduce
#include <poplar/Engine.hpp>

#include <poplibs_support/TestDevice.hpp>
// codelets
#include "../../lib/popops/reduction/ReductionVertex.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <cmath>
#include <string.h>

#include <stdexcept>

#include <boost/program_options.hpp>

#define PARTIALS_ARE 1
#define SCALE 2.0
#define UPDATE false

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

static bool do_test(const DeviceType &deviceType, const Type &inType,
                    const Type &outType, unsigned outerDim,
                    unsigned INNER_DIM) {
  static unsigned outer_dim = outerDim * (1 + PARTIALS_ARE);

  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  std::vector<unsigned short> has(2 * outerDim);
  std::fill(has.begin(), has.end(), 15360);
  std::vector<float> answers(2 * outerDim);
  std::fill(answers.begin(), answers.end(), 1.0);
  std::vector<char> ans_data(2 * outerDim * 4);

  if (outType == FLOAT) {
    memcpy(ans_data.data(), answers.data(), outerDim * 4 * 2);
  } else {
    memcpy(ans_data.data(), has.data(), outerDim * 2 * 2);
  }

  std::vector<unsigned char> data(INNER_DIM * outer_dim * 4);
  std::vector<float> nums(INNER_DIM * outer_dim);
  for (unsigned i = 0; i < INNER_DIM * outer_dim; ++i) {
    nums[i] = 1.0 * (i % outerDim);
  }
  copy(target, nums.data(), INNER_DIM * outer_dim, inType, data.data());

  std::vector<unsigned> counts(2);
  counts[0] = INNER_DIM;
  counts[1] = INNER_DIM;

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  auto partials = graph.addVariable(inType, {INNER_DIM, outer_dim});
  auto partials_2 = graph.addVariable(inType, {INNER_DIM, outer_dim});
  auto out = graph.addVariable(outType, {2, outerDim});

  const auto specialisation =
      outerDim == 1 ? ReductionSpecialisation::SCALAR_OUTPUT_REGIONS
                    : ReductionSpecialisation::DEFAULT;
  const auto vertexClass =
      templateVertex("popops::ScaledReduce", "popops::ReduceAdd", inType,
                     outType, UPDATE, specialisation);
  auto v1 = graph.addVertex(cs, vertexClass);

  for (unsigned i = 0; i < INNER_DIM; ++i) {
    Tensor Row = partials.slice(i, i + 1, 0);
    graph.connect(v1["partials"][i], Row.reshape({outer_dim}));
  }
  for (unsigned i = 0; i < INNER_DIM; ++i) {
    Tensor Row = partials_2.slice(i, i + 1, 0);
    graph.connect(v1["partials"][i + INNER_DIM], Row.reshape({outer_dim}));
  }
  graph.setFieldSize(v1["partials"], 2 * INNER_DIM);
  graph.connect(v1["out"], out);

  auto scale = graph.addVariable(FLOAT, {});
  graph.setTileMapping(scale, 0);
  graph.setInitialValue(scale, SCALE);
  graph.connect(v1["k"], scale.reshape({1}));
  auto t = graph.addConstant(UNSIGNED_SHORT, {counts.size()}, counts.data());
  graph.setTileMapping(t, 0);
  graph.connect(v1["numPartials"], t);

  graph.setTileMapping(v1, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(partials_2, 0);
  graph.setTileMapping(out, 0);

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("partials_2", partials_2);
  graph.createHostWrite("outw", out);
  graph.createHostRead("out", out);

  prog.add(Execute(cs));

  Engine e(graph, prog);
  auto outSize = out.numElements() * target.getTypeSize(outType);

  device.bind([&](const Device &d) {
    e.load(d);

    e.writeTensor("partials", data.data(),
                  data.data() +
                      partials.numElements() * target.getTypeSize(inType));
    e.writeTensor("partials_2", data.data(),
                  data.data() +
                      partials_2.numElements() * target.getTypeSize(inType));
    e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);

    e.run();

    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);
  });

  copy(target, outType, ans_data.data(), answers.data(), outerDim * 2);

  bool sucess = true;
  for (unsigned i = 0; i < outerDim * 2; ++i) {
    if ((INNER_DIM * 2.0 * SCALE * (i % outerDim)) != answers[i]) {
      sucess = false;
      std::cerr << "Condition failed: index " << i << " expected "
                << (INNER_DIM * 2.0 * SCALE * (i % outerDim)) << " actual "
                << answers[i] << "\n";
    }
  }
  return sucess;
}

static bool do_test_multi(const DeviceType &deviceType, const Type &inType,
                          const Type &outType, unsigned outerDim) {
  static unsigned outer_dim = outerDim * (1 + PARTIALS_ARE);
  unsigned INNER_DIM = 2;
  if (outType == HALF) {
    INNER_DIM = 1;
  }

  std::vector<float> answers(4 * 2 * outerDim);
  std::vector<char> ans_data(4 * 2 * outerDim * 4);
  std::fill(answers.begin(), answers.end(), 0.0);

  std::fill(ans_data.begin(), ans_data.end(), 0);
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  std::vector<unsigned char> data(INNER_DIM * outer_dim * 4);
  std::vector<float> nums(INNER_DIM * outer_dim);
  for (unsigned i = 0; i < INNER_DIM * outer_dim; ++i) {
    nums[i] = 1.0 * (i % outerDim);
  }
  copy(target, nums.data(), INNER_DIM * outer_dim, inType, data.data());

  std::vector<unsigned> counts(2);
  counts[0] = INNER_DIM;
  counts[1] = INNER_DIM;

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  auto partials = graph.addVariable(inType, {INNER_DIM, outer_dim});
  auto partials_2 = graph.addVariable(inType, {INNER_DIM, outer_dim});

  std::vector<Tensor> outs(4);
  for (unsigned i = 0; i < outs.size(); ++i) {
    outs[i] = graph.addVariable(outType, {2, outerDim});
  }

  unsigned specialisation = outerDim == 1;
  const auto specialisationType =
      specialisation == 0 ? ReductionSpecialisation::DEFAULT
                          : ReductionSpecialisation::SCALAR_OUTPUT_REGIONS;
  const auto mul_vertex =
      templateVertex("popops::ScaledReduce", "popops::ReduceMul", inType,
                     outType, false, specialisationType);
  const auto max_vertex =
      templateVertex("popops::ScaledReduce", "popops::ReduceMax", inType,
                     outType, false, specialisationType);
  const auto min_vertex =
      templateVertex("popops::ScaledReduce", "popops::ReduceMin", inType,
                     outType, false, specialisationType);
  const auto sqadd_vertex =
      templateVertex("popops::ScaledReduce", "popops::ReduceSquareAdd", inType,
                     outType, false, specialisationType);
  auto v_mul = graph.addVertex(cs, mul_vertex);
  auto v_max = graph.addVertex(cs, max_vertex);
  auto v_min = graph.addVertex(cs, min_vertex);
  auto v_sqadd = graph.addVertex(cs, sqadd_vertex);

  for (unsigned i = 0; i < INNER_DIM; ++i) {
    Tensor Row = partials.slice(i, i + 1, 0);
    graph.connect(v_mul["partials"][i], Row.reshape({outer_dim}));
    graph.connect(v_max["partials"][i], Row.reshape({outer_dim}));
    graph.connect(v_min["partials"][i], Row.reshape({outer_dim}));
    graph.connect(v_sqadd["partials"][i], Row.reshape({outer_dim}));
  }
  for (unsigned i = 0; i < INNER_DIM; ++i) {
    Tensor Row = partials_2.slice(i, i + 1, 0);
    graph.connect(v_mul["partials"][i + INNER_DIM], Row.reshape({outer_dim}));
    graph.connect(v_max["partials"][i + INNER_DIM], Row.reshape({outer_dim}));
    graph.connect(v_min["partials"][i + INNER_DIM], Row.reshape({outer_dim}));
    graph.connect(v_sqadd["partials"][i + INNER_DIM], Row.reshape({outer_dim}));
  }
  graph.setFieldSize(v_mul["partials"], 2 * INNER_DIM);
  graph.setFieldSize(v_max["partials"], 2 * INNER_DIM);
  graph.setFieldSize(v_min["partials"], 2 * INNER_DIM);
  graph.setFieldSize(v_sqadd["partials"], 2 * INNER_DIM);
  graph.connect(v_mul["out"], outs[0]);
  graph.connect(v_max["out"], outs[1]);
  graph.connect(v_min["out"], outs[2]);
  graph.connect(v_sqadd["out"], outs[3]);

  auto t = graph.addConstant(UNSIGNED_SHORT, {counts.size()}, counts.data());
  graph.setTileMapping(t, 0);
  graph.connect(v_mul["numPartials"], t);
  graph.connect(v_max["numPartials"], t);
  graph.connect(v_min["numPartials"], t);
  graph.connect(v_sqadd["numPartials"], t);

  auto scale = graph.addVariable(FLOAT, {});
  graph.setTileMapping(scale, 0);
  graph.setInitialValue(scale, SCALE);
  graph.connect(v_sqadd["k"], scale.reshape({1}));
  graph.connect(v_mul["k"], scale.reshape({1}));
  graph.connect(v_max["k"], scale.reshape({1}));
  graph.connect(v_min["k"], scale.reshape({1}));

  graph.setTileMapping(v_mul, 0);
  graph.setTileMapping(v_max, 0);
  graph.setTileMapping(v_min, 0);
  graph.setTileMapping(v_sqadd, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(partials_2, 0);
  for (unsigned i = 0; i < outs.size(); ++i) {
    graph.setTileMapping(outs[i], 0);
  }

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("partials_2", partials_2);
  for (unsigned i = 0; i < outs.size(); ++i) {
    graph.createHostWrite("outw" + std::to_string(i), outs[i]);
    graph.createHostRead("out" + std::to_string(i), outs[i]);
  }

  prog.add(Execute(cs));

  Engine e(graph, prog);
  device.bind([&](const Device &d) {
    e.load(d);
    e.writeTensor("partials", data.data(),
                  data.data() +
                      partials.numElements() * target.getTypeSize(inType));
    e.writeTensor("partials_2", data.data(),
                  data.data() +
                      partials_2.numElements() * target.getTypeSize(inType));
    for (unsigned k = 0; k < 4; ++k) {
      auto outSize = outs[k].numElements() * target.getTypeSize(inType);
      auto pData = &ans_data[k * 2 * outerDim * 4];
      e.writeTensor("outw" + std::to_string(k), pData, pData + outSize);
      e.readTensor("out" + std::to_string(k), pData, pData + outSize);
    }

    e.run();
    for (unsigned k = 0; k < 4; ++k) {
      auto outSize = outs[k].numElements() * target.getTypeSize(inType);
      auto pData = &ans_data[k * 2 * outerDim * 4];
      e.readTensor("out" + std::to_string(k), pData, pData + outSize);
    }
  });

  for (unsigned k = 0; k < 4; ++k) {
    unsigned size_of_out = (FLOAT == outType) ? 4 : 2;
    copy(target, outType, &ans_data[k * 2 * outerDim * 4],
         &answers[k * 2 * outerDim], outerDim);
    copy(target, outType,
         &ans_data[k * 2 * outerDim * 4 + outerDim * size_of_out],
         &answers[k * 2 * outerDim + outerDim], outerDim);
  }

  for (int j = 0; j < 2; ++j) {
    for (unsigned i = 0; i < outerDim; ++i) {
      if (pow(i, 2 * INNER_DIM) * SCALE != answers[j * outerDim + i]) {
        std::cerr << "Condition failed: index " << i << " " << j << " expected "
                  << pow(i, 2 * INNER_DIM) * SCALE << " actual "
                  << answers[j * outerDim + i] << "\n";
        return false;
      }
    }
  }
  for (int j = 0; j < 2; ++j) {
    for (unsigned i = 0; i < outerDim; ++i) {
      if (i * SCALE != answers[2 * outerDim + j * outerDim + i]) {
        std::cerr << "Condition failed: index " << i << " " << j << " expected "
                  << i * SCALE << " actual "
                  << answers[2 * outerDim + j * outerDim + i] << "\n";
        return false;
      }
      if (i * SCALE != answers[2 * 2 * outerDim + j * outerDim + i]) {
        std::cerr << "Condition failed: index " << i << " " << j << " expected "
                  << i * SCALE << " actual "
                  << answers[2 * outerDim + j * outerDim + i] << "\n";
        return false;
      }
    }
  }
  for (int j = 0; j < 2; ++j) {
    for (unsigned i = 0; i < outerDim; ++i) {
      if (i * i * INNER_DIM * 2 * SCALE !=
          answers[3 * 2 * outerDim + j * outerDim + i]) {
        std::cerr << "Condition failed: index " << i << " " << j << " expected "
                  << i * i * INNER_DIM * 2 * SCALE << " actual "
                  << answers[3 * 2 * outerDim + j * outerDim + i] << "\n";
        return false;
      }
    }
  }
  return true;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType;
  Type inType;
  Type outType;
  unsigned outerDim;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options() ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("in-type",
     po::value<Type>(&inType)->required(),
     "In Type")
    ("out-type",
     po::value<Type>(&outType)->required(),
     "Output Type")
    ("outer-dim",
     po::value<unsigned>(&outerDim)->required(),
     "Outer dimension");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  if (!do_test(deviceType, inType, outType, outerDim, 4)) {
    return 1;
  }
  if (inType == outType) {
    if (!do_test_multi(deviceType, inType, outType, outerDim)) {
      return 1;
    }
  }
  return 0;
}


#include "SchematLib/Algorithms/MapSimplificationPipeline.h"

#include <boost/property_tree/ptree.hpp>

#include "SchematLib/MapSimplification/BboxFaceMerge.h"
#include "SchematLib/MapSimplification/RemoveFaceTendrils.h"

namespace SchematLib::Algorithms {
    Models::NT MapSimplificationPipeline::maxBboxAxis() const
    {
        return std::max(m_bboxWidth, m_bboxHeight);
    }

    std::string MapSimplificationPipeline::stageName(const std::string& name) const
    {
        if (m_currentSettings.m_totalIterations == 1 && !m_currentSettings.m_untillConvergence)
        {
            return name;
        }
        return std::to_string(m_currentSettings.m_currentIteration) + ":" + name;
    }

    void MapSimplificationPipeline::applyRemoveTendrils(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::RemoveTendrils<Models::UndirectedEmbeddedGraph, SimpObs> removeTendrils;
        removeTendrils.setMaxLength(m_currentSettings.m_maxPruneLength);

        removeTendrils(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyDegree2Smoothing(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::Degree2VertexPreprocess<Models::UndirectedEmbeddedGraph> degree2Smoothing(
            m_currentSettings.m_maxTurningAngle / 180. * M_PI);
        degree2Smoothing.setRepeatUntilConvergence(m_currentSettings.m_edgeSmoothUntilConvergence);
        degree2Smoothing(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyDegree2SmoothingPostFM(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::Degree2VertexPreprocess<Models::UndirectedEmbeddedGraph> degree2Smoothing(
            m_currentSettings.m_maxTurningAnglePostFM / 180. * M_PI);
        degree2Smoothing.setRepeatUntilConvergence(m_currentSettings.m_edgeSmoothUntilConvergence);
        degree2Smoothing(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyVertexCollapse(Graph& target, SimpObs& simplificationObserver)
    {
        Graph tempCopy = target;
        const auto radius = m_currentSettings.m_radius * std::max(m_bboxHeight, m_bboxWidth);
        if (m_preprocessApproach == VerexOrderApproach::DenseVertexOrder)
        {
            //// Setup merger algorithm
            MapSimplification::VertexMerger<Models::UndirectedEmbeddedGraph, Models::UndirectedGraphNodeIndex, SimpObs,
                                            MapSimplification::MergeOrders::MostDenseVertexOrder> merger(radius);
            merger.setUseMeanPosition(m_currentSettings.m_useMeanPosition);
            //// Compute merge groups
            merger(tempCopy, simplificationObserver, target);
        }
        else
        {
            //// Setup merger algorithm
            MapSimplification::VertexMerger<Models::UndirectedEmbeddedGraph, Models::UndirectedGraphNodeIndex, SimpObs>
                merger(radius);
            merger.setUseMeanPosition(m_currentSettings.m_useMeanPosition);
            //// Compute merge groups
            merger(tempCopy, simplificationObserver, target);
        }
    }

    void MapSimplificationPipeline::applyVertexToEdgeMerge(Graph& target, SimpObs& simplificationObserver)
    {
        VertexEdgeMerger vertexEdgeMerger;
        vertexEdgeMerger.setThreshold(m_currentSettings.m_radius * maxBboxAxis());
        vertexEdgeMerger.setRepeatUntilConvergence(m_currentSettings.m_edgeMergeUntilConverge);
        vertexEdgeMerger(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyFaceMerge(Graph& target, SimpObs& simplificationObserver)
    {
        namespace ms = SchematLib::MapSimplification;
        // Alternative with bounding box.
        std::array<Models::Point, 2> minMaxBBPoints;
        GCpp::DS::computeBoundingBox(target, minMaxBBPoints);

        Models::NT area = (minMaxBBPoints.at(1).x() - minMaxBBPoints.at(0).x()) * (minMaxBBPoints.at(1).y() -
            minMaxBBPoints.at(0).y());
        auto areaThreshold = m_currentSettings.m_faceCollapseAreaFraction * area;
        using Selector_t = ms::FaceSelectApproach::SelectWithSmallArea<Models::NT, ms::FaceMergeTraits::Kernel>;
        Selector_t selector;
        selector.setAreaThreshold(areaThreshold);

        // using Selector_t = ms::FaceSelectApproach::SelectWithBoundedComplexity;
        // Selector_t selector(m_maxFaceComplexity);

        MapSimplification::FaceMerger<Models::UndirectedEmbeddedGraph,
                                      Selector_t, ms::FaceMergeOrders::LowestFaceComplexityOrder,
                                      ms::FaceMergeApproach::DeleteSmallFacesToNeighbourViaContraction,
                                      SimpObs> faceMerger(0, selector);
        faceMerger.setNormalizeCoordinates(false);
        faceMerger(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyContinuationFaceMerge(Graph& target, SimpObs& simplificationObserver)
    {
        namespace ms = SchematLib::MapSimplification;
        // Alternative with bounding box.
        std::array<Models::Point, 2> minMaxBBPoints;
        GCpp::DS::computeBoundingBox(target, minMaxBBPoints);

        Models::NT area = (minMaxBBPoints.at(1).x() - minMaxBBPoints.at(0).x()) * (minMaxBBPoints.at(1).y() -
            minMaxBBPoints.at(0).y());
        auto areaThreshold = m_currentSettings.m_faceCollapseAreaFraction * area;
        using Selector_t = ms::FaceSelectApproach::SelectWithSmallArea<Models::NT, ms::FaceMergeTraits::Kernel>;
        Selector_t selector;
        selector.setAreaThreshold(areaThreshold);

        // using Selector_t = ms::FaceSelectApproach::SelectWithBoundedComplexity;
        // Selector_t selector(m_maxFaceComplexity);

        MapSimplification::FaceMerger<Models::UndirectedEmbeddedGraph,
            Selector_t, ms::FaceMergeOrders::LowestFaceComplexityOrder,
            ms::FaceMergeApproach::CollapseGoodContinuityFacesByEdge,
            SimpObs> faceMerger(0, selector);
        faceMerger.setNormalizeCoordinates(false);
        faceMerger(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyBboxFaceMerge(Graph& target, SimpObs& simplificationObserver)
    {
        namespace ms = SchematLib::MapSimplification;
        // Alternative with bounding box.
        std::array<Models::Point, 2> minMaxBBPoints;
        GCpp::DS::computeBoundingBox(target, minMaxBBPoints);

        Models::NT area = (minMaxBBPoints.at(1).x() - minMaxBBPoints.at(0).x()) * (minMaxBBPoints.at(1).y() -
            minMaxBBPoints.at(0).y());
        auto areaThreshold = m_currentSettings.m_faceCollapseAreaFraction * area;
        using Selector_t = ms::FaceSelectApproach::SelectWithSmallArea<Models::NT, ms::FaceMergeTraits::Kernel>;
        Selector_t selector;
        selector.setAreaThreshold(areaThreshold);

        MapSimplification::FaceMerger<Models::UndirectedEmbeddedGraph,
            Selector_t, ms::FaceMergeOrders::LowestFaceComplexityOrder,
            ms::FaceMergeApproach::BboxFaceMerge,
            SimpObs> faceMerger(0, selector);
        faceMerger(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyLargestComponentSelect(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::LargestConnectedComponent<Graph> lcc;
        lcc(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyVertexCollapsePostFM(Graph& target, SimpObs& simplificationObserver)
    {
        Graph tempCopy = target;
        const auto radius = m_currentSettings.m_postRadius * std::max(m_bboxHeight, m_bboxWidth);
        if (m_preprocessApproach == VerexOrderApproach::DenseVertexOrder)
        {
            //// Setup merger algorithm
            MapSimplification::VertexMerger<Models::UndirectedEmbeddedGraph, Models::UndirectedGraphNodeIndex, SimpObs,
                                            MapSimplification::MergeOrders::MostDenseVertexOrder> merger(radius);
            merger.setUseMeanPosition(m_currentSettings.m_useMeanPosition);
            //// Compute merge groups
            merger(tempCopy, simplificationObserver, target);
        }
        else
        {
            //// Setup merger algorithm
            MapSimplification::VertexMerger<Models::UndirectedEmbeddedGraph, Models::UndirectedGraphNodeIndex, SimpObs>
                merger(radius);
            merger.setUseMeanPosition(m_currentSettings.m_useMeanPosition);
            //// Compute merge groups
            merger(tempCopy, simplificationObserver, target);
        }
    }

    void MapSimplificationPipeline::applyRemoveTendrilsPostFM(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::RemoveTendrils<Models::UndirectedEmbeddedGraph, SimpObs> removeTendrils;
        removeTendrils.setMaxLength(m_currentSettings.m_maxPruneLengthPostFM);

        removeTendrils(target, simplificationObserver);
    }

    void MapSimplificationPipeline::applyFaceTendrilRemove(Graph& target, SimpObs& simplificationObserver)
    {
        MapSimplification::RemoveFaceTendrils<Graph, SimpObs> remover;
        remover(target, simplificationObserver);
    }

    void MapSimplificationPipeline::Settings::loadFromTree(const boost::property_tree::ptree& tree)
    {
#define FROM_TREE(name) m_##name = tree.get<std::decay_t<decltype(m_##name)>>(#name)
        FROM_TREE(radius);
        FROM_TREE(postRadius);
        FROM_TREE(useMeanPosition);
        FROM_TREE(maxPruneLength);
        FROM_TREE(applyFaceMerge);
        FROM_TREE(maxFaceComplexity);
        FROM_TREE(faceCollapseAreaFraction);
        FROM_TREE(edgeMergeUntilConverge);
        FROM_TREE(edgeSmoothUntilConvergence);
        FROM_TREE(maxTurningAngle);
        FROM_TREE(maxPruneLengthPostFM);
        FROM_TREE(maxTurningAnglePostFM);
#undef FROM_TREE
    }

    void MapSimplificationPipeline::Settings::saveToTree(boost::property_tree::ptree& tree)
    {
#define TO_TREE(name) tree.put(#name, m_##name)
        TO_TREE(radius);
        TO_TREE(postRadius);
        TO_TREE(useMeanPosition);
        TO_TREE(maxPruneLength);
        TO_TREE(applyFaceMerge);
        TO_TREE(maxFaceComplexity);
        TO_TREE(faceCollapseAreaFraction);
        TO_TREE(edgeMergeUntilConverge);
        TO_TREE(edgeSmoothUntilConvergence);
        TO_TREE(maxTurningAngle);
        TO_TREE(maxPruneLengthPostFM);
        TO_TREE(maxTurningAnglePostFM);
#undef TO_TREE
    }

    void MapSimplificationPipeline::setSettings(const Settings& settings)
    {
        m_currentSettings = settings;
    }

    const MapSimplificationPipeline::Settings& MapSimplificationPipeline::settings() const
    {
        return m_currentSettings;
    }

    void MapSimplificationPipeline::setPreprocessApproach(
        const VerexOrderApproach& preprocessApproach)
    {
        m_preprocessApproach = preprocessApproach;
    }

    MapSimplificationPipeline::VerexOrderApproach MapSimplificationPipeline
    ::preprocessApproach() const
    {
        return m_preprocessApproach;
    }

    void MapSimplificationPipeline::setRadius(const Models::NT& radius)
    {
        m_currentSettings.m_radius = radius;
    }

    Models::NT MapSimplificationPipeline::radius() const
    {
        return m_currentSettings.m_radius;
    }

    void MapSimplificationPipeline::setPostRadius(const Models::NT& radius)
    {
        m_currentSettings.m_postRadius = radius;
    }

    Models::NT MapSimplificationPipeline::postRadius() const
    {
        return m_currentSettings.m_postRadius;
    }

    void MapSimplificationPipeline::setUseMeanPosition(const bool& useMeanPosition)
    {
        m_currentSettings.m_useMeanPosition = useMeanPosition;
    }

    bool MapSimplificationPipeline::useMeanPosition() const
    {
        return m_currentSettings.m_useMeanPosition;
    }

    void MapSimplificationPipeline::setMaxPruneLength(const Models::NT& maxPruneLength)
    {
        m_currentSettings.m_maxPruneLength = maxPruneLength;
    }

    Models::NT MapSimplificationPipeline::maxPruneLength() const
    {
        return m_currentSettings.m_maxPruneLength;
    }

    void MapSimplificationPipeline::setMaxPruneLengthPostFM(const Models::NT& maxPruneLength)
    {
        m_currentSettings.m_maxPruneLengthPostFM = maxPruneLength;
    }

    Models::NT MapSimplificationPipeline::maxPruneLengthPostFM() const
    {
        return m_currentSettings.m_maxPruneLengthPostFM;
    }

    void MapSimplificationPipeline::setApplyFaceMerge(const bool& applyFaceMerge)
    {
        m_currentSettings.m_applyFaceMerge = applyFaceMerge;
    }

    bool MapSimplificationPipeline::applyFaceMerge() const
    {
        return m_currentSettings.m_applyFaceMerge;
    }

    void MapSimplificationPipeline::setMaxFaceComplexity(const int& maxFaceComplexity)
    {
        m_currentSettings.m_maxFaceComplexity = maxFaceComplexity;
    }

    int MapSimplificationPipeline::maxFaceComplexity() const
    {
        return m_currentSettings.m_maxFaceComplexity;
    }

    void MapSimplificationPipeline::setFaceCollapseAreaFraction(
        const Models::NT& faceCollapseAreaFraction)
    {
        m_currentSettings.m_faceCollapseAreaFraction = faceCollapseAreaFraction;
    }

    Models::NT MapSimplificationPipeline::faceCollapseAreaFraction() const
    {
        return m_currentSettings.m_faceCollapseAreaFraction;
    }

    void MapSimplificationPipeline::setEdgeMergeUntilConverge(const bool& edgeMergeUntilConverge)
    {
        m_currentSettings.m_edgeMergeUntilConverge = edgeMergeUntilConverge;
    }

    bool MapSimplificationPipeline::edgeMergeUntilConverge() const
    {
        return m_currentSettings.m_edgeMergeUntilConverge;
    }

    void MapSimplificationPipeline::setEdgeSmoothUntilConvergence(
        const bool& edgeSmoothUntilConvergence)
    {
        m_currentSettings.m_edgeSmoothUntilConvergence = edgeSmoothUntilConvergence;
    }

    bool MapSimplificationPipeline::edgeSmoothUntilConvergence() const
    {
        return m_currentSettings.m_edgeSmoothUntilConvergence;
    }

    void MapSimplificationPipeline::setMaxTurningAngle(const Models::NT& maxTurningAngle)
    {
        m_currentSettings.m_maxTurningAngle = maxTurningAngle;
    }

    Models::NT MapSimplificationPipeline::maxTurningAngle() const
    {
        return m_currentSettings.m_maxTurningAngle;
    }

    void MapSimplificationPipeline::setMaxTurningAnglePostFM(const Models::NT& maxTurningAngle)
    {
        m_currentSettings.m_maxTurningAnglePostFM = maxTurningAngle;
    }

    Models::NT MapSimplificationPipeline::maxTurningAnglePostFM() const
    {
        return m_currentSettings.m_maxTurningAnglePostFM;
    }

    void MapSimplificationPipeline::apply(const Models::UndirectedEmbeddedGraph& inputGraph)
    {
        std::array<Point, 2> bboxPoints;
        GCpp::DS::computeBoundingBox(inputGraph, bboxPoints);
        m_bboxWidth = bboxPoints[1].x() - bboxPoints[0].x();
        m_bboxHeight = bboxPoints[1].y() - bboxPoints[0].y();

        namespace ms = ::SchematLib::MapSimplification;
        // Reset all stages.
        m_stages->reset();
        m_currentSettings.m_currentIteration = 0;

        SimpObs simplificationObserver;
        // Prepare observer
        // Copy the input graph
        Models::UndirectedEmbeddedGraph target = inputGraph;
        target.m_use_canonical_edges_ids = true; //Don't return canonical edge IDs during simplification.
        //
        // Setup the observer with all knowledge of the initial graph.
        {
            std::set<std::size_t> currentVertexIds;
            GCpp::DS::computeVertexIdSet(target, currentVertexIds);
            std::map<std::size_t, std::pair<std::size_t, std::size_t>> edgeMapping;
            GCpp::DS::getEdgeIdToVerticesMapping(target, edgeMapping);
            // Mark special IDs usage.
            simplificationObserver.m_useSpecialEdgeIds = true;
            simplificationObserver.initialize(currentVertexIds, edgeMapping);
        }

        // Step 0) original
        m_stages->assignNextStage(stageName("Original"), target, simplificationObserver);

        while (m_currentSettings.m_currentIteration < m_currentSettings.m_totalIterations || m_currentSettings.m_untillConvergence)
        {
            const std::size_t initialEventCount = simplificationObserver.m_eventCount;

            // 1) Preprocess (remove tendrils)
            applyRemoveTendrils(target, simplificationObserver);
            // Preprocessed
            m_stages->assignNextStage(stageName("Preprocessed"), target, simplificationObserver);

            applyLargestComponentSelect(target, simplificationObserver);
            // Largest connected component selected
            m_stages->assignNextStage(stageName("LCC"), target, simplificationObserver);

            // 1.5) Degree 2 smoothing
            applyDegree2Smoothing(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Degree2 smoothed"), target, simplificationObserver);

            // 2) Vertex collapsing
            applyVertexCollapse(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Vertex collapsed"), target, simplificationObserver);

            // 3) Vertex to edge collapsing
            applyVertexToEdgeMerge(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Vertex->edge merged"), target, simplificationObserver);

            // 4) Face merging
            //Variants

            /*applyFaceMerge(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Face merged"), target, simplificationObserver);*/

            /*applyContinuationFaceMerge(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Face merged-continuation"), target, simplificationObserver);*/

            applyBboxFaceMerge(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Face merged-bbox"), target, simplificationObserver);

            // 4.1) Remove tendrils afterwards
            //applyRemoveTendrils(target, simplificationObserver);
            applyRemoveTendrilsPostFM(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Tendrils removed after FM"), target, simplificationObserver);

            // 4.2) Collapse vertices afterwards
            //applyVertexCollapse(target, simplificationObserver);
            applyVertexCollapsePostFM(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Vertex collapsed after FM"), target, simplificationObserver);

            applyDegree2SmoothingPostFM(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Degree 2 smoothing after FM"), target, simplificationObserver);

            applyFaceTendrilRemove(target, simplificationObserver);
            m_stages->assignNextStage(stageName("Interior tendril remove after FM"), target, simplificationObserver);

            // Update iteration
            ++m_currentSettings.m_currentIteration;
            // No new events: converged
            if (m_currentSettings.m_untillConvergence && initialEventCount == simplificationObserver.m_eventCount)
            {
                break;
            }
        }
    }
}


/**
* This file is part of Intrinsic3D.
*
* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
* Copyright (c) 2019, Technical University of Munich. 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 NVIDIA CORPORATION 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 "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 OWNER 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.
*/

#include <nv/rgbd/sensor_i3d.h>

#include <iostream>
#include <sstream>
#include <string>
#include <iomanip>
#include <fstream>

#include <Eigen/Geometry>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

namespace nv
{

    SensorI3d::SensorI3d() :
		Sensor(),
		data_folder_("")
	{
	}

    SensorI3d::~SensorI3d()
	{
	}

    bool SensorI3d::init(const std::string &dataset)
	{
		// reset data
		depth_images_.clear();
		depth_timestamps_.clear();
		color_images_.clear();
		color_timestamps_.clear();
        poses_cam_to_world_.clear();
		poses_timestamps_.clear();
		data_folder_ = dataset;

        if (data_folder_.empty())
            return false;

        // load camera intrinsics
        loadIntrinsics(data_folder_ + "/depthIntrinsics.txt", cam_depth_);
        //std::cout << "depth intrinsics " << std::endl << depth_K_ << std::endl;
        loadIntrinsics(data_folder_ + "/colorIntrinsics.txt", cam_color_);
        //std::cout << "color intrinsics " << std::endl << color_K_ << std::endl;

        // load frame filenames
        std::vector<double> timestamps_depth;
        std::vector<std::string> files_depth;
        std::vector<double> timestamps_color;
        std::vector<std::string> files_color;
        std::vector<std::string> files_poses;
        if (!listFiles(timestamps_depth, files_depth, timestamps_color, files_color, files_poses))
        {
            std::cout << "filenames could not be retrieved!" << std::endl;
            return false;
        }
        std::cout << files_depth.size() << " filenames loaded." << std::endl;

        num_frames_ = static_cast<int>(files_depth.size());

        // load rgb-d frames
        for (size_t i = 0; i < files_depth.size(); ++i)
        {
            if (i % 50 == 0)
                std::cout << "Loading frame " << i << "... " << std::endl;

            // load depth image and color image
            std::vector<unsigned char> buf_color, buf_depth;
            if (!loadFrame(files_depth[i], files_color[i], buf_depth, buf_color))
                break;
            // check if input depth and color are empty
            if (buf_depth.empty() || buf_color.empty())
                continue;

            double time_depth = timestamps_depth[i];
            double time_color = timestamps_color[i];

            // get pose
            Mat4f pose_cam_to_world = Mat4f::Identity();
            double time_pose = time_depth;
            if (!loadPose(files_poses[i], pose_cam_to_world))
                break;

            // store rgb-d frame data
            depth_images_.push_back(buf_depth);
            depth_timestamps_.push_back(time_depth);

            color_images_.push_back(buf_color);
            color_timestamps_.push_back(time_color);

            poses_cam_to_world_.push_back(pose_cam_to_world);
            poses_timestamps_.push_back(time_pose);

            if (i == 0)
            {
                // frame dimensions
                cv::Mat c = color(0);
                cam_color_.setWidth(c.cols);
                cam_color_.setHeight(c.rows);
                cv::Mat d = depth(0);
                cam_depth_.setWidth(d.cols);
                cam_depth_.setHeight(d.rows);
            }

            if (num_frames_max_ > 0 && depth_images_.size() >= num_frames_max_)
                break;
        }

		return true;
	}

    bool SensorI3d::loadIntrinsics(const std::string &filename, Camera& cam) const
    {
        bool loaded = false;

        std::ifstream input_file(filename.c_str());
        if (input_file.is_open())
        {
            Mat4f K_file;
            try
            {
                //camera intrinsics
                float val = 0.0f;
                for (int i = 0; i < 4; i++) {
                    for (int j = 0; j < 4; j++) {
                        input_file >> val;
                        K_file(i, j) = val;
                    }
                }
                Mat3f K = K_file.topLeftCorner<3,3>();
                cam.setIntrinsics(K);
                input_file.close();
                loaded = true;
            }
            catch (...)
            {
            }
        }

        if (!loaded)
        {
            std::cerr << "Intrinsics file ('" << filename << "') could not be loaded!" << std::endl;
        }

        return loaded;
    }

    bool SensorI3d::listFiles(std::vector<double> &timestamps_depth,
                                          std::vector<std::string> &files_depth,
                                          std::vector<double> &timestamps_color,
                                          std::vector<std::string> &files_color,
                                          std::vector<std::string> &files_poses) const
    {
        if (data_folder_.empty())
			return false;

        // get frame filenames
        for (size_t i = 0; i < 999999; ++i)
        {
            std::stringstream ss;
            ss << data_folder_ << "/" << "frame-" << std::setfill('0') << std::setw(6) << i;
            std::string filename_base = ss.str();

            // add depth map
            std::string filename_depth = filename_base + ".depth.png";
            if (!std::ifstream(filename_depth.c_str()).is_open())
                break;
            files_depth.push_back(filename_depth);
            double timestamp_depth = static_cast<double>(i);
            timestamps_depth.push_back(timestamp_depth);

            // add color image
            std::string filename_color = filename_base + ".color.png";
            files_color.push_back(filename_color);
            double timestamp_color = timestamp_depth;
            timestamps_color.push_back(timestamp_color);

            // add pose file
            std::string filename_pose = filename_base + ".pose.txt";
            files_poses.push_back(filename_pose);
        }

		return true;
	}

    bool SensorI3d::loadFile(const std::string &filename, std::vector<unsigned char> &data) const
	{
		// read file
        unsigned char* data_file = nullptr;
		size_t size = 0;
		std::ifstream file(filename.c_str(), std::ios::in | std::ios::binary | std::ios::ate);
		if (file.is_open())
		{
			size = file.tellg();
            data_file = new unsigned char[size];
			file.seekg(0, std::ios::beg);
            file.read((char*)data_file, size);
			file.close();

			// assign data to output
            data = std::vector<unsigned char>(data_file, data_file + size);
		}
		return size != 0;
	}

    bool SensorI3d::loadFrame(const std::string &filename_depth, const std::string &filename_color,
                                  std::vector<unsigned char> &depth, std::vector<unsigned char> &color) const
	{
        if (!loadFile(filename_depth, depth))
			return false;
        if (!loadFile(filename_color, color))
			return false;

		return true;
    }

    bool SensorI3d::loadPose(const std::string &filename, Mat4f &pose) const
    {
        bool loaded = false;

        std::ifstream input_file(filename.c_str());
        if (input_file.is_open())
        {
            try
            {
                //camera intrinsics
                float val = 0.0f;
                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < 4; j++)
                    {
                        input_file >> val;
                        pose(i, j) = val;
                    }
                }
                input_file.close();
                loaded = true;
            }
            catch (...)
            {
            }
        }

        if (!loaded)
        {
            std::cerr << "Poses file ('" << filename << "') could not be loaded!" << std::endl;
            pose = Mat4f::Identity();
        }

        return loaded;
    }

    void SensorI3d::setPose(int id, const Mat4f &p)
	{
		if (!frameExists(id))
			return;
        poses_cam_to_world_[id] = p;
	}

    Mat4f SensorI3d::pose(int id)
	{
        return frameExists(id) ? poses_cam_to_world_[id] : Mat4f::Identity();
	}

    cv::Mat SensorI3d::loadDepth(int id)
	{
		if (!frameExists(id))
			return cv::Mat();

		// decode depth image
        cv::Mat depth_in = cv::imdecode(cv::Mat(depth_images_[id]), cv::IMREAD_UNCHANGED);
		cv::Mat depth;
        depth_in.convertTo(depth, CV_32FC1, (1.0 / 1000.0));
        return depth;
	}

    cv::Mat SensorI3d::loadColor(int id)
	{
		if (!frameExists(id))
			return cv::Mat();

		// decode color image
		return cv::imdecode(cv::Mat(color_images_[id]), cv::IMREAD_UNCHANGED);
	}

    double SensorI3d::timePose(int id)
	{
		return frameExists(id) ? poses_timestamps_[id] : 0.0;
	}

    double SensorI3d::timeDepth(int id)
	{
		return frameExists(id) ? depth_timestamps_[id] : 0.0;
	}

    double SensorI3d::timeColor(int id)
	{
		return frameExists(id) ? color_timestamps_[id] : 0.0;
	}

} // namespace nv


#include <simulated_lidar_scanner/scene_builder.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <urdf/model.h>
#include <boost/filesystem.hpp>

// VTK Utils
#include <vtkSTLReader.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkAppendPolyData.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <urdf/model.h>

namespace
{
vtkSmartPointer<vtkPolyData> readSTLFile(const std::string& file)
{
  vtkSmartPointer<vtkSTLReader> reader = vtkSmartPointer<vtkSTLReader>::New();
  reader->SetFileName(file.c_str());
  reader->SetMerging(1);
  reader->Update();

  return reader->GetOutput();
}

vtkSmartPointer<vtkTransform> urdfPoseToVTKTransform(const urdf::Pose& pose)
{
  urdf::Rotation q = pose.rotation;
  urdf::Vector3 p = pose.position;
  double w, x, y, z, denom;

  if (q.w > 1)
  {
    q.normalize();
  }
  w = 2 * std::acos(q.w);
  denom = std::sqrt(1 - q.w * q.w);
  if (denom < 0.001)
  {
    // Choose an arbitrary axis since the rotation angle ~= 0
    x = 0.0;  // q.x;
    y = 0.0;  // q.y;
    z = 1.0;  // q.z;
  }
  else
  {
    x = q.x / denom;
    y = q.y / denom;
    z = q.z / denom;
  }

  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  transform->Translate(p.x, p.y, p.z);
  transform->RotateWXYZ(w * 180 / M_PI, x, y, z);

  return transform;
}

Eigen::Isometry3d urdfPoseToEigen(const urdf::Pose& pose)
{
  Eigen::Isometry3d transform(Eigen::Isometry3d::Identity());
  transform.translate(Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z));
  transform.rotate(Eigen::Quaterniond(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z));

  return transform;
}

vtkSmartPointer<vtkTransform> eigenTransformToVTK(const Eigen::Isometry3d& transform)
{
  vtkSmartPointer<vtkTransform> vtk_transform = vtkSmartPointer<vtkTransform>::New();

  Eigen::Translation3d translation(transform.translation());
  vtk_transform->Translate(translation.x(), translation.y(), translation.z());

  Eigen::AngleAxisd rotation(transform.rotation());
  vtk_transform->RotateWXYZ(rotation.angle() * 180.0 / M_PI, rotation.axis()[0], rotation.axis()[1],
                            rotation.axis()[2]);

  return vtk_transform;
}

//  visualization_msgs::Marker createMeshResourceMarker(const std::string& filename,
//                                                      const Eigen::Isometry3d& transform,
//                                                      const std::string& frame,
//                                                      const int id)
//  {
//    visualization_msgs::Marker marker;
//    marker.header.frame_id = frame;
//    marker.header.stamp = ros::Time::now();
//    marker.id = id;

//    marker.action = marker.ADD;
//    marker.type = marker.MESH_RESOURCE;
//    marker.lifetime = ros::Duration(0);
//    marker.frame_locked = false;
//    marker.mesh_resource = filename;
//    marker.mesh_use_embedded_materials = true;

//    marker.scale.x = marker.scale.y = marker.scale.z = 1.0;

//    Eigen::Translation3d translation (transform.translation());
//    Eigen::AngleAxisd rotation (transform.rotation());

//    marker.pose.position.x = translation.x();
//    marker.pose.position.x = translation.y();
//    marker.pose.position.x = translation.z();
//    marker.pose.orientation.w = rotation.angle();
//    marker.pose.orientation.x = rotation.axis()[0];
//    marker.pose.orientation.y = rotation.axis()[1];
//    marker.pose.orientation.z = rotation.axis()[2];

//    return marker;
//  }
}  // namespace

namespace simulated_lidar_scanner
{
SceneObject::SceneObject(const std::string& filename, const Eigen::Isometry3d& transform)
  : filename(filename), transform(transform)
{
}

SceneObject::SceneObject(const std::string& filename, const std::vector<double>& pose)
  : filename(filename), transform(Eigen::Isometry3d::Identity())
{
  if (pose.size() != 6)
  {
    throw std::runtime_error("Pose does not have 6 elements!");
  }
  transform.translate(Eigen::Vector3d(pose[0], pose[1], pose[2]));
  transform.rotate(Eigen::AngleAxisd(pose[3], Eigen::Vector3d::UnitX()));
  transform.rotate(Eigen::AngleAxisd(pose[4], Eigen::Vector3d::UnitY()));
  transform.rotate(Eigen::AngleAxisd(pose[5], Eigen::Vector3d::UnitZ()));
}

std::vector<SceneObject> getLinkGeometry(const std::vector<urdf::VisualSharedPtr>& visuals,
                                         const urdf::Pose& joint_pose)
{
  std::vector<SceneObject> objects;
  for (auto it = visuals.begin(); it != visuals.end(); ++it)
  {
    urdf::VisualConstSharedPtr vis = *it;
    if (vis->geometry->type == urdf::Geometry::MESH)
    {
      Eigen::Isometry3d link_transform = urdfPoseToEigen(vis->origin);
      Eigen::Isometry3d joint_transform = urdfPoseToEigen(joint_pose);
      Eigen::Isometry3d transform(joint_transform * link_transform);

      SceneObject obj(std::dynamic_pointer_cast<const urdf::Mesh>(vis->geometry)->filename, transform);
      objects.push_back(obj);
    }
  }
  return objects;
}

std::vector<SceneObject> getLinkChildGeometry(const std::vector<urdf::JointSharedPtr>& joints,
                                              const urdf::Model& urdf_model_)
{
  std::vector<SceneObject> objects;

  for (auto it = joints.begin(); it != joints.end(); ++it)
  {
    urdf::JointSharedPtr joint = *it;
    if (joint->type == urdf::Joint::FIXED)
    {
      urdf::LinkSharedPtr link;
      urdf_model_.getLink(joint->child_link_name, link);
      std::vector<SceneObject> link_objects =
          getLinkGeometry(link->visual_array, joint->parent_to_joint_origin_transform);

      objects.insert(objects.end(), link_objects.begin(), link_objects.end());

      // Check if this static link has static link children
      if (link->child_joints.size() > 0)
      {
        std::vector<SceneObject> child_objects = getLinkChildGeometry(link->child_joints, urdf_model_);
        objects.insert(objects.end(), child_objects.begin(), child_objects.end());
      }
    }
  }

  return objects;
}

std::vector<SceneObject> changeFilenames(std::vector<SceneObject> scene_data_)
{
  // Create a vector of iterators to filenames that aren't in the correct format
  std::vector<std::vector<SceneObject>::iterator> erase_its;

  // Define the word to search for
  const std::string prefix = "package://";

  // Iterate through all filenames to find the defined prefix
  for (auto it = scene_data_.begin(); it != scene_data_.end(); ++it)
  {
    // Create a reference to the current file name
    SceneObject& obj = *it;

    // Find the prefix to remove in the current filename
    size_t pos = obj.filename.find(prefix);
    if (pos != std::string::npos)
    {
      // Erase prefix from filename
      obj.filename.erase(pos, prefix.length());

      // Find the first "/" in the filename (indicating end of package name)
      size_t start_pos = obj.filename.find_first_of("/", 0);

      // Create package name string and get the full package name file path
      std::string pkg_name = obj.filename.substr(pos, start_pos);
      std::string pkg_path = ros::package::getPath(pkg_name);
      if (pkg_path.length() == 0)
      {
        // Save the iterator to the filename if the ROS package where it came from is not found
        ROS_INFO("Package not found: %s", pkg_name.c_str());
        auto erase_it = std::find_if(scene_data_.begin(), scene_data_.end(),
                                     [&obj](const SceneObject& x) { return x.filename == obj.filename; });
        erase_its.push_back(erase_it);
        continue;
      }

      // Erase the package name from the front of the filename and replace with full package path
      obj.filename.erase(0, start_pos);
      obj.filename.insert(obj.filename.begin(), pkg_path.begin(), pkg_path.end());
    }
    else
    {
      // Save the iterator to the filename if it doesn't start with the defined prefix
      ROS_INFO("Filename not in correct format: %s", obj.filename.c_str());
      auto erase_it = std::find_if(scene_data_.begin(), scene_data_.end(),
                                   [&obj](const SceneObject& x) { return x.filename == obj.filename; });
      erase_its.push_back(erase_it);
    }
  }

  // Erase all filenames that weren't in the correct format
  for (auto it = erase_its.begin(); it != erase_its.end(); ++it)
  {
    scene_data_.erase(*it);
  }

  return scene_data_;
}

std::vector<SceneObject> getSceneGeometry(const urdf::Model& urdf_model_)
{
  // Get the root link of the URDF
  urdf::LinkConstSharedPtr root_link = urdf_model_.getRoot();
  if (!root_link)
    throw std::runtime_error("No root link set in URDF");

  // Check if the root link has any mesh geometry
  std::vector<urdf::VisualSharedPtr> root_link_visuals = root_link->visual_array;
  urdf::Pose base_pose;
  base_pose.position.x = base_pose.position.y = base_pose.position.z = 0.0f;
  base_pose.rotation.w = base_pose.rotation.x = base_pose.rotation.z = base_pose.rotation.w = 0.0f;
  std::vector<SceneObject> link_objects = getLinkGeometry(root_link_visuals, base_pose);

  // Check joints of the root link for static links
  std::vector<urdf::JointSharedPtr> root_joints = root_link->child_joints;
  std::vector<SceneObject> child_objects = getLinkChildGeometry(root_joints, urdf_model_);

  // Resolve package:// URI in filenames to get full file paths
  std::vector<SceneObject> objects;
  objects.insert(objects.end(), link_objects.begin(), link_objects.end());
  objects.insert(objects.end(), child_objects.begin(), child_objects.end());
  return changeFilenames(objects);
}

vtkSmartPointer<vtkPolyData> vtkSceneFromMeshFiles(const std::vector<SceneObject>& scene_data_)
{
  if (scene_data_.empty())
    throw std::runtime_error("Scene data object is empty");

  vtkSmartPointer<vtkAppendPolyData> append_filter = vtkSmartPointer<vtkAppendPolyData>::New();
  for (auto it = scene_data_.begin(); it != scene_data_.end(); ++it)
  {
    boost::filesystem::path path(it->filename);
    if (!boost::filesystem::exists(path))
      throw std::runtime_error("File at '" + it->filename + "' does not exist");

    vtkSmartPointer<vtkPolyData> vtk_poly = readSTLFile(it->filename);
    if (!vtk_poly)
      throw std::runtime_error("Unable to read file '" + it->filename + "'");

    // Apply the input transformation to the VTK poly data
    vtkSmartPointer<vtkTransformPolyDataFilter> transform_filter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    vtkSmartPointer<vtkTransform> transform = eigenTransformToVTK(it->transform);
    transform_filter->SetInputData(vtk_poly);
    transform_filter->SetTransform(transform);
    transform_filter->Update();

    // Append this VTK object to the previous geometries
    append_filter->AddInputData(transform_filter->GetOutput());
  }
  append_filter->Update();

  return append_filter->GetOutput();
}

vtkSmartPointer<vtkPolyData> createVTKSceneFromURDF(const std::string& urdf_parameter)
{
  urdf::Model urdf_model_;
  if (!urdf_model_.initParam(urdf_parameter))
    throw std::runtime_error("URDF parameter '" + urdf_parameter + "' must be set");

  // Get filenames for the meshes of all static links
  std::vector<SceneObject> scene_data_ = getSceneGeometry(urdf_model_);
  if (scene_data_.empty())
    throw std::runtime_error("No static geometry present in URDF");

  return vtkSceneFromMeshFiles(scene_data_);
}

vtkSmartPointer<vtkPolyData> createVTKSceneFromSceneObjects(std::vector<SceneObject> scene_objects)
{
  // Replace package URI in filenames
  return vtkSceneFromMeshFiles(changeFilenames(scene_objects));
}

}  // namespace simulated_lidar_scanner


//
//  solver.hpp
//  solver
//
//  Created by Alexandre Martin on 07/02/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#ifndef SOLVER_HPP
#define SOLVER_HPP

#include "cnf.hpp"
#include "detail/clause.hpp"
#include <vector>
#include <random>
#include <list>
#include <boost/heap/d_ary_heap.hpp>

enum class guess_mode {
    LINEAR,
    RAND,
    MOMS,
    DLIS,
    VSIDS,
};

enum class cdcl_mode {
    NONE,
    NORMAL,
    INTERACTIVE,
};

class equality_solver;

struct options {
    guess_mode guess = guess_mode::LINEAR;
    cdcl_mode cdcl = cdcl_mode::NONE;
    bool wl = false;
    bool forget = false;
    equality_solver * eq_solver = nullptr;
};

class solver {
private:
    void (solver::*m_backtrack_one)(int) = nullptr;
    detail::clause * (solver::*m_deduce_one)(int, int) = nullptr;
    int (solver::*m_guess)() = nullptr;

    bool m_first_propagation_round = true;
    options m_options;

    std::vector<int> m_old_variables;

    /* member so as to avoid allocations in clause learning
       (we can't bear the use of an unordered_set here, profiling has shown
       that it was less performant) */
    std::vector<bool> m_already_seen;

    // preallocated vector for MOMS
    std::vector<size_t> m_moms_counts;
    int m_min_clause;

    class heap_data {
    private:
        int m_var;
        solver * m_father;

    public:
        heap_data(int var, solver * father) : m_var { var }, m_father { father } { }

        int var() const {
            return m_var;
        }

        bool operator <(heap_data const & rhs) const {
            return m_father->m_vsids_score[m_var].first < m_father->m_vsids_score[rhs.m_var].first;
        }
    };

    using vsids_heap = boost::heap::d_ary_heap<
        heap_data,
        boost::heap::mutable_<true>,
        boost::heap::arity<2>
    >;

    vsids_heap m_vsids_heap;
    std::vector<std::pair<double, vsids_heap::handle_type>> m_vsids_score;

    size_t m_remaining_variables;

    detail::clause m_dummy_clause;
    bool m_first = true;

    std::list<detail::clause> m_clauses;
    guess_mode m_guess_mode;
    cdcl_mode m_cdcl;

    std::mt19937 m_rng;

    // the total number of clauses not yet satisfied
    int m_remaining_clauses;

    std::vector<std::vector<detail::clause *>> m_watches;

    // for clause removal
    using clause_it = std::list<detail::clause>::iterator;
    std::vector<clause_it> m_learnt;

    // valuation stack
    std::vector<int> m_valuation;

    // current assignment of variables
    detail::assignment m_assignment;

    void enqueue(int, int, detail::clause *);
    int dequeue();

    detail::clause * deduce_one_wl(int, int);
    detail::clause * deduce_one_default(int, int);
    detail::clause * deduce(int);

    void backtrack_one_wl(int);
    void backtrack_one_default(int);
    int backtrack(int);

    int new_to_old_lit(int);

    double calculate_score(int);
    int guess_linear();
    int guess_moms();
    int guess_dlis();
    int guess_rand();
    int guess_vsids();

    bool can_learn() const {
        return m_options.cdcl != cdcl_mode::NONE;
    }

    void interac(detail::clause *, int, int);
    void draw(detail::clause *, int, int);

    std::pair<detail::clause, int> learn(detail::clause *, int);
    detail::clause * add_clause(detail::clause);
    void remove_learnt(size_t);
public:
    solver() = default;
    solver(cnf, options);
    valuation solve();

    bool satisfiable() {
        if (m_first || m_remaining_clauses > 0)
            solve();
        return m_remaining_clauses != -1;
    }
};

#endif


// Copyright (c) 2014-2021 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <consensus/amount.h>
#include <net.h>
#include <signet.h>
#include <uint256.h>
#include <validation.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(validation_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    const int maxHalvings = 64;
    const CAmount nInitialSubsidy = consensusParams.initialSubsidy;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        /* In Xaya, we have the special rule that the block reward was set
           to 1 CHI at the beginning of the first halving period (before the
           POST_ICO fork).  Thus we use the *end* of the halving period
           for the purpose of this test.  */
        nHeight += consensusParams.nSubsidyHalvingInterval - 1;

        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    /* Testing other intervals as is done upstream doesn't work, as we would
       need to craft a ConsensusRules instance for the POST_ICO fork check.  */
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    CAmount nSum = 0;
    CAmount nLastSubsidy;
    int nLastHeight = 0;

    /* Updates the current state (contained in the variables this closes over)
       based on the block subsidy at the given height, assuming that it is the
       same from nLastHeight+1 up to height.  */
    const auto updateState = [&] (const int height)
      {
        const int numBlocks = height - nLastHeight;
        BOOST_CHECK(numBlocks > 0);

        const CAmount nSubsidy = GetBlockSubsidy(height, chainParams->GetConsensus());
        BOOST_CHECK(nSubsidy <= 4 * COIN);

        nSum += nSubsidy * numBlocks;
        nLastSubsidy = nSubsidy;
        BOOST_CHECK(MoneyRange(nSum));
        nLastHeight = height;
      };

    updateState(1);
    updateState(439999);
    BOOST_CHECK_EQUAL(nLastSubsidy, 1 * COIN);
    updateState(440000);
    BOOST_CHECK(nLastSubsidy > 3 * COIN);
    updateState(4199999);
    BOOST_CHECK(nLastSubsidy > 3 * COIN);
    updateState(4200000);
    BOOST_CHECK(nLastSubsidy < 2 * COIN);

    /* We need to call updateState always at the *last* block that has
       a certain subsidy.  Thus start iterating on the next block batch
       (100k more than first halving), and always update on height-1.  */
    for (int height = 4300000; height < 140000000; height += 100000)
        updateState(height - 1);

    BOOST_CHECK_EQUAL(nLastSubsidy, CAmount{0});
    /* This is the real PoW coin supply, taking rounding into account.  The
       final coin supply is larger by the presale amount.  */
    BOOST_CHECK_EQUAL(nSum, CAmount{3092157231160000});
}

BOOST_AUTO_TEST_CASE(subsidy_post_ico_fork_test)
{
    const auto main = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(439999, main->GetConsensus()), COIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(440000, main->GetConsensus()), 382934346);

    const auto test = CreateChainParams(*m_node.args, CBaseChainParams::TESTNET);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(10999, test->GetConsensus()), COIN);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(11000, test->GetConsensus()), 10 * COIN);

    const auto regtest = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST);
    BOOST_CHECK_EQUAL(GetBlockSubsidy(1, regtest->GetConsensus()), 50 * COIN);
}

BOOST_AUTO_TEST_CASE(signet_parse_tests)
{
    ArgsManager signet_argsman;
    signet_argsman.ForceSetArg("-signetchallenge", "51"); // set challenge to OP_TRUE
    const auto signet_params = CreateChainParams(signet_argsman, CBaseChainParams::SIGNET);
    CBlock block;
    BOOST_CHECK(signet_params->GetConsensus().signet_challenge == std::vector<uint8_t>{OP_TRUE});
    CScript challenge{OP_TRUE};

    // empty block is invalid
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no witness commitment
    CMutableTransaction cb;
    cb.vout.emplace_back(0, CScript{});
    block.vtx.push_back(MakeTransactionRef(cb));
    block.vtx.push_back(MakeTransactionRef(cb)); // Add dummy tx to exercise merkle root code
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no header is treated valid
    std::vector<uint8_t> witness_commitment_section_141{0xaa, 0x21, 0xa9, 0xed};
    for (int i = 0; i < 32; ++i) {
        witness_commitment_section_141.push_back(0xff);
    }
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // no data after header, valid
    std::vector<uint8_t> witness_commitment_section_325{0xec, 0xc7, 0xda, 0xa2};
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // Premature end of data, invalid
    witness_commitment_section_325.push_back(0x01);
    witness_commitment_section_325.push_back(0x51);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // has data, valid
    witness_commitment_section_325.push_back(0x00);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(SignetTxs::Create(block, challenge));
    BOOST_CHECK(CheckSignetBlockSolution(block, signet_params->GetConsensus()));

    // Extraneous data, invalid
    witness_commitment_section_325.push_back(0x00);
    cb.vout.at(0).scriptPubKey = CScript{} << OP_RETURN << witness_commitment_section_141 << witness_commitment_section_325;
    block.vtx.at(0) = MakeTransactionRef(cb);
    BOOST_CHECK(!SignetTxs::Create(block, challenge));
    BOOST_CHECK(!CheckSignetBlockSolution(block, signet_params->GetConsensus()));
}

//! Test retrieval of valid assumeutxo values.
BOOST_AUTO_TEST_CASE(test_assumeutxo)
{
    const auto params = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST);

    // These heights don't have assumeutxo configurations associated, per the contents
    // of chainparams.cpp.
    std::vector<int> bad_heights{0, 100, 111, 115, 209, 211};

    for (auto empty : bad_heights) {
        const auto out = ExpectedAssumeutxo(empty, *params);
        BOOST_CHECK(!out);
    }

    const auto out110 = *ExpectedAssumeutxo(110, *params);
    BOOST_CHECK_EQUAL(out110.hash_serialized.ToString(), "dc81af66a58085fe977c6aab56b49630d87b84521fc5a8a5c53f2f4b23c8d6d5");
    BOOST_CHECK_EQUAL(out110.nChainTx, 110U);

    const auto out210 = *ExpectedAssumeutxo(200, *params);
    BOOST_CHECK_EQUAL(out210.hash_serialized.ToString(), "51c8d11d8b5c1de51543c579736e786aa2736206d1e11e627568029ce092cf62");
    BOOST_CHECK_EQUAL(out210.nChainTx, 200U);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <algorithm>
#include <numeric>
#include <functional>
#include <fstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include "FactorGraph.h"

namespace Grante {

// private
FactorGraph::FactorGraph() {
}

FactorGraph::FactorGraph(const FactorGraphModel* model,
	const std::vector<unsigned int>& card)
	: model(model), cardinalities(card) {
}

FactorGraph::~FactorGraph() {
	for (unsigned int fi = 0; fi < factors.size(); ++fi)
		delete (factors[fi]);

	for (unsigned int dsi = 0; dsi < datasources.size(); ++dsi)
		delete (datasources[dsi]);
}

const FactorGraphModel* FactorGraph::Model() const {
	return (model);
}

const std::vector<Factor*>& FactorGraph::Factors() const {
	return (factors);
}

const std::vector<unsigned int>& FactorGraph::Cardinalities() const {
	return (cardinalities);
}

void FactorGraph::ForwardMap() {
	#pragma omp critical
	{
		// Simply perform forward map for each factor
		for (std::vector<Factor*>::iterator fi = factors.begin();
			fi != factors.end(); ++fi) {
			(*fi)->ForwardMap();
		}
	}
}

void FactorGraph::EnergiesRelease() {
	for (std::vector<Factor*>::iterator fi = factors.begin();
		fi != factors.end(); ++fi) {
		(*fi)->EnergiesRelease();
	}
}

double FactorGraph::EvaluateEnergy(
	const std::vector<unsigned int>& state) const {
	assert(state.size() == cardinalities.size());

	// Sum energy of all factors
	double energy = 0.0;
	for (std::vector<Factor*>::const_iterator fi = factors.begin();
		fi != factors.end(); ++fi) {
		energy += (*fi)->EvaluateEnergy(state);
	}
	return (energy);
}

double FactorGraph::EvaluateEnergy(
	const std::vector<std::vector<double> >& exp) const {
	assert(exp.size() == factors.size());
	// Sum energy of all factors
	double energy = 0.0;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		assert(factors[fi]->Energies().size() == exp[fi].size());
		energy += std::inner_product(exp[fi].begin(), exp[fi].end(),
			factors[fi]->Energies().begin(), 0.0);
	}
	return (energy);
}

double FactorGraph::EvaluateEnergy(const FactorGraphObservation* obs) const {
	if (obs->Type() == FactorGraphObservation::DiscreteLabelingType)
		return (EvaluateEnergy(obs->State()));

	assert(obs->Type() == FactorGraphObservation::ExpectationType);
	return (EvaluateEnergy(obs->Expectation()));
}

void FactorGraph::ScaleEnergies(double factor) {
	for (std::vector<Factor*>::const_iterator fi = factors.begin();
		fi != factors.end(); ++fi) {
		std::vector<double>& fac_energies = (*fi)->Energies();
		// E[n] *= factor
		std::transform(fac_energies.begin(), fac_energies.end(),
			fac_energies.begin(),
			std::bind2nd(std::multiplies<double>(), factor));
	}
}

void FactorGraph::AddFactor(Factor* factor) {
	factors.push_back(factor);
}

void FactorGraph::AddDataSource(const FactorDataSource* datasource) {
	datasources.push_back(datasource);
}

void FactorGraph::Save(const std::string& filename) const {
	std::ofstream ofs(filename.c_str());
	{
		boost::archive::text_oarchive oa(ofs);
		oa << *this;
	}
}

FactorGraph* FactorGraph::Load(const std::string& filename) {
	FactorGraph* fg = new FactorGraph();
	{
		std::ifstream ifs(filename.c_str());
		boost::archive::text_iarchive ia(ifs);
		ia >> *fg;
	}
	return (fg);
}

void FactorGraph::Print() const {
	std::cout << "FACTORGRAPH BEGIN" << std::endl;
	std::cout << cardinalities.size() << " variables, "
		<< factors.size() << " factors" << std::endl;
	std::cout << "VARIABLES BEGIN" << std::endl;
	for (unsigned int vi = 0; vi < cardinalities.size(); ++vi) {
		std::cout << "  " << vi << " (" << cardinalities[vi]
			<< ") appears in factors:";
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			if (std::find(factors[fi]->Variables().begin(),
				factors[fi]->Variables().end(), vi) ==
				factors[fi]->Variables().end())
				continue;
			std::cout << " " << fi;
		}
		std::cout << std::endl;
	}
	std::cout << "VARIABLES END" << std::endl;
	std::cout << "FACTORS BEGIN" << std::endl;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		std::cout << "  " << fi << " ("
			<< factors[fi]->Variables().size() << ", '"
			<< factors[fi]->Type()->Name() << "'), data size "
			<< factors[fi]->Data().size() << " acts on var:";
		for (unsigned int vi = 0; vi < factors[fi]->Variables().size(); ++vi) {
			std::cout << " " << factors[fi]->Variables()[vi];
		}
		std::cout << std::endl;
	}
	std::cout << "FACTORS END" << std::endl;
	std::cout << "FACTORGRAPH END" << std::endl;
}

}


#ifndef IDMLIB_ISE_COMMON_HPP
#define IDMLIB_ISE_COMMON_HPP

#include <idmlib/idm_types.h>

#include <string>
#include <vector>
#include <exception>

#include <boost/assert.hpp>
#include <boost/foreach.hpp>

namespace idmlib{ namespace ise{

typedef uint64_t Chunk;

static const unsigned CHUNK_BIT = sizeof(Chunk) * 8;

static const unsigned SKETCH_BIT = 128;
static const unsigned SKETCH_SIZE = SKETCH_BIT / CHUNK_BIT;
static const unsigned SKETCH_PLAN_DIST = 3;
static const unsigned SKETCH_DIST = 4; // accept dist < 4
static const unsigned SKETCH_TIGHT_DIST = 3; // accept dist < 4
static const unsigned SKETCH_DIST_OFFLINE = 3;

static const unsigned MIN_IMAGE_SIZE = 50;
static const unsigned MIN_QUERY_IMAGE_SIZE = 20;
static const unsigned QUERY_IMAGE_SCALE_THRESHOLD = 80;
static const unsigned MAX_IMAGE_SIZE = 250;

typedef Chunk SketchData[SKETCH_SIZE];

struct Sketch
{
    SketchData desc;

    int compare(const Sketch& other) const
    {
        for (unsigned i = 0; i < SKETCH_SIZE; i++)
        {
            if (desc[i] < other.desc[i])
                return -1;
            else if (desc[i] > other.desc[i])
                return 1;
        }
        return 0;
    }

    bool operator<(const Sketch& other) const
    {
        return compare(other) == -1;
    }

    bool operator!=(const Sketch& other) const
    {
        return compare(other) != 0;
    }
};

static const unsigned SIMHASH_BIT = 64;
static const unsigned SIMHASH_SIZE = SIMHASH_BIT / CHUNK_BIT;

typedef Chunk SimHashData[SIMHASH_SIZE];

struct SimHash
{
    SimHashData desc;

    int compare(const SimHash& other) const
    {
        for (unsigned i = 0; i < SIMHASH_SIZE; i++)
        {
            if (desc[i] < other.desc[i])
                return -1;
            else if (desc[i] > other.desc[i])
                return 1;
        }
        return 0;
    }

    bool operator<(const SimHash& other) const
    {
        return compare(other) == -1;
    }

    bool operator!=(const SimHash& other) const
    {
        return compare(other) != 0;
    }
};

struct Region
{
    float x, y, r, t;
};

}}

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include <armnnDeserializer/IDeserializer.hpp>

#include <string>

BOOST_AUTO_TEST_SUITE(Deserializer)

struct Pooling2dFixture : public ParserFlatbuffersSerializeFixture
{
    explicit Pooling2dFixture(const std::string &inputShape,
                              const std::string &outputShape,
                              const std::string &dataType,
                              const std::string &dataLayout,
                              const std::string &poolingAlgorithm)
    {
        m_JsonString = R"(
    {
            inputIds: [0],
            outputIds: [2],
            layers: [
            {
                layer_type: "InputLayer",
                layer: {
                      base: {
                            layerBindingId: 0,
                            base: {
                                index: 0,
                                layerName: "InputLayer",
                                layerType: "Input",
                                inputSlots: [{
                                    index: 0,
                                    connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                }],
                                outputSlots: [ {
                                    index: 0,
                                    tensorInfo: {
                                        dimensions: )" + inputShape + R"(,
                                        dataType: )" + dataType + R"(
                                        }}]
                                }
                }}},
                {
                layer_type: "Pooling2dLayer",
                layer: {
                      base: {
                           index: 1,
                           layerName: "Pooling2dLayer",
                           layerType: "Pooling2d",
                           inputSlots: [{
                                  index: 0,
                                  connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                           }],
                           outputSlots: [ {
                                  index: 0,
                                  tensorInfo: {
                                       dimensions: )" + outputShape + R"(,
                                       dataType: )" + dataType + R"(

                           }}]},
                      descriptor: {
                           poolType: )" + poolingAlgorithm + R"(,
                           outputShapeRounding: "Floor",
                           paddingMethod: Exclude,
                           dataLayout: )" + dataLayout + R"(,
                           padLeft: 0,
                           padRight: 0,
                           padTop: 0,
                           padBottom: 0,
                           poolWidth: 2,
                           poolHeight: 2,
                           strideX: 2,
                           strideY: 2
                           }
                }},
                {
                layer_type: "OutputLayer",
                layer: {
                    base:{
                          layerBindingId: 0,
                          base: {
                                index: 2,
                                layerName: "OutputLayer",
                                layerType: "Output",
                                inputSlots: [{
                                    index: 0,
                                    connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                                }],
                                outputSlots: [ {
                                    index: 0,
                                    tensorInfo: {
                                        dimensions: )" + outputShape + R"(,
                                        dataType: )" + dataType + R"(
                                    },
                            }],
                        }}},
            }]
     }
 )";
        SetupSingleInputSingleOutput("InputLayer", "OutputLayer");
    }
};

struct SimpleAvgPooling2dFixture : Pooling2dFixture
{
    SimpleAvgPooling2dFixture() : Pooling2dFixture("[ 1, 2, 2, 1 ]",
                                                   "[ 1, 1, 1, 1 ]",
                                                   "Float32", "NHWC", "Average") {}
};

struct SimpleAvgPooling2dFixture2 : Pooling2dFixture
{
    SimpleAvgPooling2dFixture2() : Pooling2dFixture("[ 1, 2, 2, 1 ]",
                                                    "[ 1, 1, 1, 1 ]",
                                                    "QuantisedAsymm8", "NHWC", "Average") {}
};

struct SimpleMaxPooling2dFixture : Pooling2dFixture
{
    SimpleMaxPooling2dFixture() : Pooling2dFixture("[ 1, 1, 2, 2 ]",
                                                   "[ 1, 1, 1, 1 ]",
                                                   "Float32", "NCHW", "Max") {}
};

struct SimpleMaxPooling2dFixture2 : Pooling2dFixture
{
    SimpleMaxPooling2dFixture2() : Pooling2dFixture("[ 1, 1, 2, 2 ]",
                                                    "[ 1, 1, 1, 1 ]",
                                                    "QuantisedAsymm8", "NCHW", "Max") {}
};

struct SimpleL2Pooling2dFixture : Pooling2dFixture
{
    SimpleL2Pooling2dFixture() : Pooling2dFixture("[ 1, 2, 2, 1 ]",
                                                  "[ 1, 1, 1, 1 ]",
                                                  "Float32", "NHWC", "L2") {}
};

BOOST_FIXTURE_TEST_CASE(Pooling2dFloat32Avg, SimpleAvgPooling2dFixture)
{
    RunTest<4, armnn::DataType::Float32>(0, { 2, 3, 5, 2 }, { 3 });
}

BOOST_FIXTURE_TEST_CASE(Pooling2dQuantisedAsymm8Avg, SimpleAvgPooling2dFixture2)
{
    RunTest<4, armnn::DataType::QAsymmU8>(0,{ 20, 40, 60, 80 },{ 50 });
}

BOOST_FIXTURE_TEST_CASE(Pooling2dFloat32Max, SimpleMaxPooling2dFixture)
{
    RunTest<4, armnn::DataType::Float32>(0, { 2, 5, 5, 2 }, { 5 });
}

BOOST_FIXTURE_TEST_CASE(Pooling2dQuantisedAsymm8Max, SimpleMaxPooling2dFixture2)
{
    RunTest<4, armnn::DataType::QAsymmU8>(0,{ 20, 40, 60, 80 },{ 80 });
}

BOOST_FIXTURE_TEST_CASE(Pooling2dFloat32L2, SimpleL2Pooling2dFixture)
{
    RunTest<4, armnn::DataType::Float32>(0, { 2, 3, 5, 2 }, { 3.2403703f });
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef PERFORMANCE_HPP
#define PERFORMANCE_HPP

#include <boost/math/special_functions/relative_difference.hpp>
#include <boost/array.hpp>
#include <boost/chrono.hpp>
#include <boost/regex.hpp>
#include <iostream>
#include <iomanip>

extern std::vector<std::vector<double> > data;

template <class Array>
void add_data(const Array& a)
{
   //
   // This function is called multiple times to merge multiple data sets into one big table:
   //
   for(typename Array::const_iterator i = a.begin(); i != a.end(); ++i)
   {
      data.push_back(std::vector<double>());
      for(typename Array::value_type::const_iterator j = i->begin(); j != i->end(); ++j)
      {
         data.back().push_back(*j);
      }
   }
}

template <class Func, class Result>
void screen_data(Func f, Result r)
{
   //
   // If any of the implementations being tested produces garbage for one of our
   // test cases (or else if we test a domain they don't support), then we remove that
   // row from the table.  This allows us to only test a common supported sub-set for performance:
   //
   for(std::vector<std::vector<double> >::size_type row = 0; row < data.size(); ++row)
   {
      try
      {
         double computed = f(data[row]);
         double expected = r(data[row]);
         double err = boost::math::relative_difference(computed, expected);
         if(err > 1e-7)
         {
            std::cout << "Erasing row: ";
            for(unsigned i = 0; i < data[row].size(); ++i)
            {
               std::cout << data[row][i] << " ";
            }
            std::cout << "Error was " << err << std::endl;
            data.erase(data.begin() + row);
            --row;
         }
      }
      catch(const std::exception& e)
      {
         std::cout << "Erasing row: ";
         for(unsigned i = 0; i < data[row].size(); ++i)
         {
            std::cout << data[row][i] << " ";
         }
         std::cout << "due to thrown exception: " << e.what() << std::endl;
         data.erase(data.begin() + row);
         --row;
      }
   }
}

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

private:
   typename Clock::time_point m_start;
};

double sum = 0;

template <class Func>
double exec_timed_test(Func f)
{
   double t = 0;
   unsigned repeats = 1;
   do{
      stopwatch<boost::chrono::high_resolution_clock> w;

      for(unsigned count = 0; count < repeats; ++count)
      {
         for(std::vector<std::vector<double> >::const_iterator i = data.begin(); i != data.end(); ++i)
            sum += f(*i);
      }

      t = boost::chrono::duration_cast<boost::chrono::duration<double>>(w.elapsed()).count();
      if(t < 0.5)
         repeats *= 2;
   } while(t < 0.5);
   return t / (repeats * data.size());
}

#endif // PERFORMANCE_HPP


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include <boost/test/unit_test.hpp>
#include <boost/mpl/vector.hpp>

#include "array_parameters.h"
#include "array_parameters_dense.h"

#include "test_common.h"

constexpr auto LENGTH = 16;
constexpr auto STRIDE_SHIFT = 2;

using weight_types = boost::mpl::vector<sparse_parameters, dense_parameters>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_default_function_weight_initialization_strided_index, T, weight_types)
{
  T w(LENGTH, STRIDE_SHIFT);
  auto weight_initializer = [](weight* weights, uint64_t index) { weights[0] = 1.f * index; };
  w.set_default(weight_initializer);
  for (size_t i = 0; i < LENGTH; i++) { BOOST_CHECK_CLOSE(w.strided_index(i), 1.f * (i * w.stride()), FLOAT_TOL); }
}

#ifdef PRIVACY_ACTIVATION
BOOST_AUTO_TEST_CASE_TEMPLATE(test_feature_is_activated, T, weight_types)
{
  // unit test to check if number of bits equal to the threshold in
  // bitset set to 1 for a feature returns true
  int threshold = 10;
  T w(LENGTH, STRIDE_SHIFT);
  w.privacy_activation_threshold(threshold);
  int feature_index = 0;
  // function to set the bits in bitset to 1 equal to the threshold
  for (int tag_hash = 0; tag_hash < threshold; tag_hash++)
  {
    w.set_tag(tag_hash);
    auto& weight = w[feature_index];
  }
  BOOST_CHECK_EQUAL(w.is_activated(feature_index), true);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_feature_not_activated, T, weight_types)
{
  // unit test to check if number of bits less than the threshold in
  // bitset set to 1 for a feature returns false
  int threshold = 10;
  T w(LENGTH, STRIDE_SHIFT);
  w.privacy_activation_threshold(threshold);
  int feature_index = 0;
  // function to set the bits in bitset to 1 equal to the (threshold-1)
  for (int tag_hash = 0; tag_hash < (threshold - 1); tag_hash++)
  {
    w.set_tag(tag_hash);
    auto& weight = w[feature_index];
  }
  BOOST_CHECK_EQUAL(w.is_activated(feature_index), false);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_feature_could_be_activated_but_feature_not_initialized, T, weight_types)
{
  // unit test to check if number of bits equal to the threshold in
  // bitset set to 1 for a feature returns true
  int threshold = 10;
  T w(LENGTH, STRIDE_SHIFT);
  int feature_index = 0;
  // function to set the bits in bitset to 1 equal to the threshold
  for (int tag_hash = 0; tag_hash < threshold; tag_hash++)
  {
    w.set_tag(tag_hash);
    auto& weight = w[feature_index];
  }
  BOOST_CHECK_EQUAL(w.is_activated(feature_index), false);
}
#endif


//
// SPDX-License-Identifier: MIT
// Copyright (c) 2016-2020 Michael Purcaro, Henry Pratt, Jill Moore, Zhiping Weng
//

#include <vector>
#include <iostream>
#include <string>
#include <unordered_map>
#include <fstream>
#include <sstream>
#include <cmath>
#include <numeric>
#include <memory>
#include <armadillo>

#include <zi/zargs/zargs.hpp>

//ZiARG_bool(boolExample, false, "bool example");
ZiARG_int32(j, 1, "cores");
ZiARG_string(rootPath, "/data/projects/cREs/", "root path");

#include <boost/filesystem.hpp>
#include <boost/filesystem/operations.hpp>

// #include "hypertorus/src/tensor.hpp"
// #include "hypertorus/src/hosvd.hpp"

#include "zentLib/src/BigWigWrapper.hpp"

#include "cpp/files.hpp"
#include "cpp/string_utils.hpp"

#include "common/utils.hpp"
#include "common/lambda.hpp"
#include "common/region.hpp"
#include "paths.hpp"
#include "lookup_matrix.hpp"
#include "common/zscore.hpp"
#include "common/rDHS.hpp"
#include "common/saturation.hpp"

// #include "epitensor/epitensor.hpp"

#include "encode.hpp"
#include "cistrome.hpp"

namespace SCREEN {

  /* void run_cistrome_rDHS(const std::string &assembly, bool force_recompute = false) {

    // load list of files
    std::vector<bfs::path> cistrome_list;
    std::vector<bfs::path> cistrome_comp;

    const auto lines = bib::files::readStrings(path.cistromeList());
    std::vector<int> comp_idx;
    std::vector<int> read_idx;

    int i = 0;
    for(const auto& line : lines){
      cistrome_list.push_back(line);
      cistrome_comp.push_back(path.DHS_ZScore(trim_ext(basename(cistrome_list[i])), "cistrome").string());
      if (force_recompute || !bfs::exists(cistrome_comp[i])) {
	comp_idx.push_back(i++);
      } else {
	read_idx.push_back(i++);
      }
    }

#pragma omp parallel for
    for (auto n = 0; n < comp_idx.size(); ++n) {
      int i = comp_idx[n];
      ZScore z(cistrome_list[i], false);
      z.qfilter(0.001);
      z.write(trim_ext(basename(cistrome_list[i])),
	      cistrome_comp[i]);
      std::cout << cistrome_list[i] << "\t" << z.zscores_.size() << "\n";
    }

    // compute rDHSs
    std::cout << "computing rDHSs for " << cistrome_comp.size() << " exps\n";
    for (const auto& fnp : cistrome_comp) {
      std::cout << fnp << "\n";
    }
    rDHS rr(cistrome_comp);
  } */

} // namespace SCREEN

void ENCODE_pipeline_run(SCREEN::ENCODE e) {
  std::cout << "making saturation\n";
  e.make_saturation();
  return;
  std::cout << "making binary signal files\n";
  e.binarizeDHS();
  std::cout << "computing Z-scores...\n";
  e.computeZScores();
  std::cout << "making rDHSs...\n";
  e.make_rDHS();
  std::cout << "making binary rDHS...\n";
  e.binarize_rDHS();
  std::cout << "creating cREs...\n";
  e.create_cREs();
}

/*
 *  test entry point
 */
int main(int argc, char **argv){

  zi::parse_arguments(argc, argv, true);  // modifies argc and argv
  std::vector<std::string> args(argv, argv + argc); // remaining arguments

  // cistrome hg38
  std::cout << "*** Cistrome hg38 ***\n";
  SCREEN::Cistrome c("/data/projects/cistrome/data/raw", ZiARG_rootPath, "hg38");
  std::cout << "computing Z-scores...\n";
  c.computeZScores();
  std::cout << "creating rDHSs...\n";
  c.make_rDHS();

  // hg38
  std::cout << "*** ENCODE hg38 ***\n";
  ENCODE_pipeline_run(SCREEN::ENCODE(ZiARG_rootPath, "hg38"));

  // hg19
  std::cout << "*** ENCODE hg19 ***\n";
  ENCODE_pipeline_run(SCREEN::ENCODE(ZiARG_rootPath, "hg19"));

  // mm10
  std::cout << "*** ENCODE mm10 ***\n";
  ENCODE_pipeline_run(SCREEN::ENCODE(ZiARG_rootPath, "mm10"));

  /*
  std::cout << "correlating...\n";
  e.runCorrelation();
  */

  //std::cout << "computing density...\n";
  //e.compute_density("CTCF", e.ctcf_list_, 10000);
  //e.compute_density("H3K4me3", e.h3k4me3_list_, 10000);
  //e.similar_DNase_jaccard();

  //run_saturation("hg19");

  //std::cout << "\n*** ENCODE mm10 ***\n";
  //SCREEN::run_rDHS("mm10");
  //SCREEN::run_cistrome_rDHS("mm10");

  return 0;
}


/*
 *  testPMLField.h
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 2/12/12.
 *  Copyright 2012 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestPMLField

#include <boost/test/unit_test.hpp>
#include <cmath>

#include "PMLField.h"
#include "PhysicalConstants.h"

using namespace std;

// Test the static function to return depth into the PML
BOOST_AUTO_TEST_CASE( TestPMLDepth_Left )
{
    Rect3i yeeCells(0, 0, 0, 99, 0, 0);
    Rect3i nonPMLHalfCells(20, 0, 0, 199, 1, 1);
    PhysicalExtents extents(yeeCells, nonPMLHalfCells);
    BOOST_CHECK(extents.hasPML(0));
    BOOST_CHECK(false == extents.hasPML(1));

    RLE::DynamicRLE3<double> depth = pmlDepth(extents, 0);

    BOOST_CHECK(depth.hasDimension(0));
    BOOST_CHECK(false == depth.hasDimension(1));
    BOOST_CHECK(false == depth.hasDimension(2));

    Rect3i leftPML = extents.pmlHalfCells(0);
    BOOST_CHECK_EQUAL(leftPML.count(), 4*20);

    for (int nn = 0; nn < 20; nn++)
    {
        double d = (20-nn)/20.0;
        BOOST_CHECK_CLOSE(depth.at(nn,0,0), d, 1e-6);
    }
}

// The way PML works is:
// - the outermost non-PML cell has depth = 0
// - the outermost PML cell has depth = 1
BOOST_AUTO_TEST_CASE( TestPMLDepth_Both )
{
    Rect3i yeeCells(0, 0, 0, 99, 0, 0);
    Rect3i nonPMLHalfCells(20, 0, 0, 179, 1, 1);
    PhysicalExtents extents(yeeCells, nonPMLHalfCells);
    BOOST_CHECK(extents.hasPML(0));
    BOOST_CHECK(extents.hasPML(1));

    RLE::DynamicRLE3<double> depth = pmlDepth(extents, 0);

    BOOST_CHECK(depth.hasDimension(0));
    BOOST_CHECK(false == depth.hasDimension(1));
    BOOST_CHECK(false == depth.hasDimension(2));

    Rect3i leftPML = extents.pmlHalfCells(0);
    BOOST_CHECK_EQUAL(leftPML.count(), 4*20);
    Rect3i rightPML = extents.pmlHalfCells(1);
    BOOST_CHECK_EQUAL(rightPML.count(), 4*20);

//    cerr << "PML rects " << leftPML << " and " << rightPML << "\n";

    // Test the LEFT side depths
    for (int nn = 0; nn < 20; nn++)
    {
        double d = (20-nn)/20.0;
        BOOST_CHECK_CLOSE(depth.at(nn,0,0), d, 1e-6);
    }
    // In particular:
    BOOST_CHECK_CLOSE(depth.at(leftPML.p1[0],0,0), 1.0, 1e-6);
    BOOST_CHECK_CLOSE(depth.at(leftPML.p2[0],0,0), 1.0/20.0, 1e-6);

    // Test the RIGHT side depths
    for (int nn = 180; nn < 200; nn++)
    {
        double d = (nn-179)/20.0;
        BOOST_CHECK_CLOSE(depth.at(nn,0,0), d, 1e-6);
    }
    for (int nn = 20; nn < 180; nn++)
        BOOST_CHECK_EQUAL(depth.at(nn,0,0), 0.0);
    // In particular:
    BOOST_CHECK_CLOSE(depth.at(rightPML.p1[0],0,0), 1.0/20.0, 1e-6);
    BOOST_CHECK_CLOSE(depth.at(rightPML.p2[0],0,0), 1.0, 1e-6);
}

BOOST_AUTO_TEST_CASE( TestSigma )
{
    Precision::Vec3 dxyz(1, 1, 1);
    Rect3i yeeCells(0, 0, 0, 99, 0, 0);
    Rect3i nonPMLHalfCells(20, 0, 0, 179, 1, 1);
    PhysicalExtents extents(yeeCells, nonPMLHalfCells);
    BOOST_CHECK(extents.hasPML(0));
    BOOST_CHECK(extents.hasPML(1));

    Rect3i leftPML = extents.pmlHalfCells(0);
    BOOST_CHECK_EQUAL(leftPML.count(), 4*20);
    Rect3i rightPML = extents.pmlHalfCells(1);
    BOOST_CHECK_EQUAL(rightPML.count(), 4*20);

//    cerr << "PML rects " << leftPML << " and " << rightPML << "\n";

    PMLField eField(YeeUtilities::Octant::electric());

    eField.init(extents, dxyz, Map<std::string, std::string>());

    const RLE::DynamicRLE3<PMLParameters>* xParams = eField.along(0);

    double eps0 = Constants::eps0;
    double mu0 = Constants::mu0;
    double c = 1.0/sqrt(eps0*mu0);

    double gridDiagonal = norm(dxyz*extents.physicalYeeCells().size());

    // Test the left side PML.
    Rect3i pmlYee;

    // Ex field: no attenuation should be carried out, but I guess I still
    // calculate the PML parameters.  Seems a little lazy/misleading, but as
    // long as the actual PML implementation doesn't do anything to Ex it's
    // probably ok, right?

    pmlYee = YeeUtilities::halfToYee(leftPML, YeeUtilities::octantE(0));

    for (int ii = pmlYee.p1[0]; ii <= pmlYee.p2[0]; ii++)
    {
        double x = (ii+0.5)*dxyz[0];
        double d = (20*dxyz[0]*0.5-x)/(20.0*dxyz[0]*0.5);

        double sigma = d*d*d*0.8*4/sqrt(mu0/eps0)/dxyz[0];
        BOOST_CHECK_CLOSE(xParams[0].at(ii,0,0).sigma(), sigma, 1e-6);

        double kappa = 1 + (5-1)*d*d*d;
        BOOST_CHECK_CLOSE(xParams[0].at(ii,0,0).kappa(), kappa, 1e-6);

        double alpha = (1-d)*c*eps0/gridDiagonal;
        BOOST_CHECK_CLOSE(xParams[0].at(ii,0,0).alpha(), alpha, 1e-6);

//        cerr << "k(" << x << ") = " << kappa << "\n";
    }

    // Ey field:

    for (int fieldXYZ = 0; fieldXYZ < 3; fieldXYZ++)
    {
        Rect3i r;
    }
}

BOOST_AUTO_TEST_CASE( Test1 )
{
    float dx = 5e-9;
    float gridDiagonalSize = 1000e-9;
    float greaterThanOne = 1.001;

    float sigmaBounds[] = { 0.0, greaterThanOne*0.8f*4/float(sqrt(Constants::eta0))/dx };
    float alphaBounds[] = { 0.0, greaterThanOne*float(Constants::c)*float(Constants::eps0)/50/dx };
    float kappaBounds[] = { 1.0, greaterThanOne*5.0f };

    BOOST_TEST_MESSAGE("Cell size " << dx << " meters.");
    BOOST_TEST_MESSAGE("Sigma bounds [" << sigmaBounds[0] << ", " <<
        sigmaBounds[1] << "].");
    BOOST_TEST_MESSAGE("Kappa bounds [" << kappaBounds[0] << ", " <<
        kappaBounds[1] << "].");
    BOOST_TEST_MESSAGE("Alpha bounds [" << alphaBounds[0] << ", " <<
        alphaBounds[1] << "].");

    CalculatePMLParameters callback(dx, gridDiagonalSize,
        Constants::eps0, Constants::mu0);

    for (double depth = -1.0; depth <= 2.0; depth += 0.1)
    {
        PMLParameters p = callback(depth);

        // depths out of range should yield default PML parameters
        if (depth <= 0.0 || depth > 1.0)
        {
            BOOST_CHECK(p == PMLParameters());
        }
        else
        {
            BOOST_CHECK(p.sigma() > 0.0);
            BOOST_CHECK(p.kappa() > 0.0);
            BOOST_CHECK(p.alpha() >= 0.0);

            BOOST_CHECK(p.sigma() >= sigmaBounds[0]);
            BOOST_CHECK(p.sigma() <= sigmaBounds[1]);
            BOOST_CHECK(p.kappa() >= kappaBounds[0]);
            BOOST_CHECK(p.kappa() <= kappaBounds[1]);
            BOOST_CHECK(p.alpha() >= alphaBounds[0]);
            BOOST_CHECK(p.alpha() <= alphaBounds[1]);

            // these are sanity tests really, aren't they?  well they helped
            // me find a bug.
        }
    }
}