#pragma once
#ifndef BOONDI_SRC_MAIN_EXAMPLE_OPENCLEXAMPLES_HPP_
#define BOONDI_SRC_MAIN_EXAMPLE_OPENCLEXAMPLES_HPP_

#define ANKERL_NANOBENCH_IMPLEMENT
#include <nanobench.h>
#include <memory>
#include <vector>
#include <algorithm>
#include <functional>
#include <cmath>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/math.hpp>
#include <boost/compute/core.hpp>
#include "utils/Log.hpp"

namespace grkl {
namespace compute = boost::compute;
class OpenCLExamples {
 private:
  compute::device _device;
  std::unique_ptr<compute::context> _context;
  std::unique_ptr<compute::command_queue> _commandQueue;
  std::unique_ptr<std::vector<float>>  _input;
  std::unique_ptr<std::vector<float>> _output;
  std::unique_ptr<compute::vector<float>> _deviceVector;
  const std::size_t inputSize = 100000;

 public:
  OpenCLExamples() {
    _device = compute::system::default_device();
    _context = std::make_unique<compute::context>(_device);
    _commandQueue = std::make_unique<compute::command_queue>(*_context, _device);
    _input = std::make_unique<std::vector<float>>(inputSize);
    _output = std::make_unique<std::vector<float>>(inputSize);
    // create a vector on the device
    _deviceVector = std::make_unique<compute::vector<float>>(inputSize, *_context);
    // transfer data from the host to the device
    compute::copy(
        _input->begin(), _input->end(), _deviceVector->begin(), *_commandQueue
    );
    // generate random data on the host
    std::generate(_input->begin(), _input->end(), rand);
  }

  void printOpenClVersions() const {
    // print the device's name and platform
    info("Hello world from {} (platform {})", _device.name(), _device.platform().name());
  }

  void computeCLSqrRoot() const {
    std::vector<float> const& host_vector = *_input;
    compute::vector<float> const& device_vector = *_deviceVector;
    // calculate the square-root of each element in-place
    compute::transform(
        device_vector.begin(),
        device_vector.end(),
        device_vector.begin(),
        compute::sqrt<float>(),
        *_commandQueue
    );
  }

  void computeSqrRoot() const {
    std::vector<float>& input = *_input;
    std::vector<float>& output = *_output;
    // calculate the square-root of each element
    std::transform(input.begin(), input.end(), output.begin(), [](float i) {return std::sqrt(i);});
  }

  void finalize() const {
    std::vector<float>& host_vector = *_input;
    compute::vector<float>& device_vector = *_deviceVector;
    // copy values back to the host
    compute::copy(
        device_vector.begin(), device_vector.end(), host_vector.begin(), *_commandQueue
    );
  }

  static void runExamples() {
    OpenCLExamples openCLExamples;
//  ankerl::nanobench::Bench().minEpochIterations(1085).run("OpenCL: Print Versions", [&] {
//    openCLExamples.printOpenClVersions();
//  });

    const std::size_t numItr = 1085;
//  const std::size_t numItr = 500;
    ankerl::nanobench::Bench().minEpochIterations(numItr*17).run("OpenCL: Compute Sqrt", [&openCLExamples] {
      openCLExamples.computeCLSqrRoot();
    });

    ankerl::nanobench::Bench().minEpochIterations(numItr).run("General: Compute Sqrt", [&openCLExamples] {
      openCLExamples.computeSqrRoot();
    });

    openCLExamples.finalize();
    ankerl::nanobench::Bench().minEpochIterations(numItr).run("General: Some double ops", [] {
      double d = 1.0;
      d += 1.0 / d;
      if (d > 5.0) {
        d -= 5.0;
      }
      ankerl::nanobench::doNotOptimizeAway(d);
    });
  }
};
}
#endif //BOONDI_SRC_MAIN_EXAMPLE_OPENCLEXAMPLES_HPP_


// Copyright (c) 2012-2019 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 <policy/policy.h>
#include <script/interpreter.h>
#include <script/script.h>

#include <test/lcg.h>
#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

#include <core_io.h>

BOOST_FIXTURE_TEST_SUITE(op_mulpow2_tests, BasicTestingSetup)

typedef std::vector<uint8_t> valtype;
typedef std::vector<valtype> stacktype;
std::array<uint32_t, 2> flagset{{0, STANDARD_SCRIPT_VERIFY_FLAGS}};
std::vector<int64_t> interesting_numbers = {
    0,
    1,
    -1,
    2,
    -2,
    4,
    -4,
    10,
    -10,
    127,
    -127,
    256,
    -256,
    0x7fffffff,
    -0x7fffffff,
    0x100000000,
    -0x100000000,
    0x7fffffffff,
    -0x7fffffffff,
    0x10000000000,
    -0x10000000000,
    0x7fffffffffffff,
    -0x7fffffffffffff,
    0x100000000000000,
    -0x100000000000000,
    0x7fffffffffffffff,
    -0x7fffffffffffffff,
};

static void CheckSuccess(const CScript &script) {
    BaseSignatureChecker sigchecker;
    for (uint32_t flags : flagset) {
        ScriptError err = ScriptError::OK;
        stacktype stack{};
        bool r = EvalScript(stack, script, flags, sigchecker, &err);
        BOOST_CHECK_MESSAGE(r, "Expected OK but got "
                                   << ScriptErrorString(err) << " for '"
                                   << ScriptToAsmStr(script) << "'");
    }
}

static void CheckError(const CScript &script,
                       const ScriptError expected_error) {
    BaseSignatureChecker sigchecker;
    for (uint32_t flags : flagset) {
        ScriptError err = ScriptError::OK;
        stacktype stack{};
        bool r = EvalScript(stack, script, flags, sigchecker, &err);
        BOOST_CHECK_MESSAGE(
            !r, "Expected " << ScriptErrorString(expected_error) << " but got "
                            << ScriptErrorString(err) << " for '"
                            << ScriptToAsmStr(script) << "'");
        BOOST_CHECK_EQUAL(err, expected_error);
    }
}

static bool Pow2(int64_t base, int64_t exponent, int64_t &result) {
    constexpr int64_t LIMIT = 0x4000'0000'0000'0000;
    result = base;
    if (base == 0) {
        return true;
    }
    if (exponent > 0) {
        for (int64_t i = 0; i < exponent; ++i) {
            if (result >= LIMIT || result <= -LIMIT) {
                return false;
            }
            result *= 2;
        }
    } else if (exponent < 0) {
        for (int64_t i = 0; i < -exponent; ++i) {
            result /= 2;
            if (result == 0) {
                return true;
            }
        }
    }
    return true;
}

static void CheckTestI64Shift(int64_t base, int32_t exponent) {
    int64_t result;
    if (Pow2(base, exponent, result)) {
        CheckSuccess(CScript()
                     << base << exponent << OP_MULPOW2 << result << OP_EQUAL);
    } else {
        CheckError(CScript() << base << exponent << OP_MULPOW2 << OP_1,
                   ScriptError::UNKNOWN);
    }
}

BOOST_AUTO_TEST_CASE(rng_shift_test) {
    MMIXLinearCongruentialGenerator lcg;
    for (int test = 0; test < 20; ++test) {
        uint64_t num = lcg.next();
        num <<= 32;
        num |= lcg.next();
        const int64_t base = int64_t(num);
        for (int64_t i = 0; i <= 63; ++i) {
            CheckTestI64Shift(base, i);
        }
    }
}

BOOST_AUTO_TEST_CASE(interesting_numbers_test) {
    for (int64_t base : interesting_numbers) {
        for (int64_t exponent : interesting_numbers) {
            CheckTestI64Shift(base, exponent);
        }
        for (int64_t exponent = 0; exponent <= 63; ++exponent) {
            CheckTestI64Shift(base, exponent);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#define CATCH_CONFIG_MAIN
#include <armadillo>
#define CATCH_CONFIG_ENABLE_BENCHMARKING
#include <catch2/catch.hpp>
#include <cmath>
#include <fstream>
#include <libKriging/OrdinaryKriging.hpp>

auto f = [](const arma::rowvec& row) {
  double sum = 0;
  for (auto&& x : row) {
    // sum += ((x - .5) * (x - .5));  // cas 1
    sum += (x * x);  // cas 2
  }
  return sum;
};

auto prepare_and_run_bench = [](auto&& bench) {
  const int count = 11;
  const auto i = GENERATE_COPY(range(0, count));

  arma::arma_rng::seed_type seed_val = 123;  // populate somehow (fixed value => reproducible)

  const double logn = 1 + 0.1 * i;
  const arma::uword n = floor(std::pow(10., logn));
  const arma::uword d = floor(2 + i / 3);

  INFO("dimensions are n=" << n << " x d=" << d);

  arma::arma_rng::set_seed(seed_val);
  arma::mat X(n, d, arma::fill::randu);
  arma::colvec y(n);
  for (arma::uword k = 0; k < n; ++k)
    y(k) = f(X.row(k));

  bench(y, X, i);

  CHECK(true);
};

TEST_CASE("workflow") {
  prepare_and_run_bench([](const arma::colvec& y, const arma::mat& X, int i) {
    OrdinaryKriging ok;
    ok.fit(y, X);  // FIXME no move
    const double theta = 0.5;
    arma::vec theta_vec(X.n_cols);
    theta_vec.fill(theta);
    return ok.logLikelihoodGrad(theta_vec);
  });
}

TEST_CASE("fit benchmark", "[.benchmark]") {
  prepare_and_run_bench([](const arma::colvec& y, const arma::mat& X, int i) {
    OrdinaryKriging ok;
    BENCHMARK("OrdinaryKriging::fit#" + std::to_string(i)) {
      return ok.fit(y, X);  // FIXME no move
    };
  });
}

TEST_CASE("logLikelihood benchmark", "[.benchmark]") {
  prepare_and_run_bench([](const arma::colvec& y, const arma::mat& X, int i) {
    OrdinaryKriging ok;
    ok.fit(y, X);  // FIXME no move

    const double theta = 0.5;
    arma::vec theta_vec(X.n_cols);
    theta_vec.fill(theta);

    BENCHMARK("OrdinaryKriging::logLikelihood#" + std::to_string(i)) {
      return ok.logLikelihood(theta_vec);  //
    };
  });
}

TEST_CASE("logLikelihoodGrad benchmark", "[.benchmark]") {
  prepare_and_run_bench([](const arma::colvec& y, const arma::mat& X, int i) {
    OrdinaryKriging ok;
    ok.fit(y, X);  // FIXME no move

    const double theta = 0.5;
    arma::vec theta_vec(X.n_cols);
    theta_vec.fill(theta);

    BENCHMARK("OrdinaryKriging::logLikelihoodGrad#" + std::to_string(i)) {
      return ok.logLikelihoodGrad(theta_vec);  //
    };
  });
}


#include <functional>

#include "Describer.h"
#include "common_defs.h"

#include <numeric>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std::placeholders;

namespace
{
	bool hasNameAndAttribute(const char* nodeName, const char* attrName, const char* attrValue, pugi::xml_node node)
	{
		if(strcmp(node.name(), nodeName))
			return false;

		return !strcmp(node.attribute(attrName).as_string(), attrValue);
	}

	pugi::xml_node getNode(const pugi::xml_document& doc, const char* attrType)
	{
		pugi::xml_node node = doc.find_node(std::bind(hasNameAndAttribute, "AudioDescriptor", "xsi:type", attrType, _1));
		if (node.empty())
			throw Describer::RequiredDescriptorNotFound(std::string("Descriptor: AudioDescriptor with attribute xsi:type at value: ") + attrType + " not found");

		return node;
	}

	Descriptor getAttributeScalarValue(MPEG7Describer::MPEG7Document& doc, const char* attrType)
	{
		pugi::xml_node node = getNode(doc, attrType);
		return Descriptor(node.child("Scalar").text().as_double());
	}

	MPEG7DetailedDescriber::StatisticalDescription calculateStatisticalDescription(const std::vector<double> source)
	{
		if (source.empty())
			throw Describer::RequiredDescriptorNotFound(std::string("One of ScalarSeries audio descriptors contains invalid data"));

		double average = std::accumulate(source.begin(), source.end(), 0.0) / static_cast<double>(source.size());
		double variation = pow(std::accumulate(source.begin(), source.end(), 0.0, [average](double acc, double d){return acc + std::pow(average - d, 2.0);}) / static_cast<double>(source.size()), 0.5);

		return std::make_pair(average, variation);
	}

	std::vector<double> rawDataToVector(std::string& textual)
	{
		boost::trim(textual);
		textual.shrink_to_fit();
		std::vector<std::string> numbersTextual;
		boost::split(numbersTextual, textual, [](char c){ return c == ' '; });

		std::vector<double> series;
		series.reserve(numbersTextual.size());
		for (auto& i : numbersTextual)
			series.push_back(boost::lexical_cast<double>(i));

		return series;
	}

	MPEG7DetailedDescriber::StatisticalDescription getStatisticalDescription(MPEG7Describer::MPEG7Document& doc, const char* attrType, const char* attrValue)
	{
		pugi::xml_node type = getNode(doc, attrType);
		pugi::xml_node value = (attrValue == nullptr) ? type.child("SeriesOfScalar") : type.child(attrValue).child("SeriesOfScalar");

		std::string textual = value.child("Raw").text().as_string();

		if (textual.empty())
			throw Describer::RequiredDescriptorNotFound(std::string("Descriptor: AudioDescriptor with attribute xsi:type at value: ") + attrType + "/" + attrValue + " empty");

		return calculateStatisticalDescription(rawDataToVector(textual));
	}
}

Describer::Describer(std::istream* dataSource) : dataStream(dataSource)
{}

void Describer::setInputStream(std::istream* input)
{
	dataStream = input;
}

MPEG7Describer::MPEG7Describer(std::istream* dataSource) : Describer(dataSource)
{
}

ObjectDescription MPEG7Describer::generateObjectDescription()
{
	IR_ASSERT(dataStream == nullptr || !dataStream->eof(), "Empty data record given to MPEG7Describer!");
	pugi::xml_document mpeg7Doc;

	if (!mpeg7Doc.load(*dataStream, pugi::parse_full))
		throw InputParsingError("MPEG7Describer: invalid XML file loaded");

	return getObjectDescription(mpeg7Doc);
}

ObjectDescription MPEG7Describer::getObjectDescription(MPEG7Document& doc)
{
	const unsigned int numberOfMPEG7Descriptors = 7;

	ObjectDescription description(numberOfMPEG7Descriptors);
	description[0] = getAttributeScalarValue(doc, "LogAttackTimeType");
	description[1] = getAttributeScalarValue(doc, "SpectralCentroidType");
	description[2] = getAttributeScalarValue(doc, "TemporalCentroidType");
	description[3] = getAttributeScalarValue(doc, "HarmonicSpectralCentroidType");
	description[4] = getAttributeScalarValue(doc, "HarmonicSpectralDeviationType");
	description[5] = getAttributeScalarValue(doc, "HarmonicSpectralSpreadType");
	description[6] = getAttributeScalarValue(doc, "HarmonicSpectralVariationType");

	return description;
}

Describer* MPEG7Describer::clone()
{
	return new MPEG7Describer(nullptr);
}

MPEG7Describer::~MPEG7Describer(void)
{
}

MPEG7DetailedDescriber::MPEG7DetailedDescriber(std::istream* dataSource) : MPEG7Describer(dataSource)
{
}

ObjectDescription MPEG7DetailedDescriber::getObjectDescription(MPEG7Document& doc)
{
	const unsigned int numberOfMPEG7Descriptors = 13;

	ObjectDescription description(numberOfMPEG7Descriptors);
	ObjectDescription simpleDescription = MPEG7Describer::getObjectDescription(doc);
	std::copy(simpleDescription.begin(), simpleDescription.end(), description.begin());

	StatisticalDescription fundamentalFrequency = getStatisticalDescription(doc, "AudioFundamentalFrequencyType", nullptr);
	description[7] = fundamentalFrequency.first;
	description[8] = fundamentalFrequency.second;
	StatisticalDescription harmonicity = getStatisticalDescription(doc, "AudioHarmonicityType", "HarmonicRatio");
	description[9] = harmonicity.first;
	description[10] = harmonicity.second;
	StatisticalDescription harmonicityLimit = getStatisticalDescription(doc, "AudioHarmonicityType", "UpperLimitOfHarmonicity");
	description[11] = harmonicityLimit.first;
	description[12] = harmonicityLimit.second;

	return description;
}

Describer* MPEG7DetailedDescriber::clone()
{
	return new MPEG7DetailedDescriber(nullptr);
}

MPEG7DetailedDescriber::~MPEG7DetailedDescriber(void)
{
}

DummyDescriber* DummyDescriber::clone()
{
    return new DummyDescriber;
}

ObjectDescription DummyDescriber::generateObjectDescription()
{
    ObjectDescription obj;
    while (*dataStream)
    {
        Descriptor desc;
        char c;
        *dataStream >> desc >> c;
        if (*dataStream)
            obj.push_back(desc);
    }

    return obj;
}


/////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::gamma::random.hpp                      //
//                                                                             //
//  (C) Copyright 2009 Erwann Rogard                                           //
//  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 BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_RANDOM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_RANDOM_HPP_ER_2009
#include <boost/math/distributions/gamma.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/location_scale.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace meta{

    template<typename T,typename P>
    struct random_distribution<
        boost::math::gamma_distribution<T,P>
    >{
        typedef boost::math::gamma_distribution<T,P> dist_;
        typedef boost::gamma_distribution<T> z_;
        typedef boost::random::location_scale_distribution<z_> type;

        static type call(const dist_& d){
            return type(
                0,
                d.scale(),
                z_(d.shape())
            );
        }
    };

}// meta
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <time.h>
#include <boost/random.hpp>

#define RNGTYPE boost::mt19937

namespace
{
  RNGTYPE *pRNG = NULL;
};

void Init()
{
  if (!pRNG) pRNG = new RNGTYPE (::time(NULL));
}

int DieRoll(int i_nsides)
{
  Init();
  boost::uniform_smallint<> die(1,i_nsides);
  boost::variate_generator<RNGTYPE&,boost::uniform_smallint<> > r(*pRNG,die);
  return r();
}

bool CoinFlip()
{
  Init();
  boost::uniform_smallint<> die(1,2);
  boost::variate_generator<RNGTYPE&,boost::uniform_smallint<> > r(*pRNG,die);
  return r() == 1;
}

int RandRange(int i_lowest,int i_highest)
{
  Init();
  boost::uniform_int<> die(i_lowest,i_highest);
  boost::variate_generator<RNGTYPE&,boost::uniform_int<> > r(*pRNG,die);
  return r();
}

double Rand01()
{
  Init();
  boost::uniform_01<RNGTYPE> r01(*pRNG);
  return r01();
}

double RandRange(double i_lowest,double i_highest)
{
  Init();
  boost::uniform_real<> die(i_lowest,i_highest);
  boost::variate_generator<RNGTYPE&,boost::uniform_real<> > r(*pRNG,die);
  return r();
}


#include <iostream>
#include <unordered_set>
#include <stack>
#include <boost/program_options.hpp>
#include <boost/optional.hpp>
//#include <pcl/io/ply_io.h>
#include <bh/eigen_utils.h>
#include <bh/eigen_options.h>
#include <bh/mLib/mLibCoreMesh.h>
#include <bh/math/geometry.h>
#include <bh/mLib/mLibUtils.h>
#include <bh/math/continuous_grid3d.h>
#include <bh/mesh/triangle_mesh.h>

using std::size_t;
using std::cout;
using std::endl;
using std::string;

//using pcl::PLYReader;
//using pcl::PolygonMesh;

using FloatType = float;
USE_FIXED_EIGEN_TYPES(FloatType);
using BoundingBoxType = bh::BoundingBox3D<FloatType>;
using TriangleType = bh::Triangle<FloatType>;
using TriangleMeshType = bh::TriangleMesh<FloatType>;
using TriangleMeshFactoryType = bh::TriangleMeshFactory<FloatType>;

void run(const string& mesh_filename, const FloatType voxel_size,
         const boost::optional<string>& text_output_filename,
         const boost::optional<string>& binary_output_filename,
         const boost::optional<string>& mesh_output_filename,
         const BoundingBoxType& clip_bbox,
         const FloatType max_squared_triangle_area,
         const bool debug_output) {
//  // Reading with PCL (quite picky on file format)
//  PolygonMesh mesh;
//  Eigen::Vector4f origin;
//  Eigen::Quaternionf orientation;
//  int ply_version;
//  PLYReader reader;
//  int result = reader.read(filename, mesh, origin, orientation, ply_version);
//  if (result <= 0) {
//    throw std::runtime_error("Error reading mesh: " + filename);
//  }
//  cout << "Mesh has " << mesh.polygons.size() << " faces and "
//       << mesh.cloud.width * mesh.cloud.height << " vertices" << endl;
  // Reading with mLib
  ml::MeshDataf mesh;
  ml::MeshIOf::loadFromPLY(mesh_filename, mesh);
  cout << "Mesh has " << mesh.m_FaceIndicesVertices.size() << " faces and "
       << mesh.m_Vertices.size() << " vertices" << endl;

  const bh::ContinuousGrid3DUtilsf grid_utils(voxel_size, 0.5f);
  std::unordered_set<Vector3, bh::EigenHash<Vector3>> intersecting_voxels;

  std::stack<TriangleType> triangle_stack;
  for (size_t i = 0; i < mesh.m_FaceIndicesVertices.size(); ++i) {
    const ml::MeshDataf::Indices::Face& face = mesh.m_FaceIndicesVertices[i];
    BH_ASSERT_STR(face.size() == 3, "Can only handle triangle meshes");
    const Vector3 v1 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[0]]);
    const Vector3 v2 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[1]]);
    const Vector3 v3 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[2]]);
    if (clip_bbox.isOutside(v1) && clip_bbox.isOutside(v2) && clip_bbox.isOutside(v3)) {
      const TriangleType tri(v1, v2, v3);
      if (!tri.intersects(clip_bbox)) {
        continue;
      }
    }
    const TriangleType tri(v1, v2, v3);
    triangle_stack.push(tri);
  }

  // For debugging
  if (debug_output) {
    BoundingBoxType mesh_bbox = BoundingBoxType::Empty();
    for (size_t i = 0; i < mesh.m_FaceIndicesVertices.size(); ++i) {
      const ml::MeshDataf::Indices::Face& face = mesh.m_FaceIndicesVertices[i];
      BH_ASSERT_STR(face.size() == 3, "Can only handle triangle meshes");
      const Vector3 v1 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[0]]);
      const Vector3 v2 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[1]]);
      const Vector3 v3 = bh::MLibUtilities::convertMlibToEigen(mesh.m_Vertices[face[2]]);
      mesh_bbox.include(v1);
      mesh_bbox.include(v2);
      mesh_bbox.include(v2);
    }
    std::cout << "Mesh bounding box: " << mesh_bbox << std::endl;
  }

  // For debugging
  if (debug_output) {
    std::vector<TriangleType> triangles;
    while (!triangle_stack.empty()) {
      triangles.push_back(triangle_stack.top());
      triangle_stack.pop();
    }
    TriangleMeshType tri_mesh = TriangleMeshFactoryType::createFromTriangles(triangles.begin(), triangles.end());
    ml::MeshDataf tri_mesh_ml = bh::MLibUtilities::convertBhToMlib(tri_mesh);
    ml::MeshIOf::saveToPLY("tmp_clip_mesh.ply", tri_mesh_ml);
    for (const TriangleType& triangle : triangles) {
      triangle_stack.push(triangle);
    }
  }

  std::cout << "Remaining triangles after clipping: " << triangle_stack.size() << std::endl;

  std::vector<TriangleType> triangles;
  cout << "Splitting up triangles" << endl;
  size_t pushed_tri_count = triangle_stack.size();
  size_t processed_tri_count = 0;
  while (!triangle_stack.empty()) {
    const TriangleType tri = triangle_stack.top();
    triangle_stack.pop();
    const float area_squared = tri.computeTriangleAreaSquare();
    if (area_squared > max_squared_triangle_area) {
//      const std::array<TriangleType, 2> split_tris = tri.splitTriangleInto2();
      const std::array<TriangleType, 3> split_tris = tri.splitTriangleInto3();
      for (size_t i = 0; i < split_tris.size(); ++i) {
        triangle_stack.push(split_tris[i]);
      }
      pushed_tri_count += split_tris.size();
      if (pushed_tri_count % 1000 == 0) {
        cout << "Pushed " << pushed_tri_count << " triangles" << endl;
      }
    }
    else {
      triangles.push_back(tri);
      ++processed_tri_count;
      if (processed_tri_count % 1000 == 0) {
        cout << "Processed " << processed_tri_count << " triangles" << endl;
      }
    }
  }

  // For debugging
  if (debug_output) {
    TriangleMeshType tri_mesh = TriangleMeshFactoryType::createFromTriangles(triangles.begin(), triangles.end());
    ml::MeshDataf tri_mesh_ml = bh::MLibUtilities::convertBhToMlib(tri_mesh);
    ml::MeshIOf::saveToPLY("tmp_split_mesh.ply", tri_mesh_ml);
  }

  // For debugging
  std::vector<ml::TriMeshf> tmp_voxel_meshes;
  std::vector<ml::TriMeshf> tmp_intersecting_voxel_meshes;

  cout << "Intersecting " << triangles.size() << " triangles with grid voxels" << endl;
  // Iterate over all triangles
//#pragma omp parallel for
  for (size_t i = 0; i < triangles.size(); ++i) {
    if (i % 100 == 0) {
      cout << "Processing triangle " << i + 1 << " out of " << triangles.size() << endl;
    }
    const TriangleType& tri = triangles[i];

    // Loop over all voxels in triangle bounding box
    const bh::BoundingBox3Df bbox = tri.boundingBox();
    const Vector3 min_voxel = grid_utils.getMinIntersectingVoxel(bbox);
    const Vector3 max_voxel = grid_utils.getMaxIntersectingVoxel(bbox);
    if (debug_output) {
      std::cout << "i: " << i << std::endl;
      std::cout << "v1: " << tri.v1().transpose() << std::endl;
      std::cout << "v2: " << tri.v2().transpose() << std::endl;
      std::cout << "v3: " << tri.v3().transpose() << std::endl;
      std::cout << "bbox: " << bbox << std::endl;
      std::cout << "min_voxel: " << min_voxel.transpose() << std::endl;
      std::cout << "max_voxel: " << max_voxel.transpose() << std::endl;
    }
    for (FloatType x = min_voxel(0); x <= max_voxel(0); x += grid_utils.getIncrement()) {
      for (FloatType y = min_voxel(1); y <= max_voxel(1); y += grid_utils.getIncrement()) {
        for (FloatType z = min_voxel(2); z <= max_voxel(2); z += grid_utils.getIncrement()) {
          const Vector3 voxel(x, y, z);
          const bh::BoundingBox3Df voxel_bbox(voxel, grid_utils.getIncrement());
          // For debugging
          if (debug_output) {
            const ml::BoundingBox3f ml_bbox(bh::MLibUtilities::convertEigenToMlib(voxel_bbox.getMinimum()),
                                            bh::MLibUtilities::convertEigenToMlib(voxel_bbox.getMaximum()));
            const ml::TriMeshf output_mesh = ml::Shapesf::box(ml_bbox);
            tmp_voxel_meshes.push_back(output_mesh);
          }
          if (tri.intersects(voxel_bbox)) {
            // For debugging
            if (debug_output) {
              const ml::BoundingBox3f ml_bbox(bh::MLibUtilities::convertEigenToMlib(voxel_bbox.getMinimum()),
                                              bh::MLibUtilities::convertEigenToMlib(voxel_bbox.getMaximum()));
              const ml::TriMeshf output_mesh = ml::Shapesf::box(ml_bbox);
              tmp_intersecting_voxel_meshes.push_back(output_mesh);
            }
          }
          if (!clip_bbox.intersects(voxel_bbox)) {
            continue;
          }
          // This does not seem to do any speedup and prevents parallelization.
//          if (intersecting_voxels.find(voxel) != intersecting_voxels.end()) {
//            continue;
//          }
          if (tri.intersects(voxel_bbox)) {
//#pragma omp critical
            {
              intersecting_voxels.insert(voxel);
            }
          }
        }
      }
    }
  }
  cout << "Found " << intersecting_voxels.size() << " intersecting voxels" << endl;

  // For debugging
  if (debug_output) {
    const ml::TriMeshf tmp_voxel_mesh = ml::Shapesf::unifyMeshes(tmp_voxel_meshes);
    ml::MeshIOf::saveToPLY("tmp_voxel_mesh.ply", tmp_voxel_mesh.getMeshData());
    const ml::TriMeshf tmp_intersecting_voxel_mesh = ml::Shapesf::unifyMeshes(tmp_intersecting_voxel_meshes);
    ml::MeshIOf::saveToPLY("tmp_intersecting_voxel_mesh.ply", tmp_intersecting_voxel_mesh.getMeshData());
  }

  Vector3 surface_min(std::numeric_limits<FloatType>::max(),
                      std::numeric_limits<FloatType>::max(),
                      std::numeric_limits<FloatType>::max());
  Vector3 surface_max(- std::numeric_limits<FloatType>::max(),
                      - std::numeric_limits<FloatType>::max(),
                      - std::numeric_limits<FloatType>::max());
  for (const Vector3& voxel : intersecting_voxels) {
    for (std::size_t i = 0; i < 3; ++i) {
      if (voxel(i) < surface_min(i)) {
        surface_min(i) = voxel(i);
      }
      if (voxel(i) > surface_max(i)) {
        surface_max(i) = voxel(i);
      }
    }
  }
  bh::BoundingBox3Df surface_bbox(surface_min, surface_max);
  cout << "Surface bounding box: " << surface_bbox << endl;

  if (text_output_filename) {
    cout << "Writing surface voxels to text file" << endl;
    std::ofstream out(*text_output_filename);
    if (!out) {
      throw std::runtime_error("ERROR: Unable to open text output file: " + *text_output_filename);
    }
    for (const Vector3 &voxel : intersecting_voxels) {
      out << voxel(0) << " " << voxel(1) << " " << voxel(2) << endl;
    }
  }

  if (binary_output_filename) {
    cout << "Writing surface voxels to binary file" << endl;
    std::ofstream out(*binary_output_filename, std::ios_base::binary);
    if (!out) {
      throw std::runtime_error("ERROR: Unable to open binary output file: " + *binary_output_filename);
    }
    const size_t num_voxels = intersecting_voxels.size();
    out.write(reinterpret_cast<const char*>(&num_voxels), sizeof(num_voxels));
    for (const Vector3 &voxel : intersecting_voxels) {
      out.write(reinterpret_cast<const char*>(&voxel(0)), sizeof(voxel(0)));
      out.write(reinterpret_cast<const char*>(&voxel(1)), sizeof(voxel(1)));
      out.write(reinterpret_cast<const char*>(&voxel(2)), sizeof(voxel(2)));
    }
  }

  if (mesh_output_filename) {
    cout << "Writing surface voxels to mesh" << endl;
    std::vector<ml::TriMeshf> output_meshes;
    for (const Vector3 &voxel : intersecting_voxels) {
      const bh::BoundingBox3Df voxel_bbox(voxel, grid_utils.getIncrement());
      const ml::BoundingBox3f ml_bbox = bh::MLibUtilities::convertBhToMlib(voxel_bbox);
      const ml::TriMeshf output_mesh = ml::Shapesf::box(ml_bbox);
      output_meshes.push_back(output_mesh);
    }
    const ml::TriMeshf output_mesh = ml::Shapesf::unifyMeshes(output_meshes);
    ml::MeshIOf::saveToPLY(*mesh_output_filename, output_mesh.getMeshData());
  }
}

int main(int argc, const char** argv) {
  try {
    /** Define and parse the program options
     */
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
            ("help", "Print help messages")
            ("voxel-size", po::value<FloatType>()->default_value(0.2), "Voxel size")
            ("mesh-filename", po::value<string>()->required(), "Mesh filename")
            ("text-output-filename", po::value<string>(), "Text output filename")
            ("binary-output-filename", po::value<string>(), "Binary output filename")
            ("mesh-output-filename", po::value<string>(), "Mesh output filename")
            ("bbox-min", po::value<Vector3>(), "Bounding box minimum")
            ("bbox-max", po::value<Vector3>(), "Bounding box maximum")
            ("max-triangle-area", po::value<FloatType>()->default_value(5.0f), "Max triangle area")
            ("debug-output", po::bool_switch()->default_value(false), "Produce debug output");

    po::variables_map vm;
    try {
      po::store(po::parse_command_line(argc, argv, desc),
                vm); // can throw

      /** --help option
       */
      if (vm.count("help"))
      {
        std::cout << "Basic Command Line Parameter App" << std::endl
                  << desc << std::endl;
        return 0;
      }

      po::notify(vm); // throws on error, so do after help in case
      // there are any problems
    }
    catch (po::error& e) {
      std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
      std::cerr << desc << std::endl;
      return -1;
    }

    const string mesh_filename = vm["mesh-filename"].as<string>();
    const FloatType voxel_size = vm["voxel-size"].as<FloatType>();
    boost::optional<string> text_output_filename = boost::none;
    boost::optional<string> binary_output_filename = boost::none;
    boost::optional<string> mesh_output_filename = boost::none;
    if (vm.count("text_output-filename")) {
      text_output_filename = vm["text-output-filename"].as<string>();
    }
    if (vm.count("binary-output-filename")) {
      binary_output_filename = vm["binary-output-filename"].as<string>();
    }
    if (vm.count("mesh-output-filename")) {
      mesh_output_filename = vm["mesh-output-filename"].as<string>();
    }

    const FloatType lowest = std::numeric_limits<FloatType>().lowest();
    const FloatType highest = std::numeric_limits<FloatType>().max();
    Vector3 bbox_min(lowest, lowest, lowest);
    Vector3 bbox_max(highest, highest, highest);
    if (vm.count("bbox-min")) {
      bbox_min = vm["bbox-min"].as<Vector3>();
    }
    if (vm.count("bbox-max")) {
      bbox_max = vm["bbox-max"].as<Vector3>();
    }
    std::cout << "Minimum bounding box: " << bbox_min.transpose() << std::endl;
    std::cout << "Maximum bounding box: " << bbox_max.transpose() << std::endl;
    const BoundingBoxType bbox(bbox_min, bbox_max);
    const FloatType max_triangle_area = vm["max-triangle-area"].as<FloatType>();
    const FloatType max_squared_triangle_area = max_triangle_area * max_triangle_area;
    run(mesh_filename, voxel_size,
        text_output_filename, binary_output_filename, mesh_output_filename,
        bbox, max_squared_triangle_area, vm["debug-output"].as<bool>());

    return 0;
  }
  catch (std::exception& e)
  {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return -1;
  }
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under 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)

#include <vector>
#include <memory>
#include <iostream>
#include <iomanip>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "Eigen/LU"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"
#include "metro/regression/NormalWeightedLogLikelihood.hpp"

// #define DEBUG_NORMALWEIGHTEDLOGLIKELIHOOD 1

namespace metro {
	namespace regression {
		namespace impl {
			double compute_mvn_constant( int const k, double determinant ) {
				return -0.5 * k * std::log( 2 * 3.141592654 ) - 0.5 * std::log( determinant ) ;
			}
		}

		NormalWeightedLogLikelihood::UniquePtr NormalWeightedLogLikelihood::create(
			LogLikelihood::UniquePtr ll,
			Vector const mean,
			Matrix const covariance,
			Normalisation normalisation
		) {
			return UniquePtr( new NormalWeightedLogLikelihood( ll, mean, covariance, normalisation ) ) ;
		}

		NormalWeightedLogLikelihood::NormalWeightedLogLikelihood(
			LogLikelihood::UniquePtr ll,
			Vector const mean,
			Matrix const covariance,
			Normalisation normalisation
		):
			m_ll( ll ),
			m_mean( mean ),
			m_covariance( covariance ),
			m_normalisation( normalisation ),
			m_solver( covariance ),
			m_inverse_covariance( m_solver.solve( Matrix::Identity( mean.size(), mean.size() ))),
			m_constant( impl::compute_mvn_constant( m_mean.size(), m_covariance.determinant() ))
		{}

		std::string NormalWeightedLogLikelihood::get_parameter_name( std::size_t i ) const {
			return m_ll->get_parameter_name(i) ;
		}
		NormalWeightedLogLikelihood::IntegerMatrix NormalWeightedLogLikelihood::identify_parameters() const {
			return m_ll->identify_parameters() ;
		}

		int NormalWeightedLogLikelihood::number_of_parameters() const {
			return m_ll->number_of_parameters() ;
		}

		int NormalWeightedLogLikelihood::number_of_outcomes() const {
			return m_ll->number_of_outcomes() ;
		}

		void NormalWeightedLogLikelihood::evaluate_at( Point const& parameters, int const numberOfDerivatives ) {
			m_ll->evaluate_at( parameters, numberOfDerivatives ) ;
			evaluate_impl( numberOfDerivatives ) ;
		}

		void NormalWeightedLogLikelihood::evaluate( int const numberOfDerivatives ) {
			m_ll->evaluate( numberOfDerivatives ) ;
			evaluate_impl( numberOfDerivatives ) ;
		}

		void NormalWeightedLogLikelihood::evaluate_impl( int const numberOfDerivatives ) {
			Vector const& parameters = m_ll->parameters() ;
			Vector solved = m_solver.solve( parameters - m_mean ) ;
			m_log_density = -0.5 * ((parameters - m_mean).transpose() * solved)(0) ;
			m_value_of_first_derivative = -solved ;
			m_value_of_second_derivative = -m_inverse_covariance ;

#if DEBUG_NORMALWEIGHTEDLOGLIKELIHOOD
			std::cerr << "NormalWeightedLogLikelihood::evaluate_impl()\:\n" ;
			std::cerr << "parameters = " << parameters.transpose() << ".\n" ;
			std::cerr << "constant = " << m_constant << ".\n" ;
			std::cerr << "solved = \n" << solved << ".\n" ;
			std::cerr << "log density = " << m_log_density << "\n" ;
#endif
		}

		std::string NormalWeightedLogLikelihood::get_summary() const {
			std::ostringstream ostr ;
			ostr << "normal-weighted:" << m_ll->get_summary() << "\n" ;
			ostr << "normal-weighted: using the following prior covariance:\n" ;
			std::size_t max_label_length = 0 ;
			for( int i = 0; i < m_covariance.rows(); ++i ) {
				max_label_length = std::max( max_label_length, m_ll->get_parameter_name(i).size() ) ;
			}
			ostr
				<< std::setw(3) << "" << "  "
				<< std::setw( max_label_length + 2 ) << "parameter"
				<< ":" ;
			for( int j = 0; j < m_covariance.cols(); ++j ) {
				ostr << " " << std::setw(5) << (j+1) ;
			}
			ostr << "\n" ;
			for( int i = 0; i < m_covariance.rows(); ++i ) {
				ostr << std::setw(3) << i+1
					<< ": " << std::setw( max_label_length + 2 ) << m_ll->get_parameter_name(i) << ":" ;
				for( int j = 0; j < m_covariance.cols(); ++j ) {
					ostr << " " << std::setw(5) << std::setprecision(4) << m_covariance(i,j) ;
				}
				ostr << "\n" ;
			}
			return ostr.str() ;
		}
	}
}


#include "icecap.h"
#include "../basic.h"

#include <random>

#include <boost/log/trivial.hpp>
#include <boost/random.hpp>

namespace WorldEngine
{

typedef bool                                 SolidDataType;
typedef boost::multi_array<SolidDataType, 2> SolidArrayType;

// Primary constants, all values should be in [0, 1]

// Only the coldest % of cold area will freeze (0 = no ice, 1 = all ice)
static const float MAX_FREEZE_PERCENTAGE = 0.6f;

// The warmest % of freezable area won't completely freeze
static const float FREEZE_CHANCE_WINDOW = 0.2f;

// Chance modifier to freeze a slightly warm tile when neighbors are frozen
static const float SURROUNDING_TILE_INFLUENCE = 0.5f;

static const uint32_t NUM_SURROUNDING_TILES = 8u;

void IcecapSimulation(World& world, uint32_t seed)
{
   BOOST_LOG_TRIVIAL(info) << "Icecap simulation start";

   std::mt19937                                    generator(seed);
   boost::random::uniform_real_distribution<float> distribution(0.0f, 1.0f);

   const int32_t width  = world.width();
   const int32_t height = world.height();

   const OceanArrayType&       ocean       = world.GetOceanData();
   const TemperatureArrayType& temperature = world.GetTemperatureData();

   // Secondary constants
   // Coldest spot in the world
   const float minTemp = *std::min_element(
      temperature.data(), temperature.data() + temperature.num_elements());

   // Upper temperature-limit for freezing effects
   const float freezeLimit = world.GetThreshold(TemperatureLevel::Polar);

   // Derived constants
   // Freeze threshold above minimum
   const float freezeThreshold =
      (freezeLimit - minTemp) * MAX_FREEZE_PERCENTAGE;
   const float freezeChanceThreshold =
      freezeThreshold * (1.0f - FREEZE_CHANCE_WINDOW);

   IcecapArrayType& icecap = world.GetIcecapData();
   icecap.resize(boost::extents[height][width]);

   // Map that is true whenever there is land or (certain) ice around
   SolidArrayType solidMap(boost::extents[height][width]);
   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         solidMap[y][x] = !ocean[y][x] || (temperature[y][x] <=
                                           freezeChanceThreshold + minTemp);
      }
   }

   // Will freeze: [minTemp, freezeChanceThreshold]
   // Can freeze:  (freezeChanceThreshold, freezeThreshold)
   static const std::vector<std::pair<float, float>> freezePoints(
      {{minTemp, 1.0f},
       {freezeChanceThreshold, 1.0f},
       {freezeThreshold, 0.0f}});
   static const std::vector<std::pair<uint32_t, float>> chancePoints(
      {{0, -1.0f}, {NUM_SURROUNDING_TILES, 1.0f}});

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         if (world.IsOcean(x, y))
         {
            float t = temperature[y][x];

            if (t - minTemp < freezeThreshold)
            {
               // Map temperature to freeze-chance (linear interpolation)
               float chance = Interpolate(t, freezePoints);

               // Count number of frozen/solid tiles around this one
               if (0 < x && x < width - 1 && 0 < y &&
                   y < height - 1) // Exclude borders
               {
                  // Count number of frozen/solid tiles around this one
                  uint32_t frozenTiles = 0;
                  for (int32_t ny = y - 1; ny <= y + 1; ny++)
                  {
                     for (int32_t nx = x - 1; nx <= x + 1; nx++)
                     {
                        if ((nx != x || ny != y) && solidMap[ny][nx])
                        {
                           frozenTiles++;
                        }
                     }
                  }

                  // Map number of tiles to chance-modifier
                  float chanceMod = Interpolate(frozenTiles, chancePoints);
                  chance += chanceMod * SURROUNDING_TILE_INFLUENCE;
               }

               if (distribution(generator) <= chance)
               {
                  solidMap[y][x] = true; // Mark tile as frozen
                  icecap[y][x] =
                     freezeThreshold - (t - minTemp); // Ice thickness
               }
            }
         }
      }
   }

   BOOST_LOG_TRIVIAL(info) << "Icecap simulation finish";
}

} // namespace WorldEngine


#include <iostream>
#include <map>
#include <fstream>

#include <unsupported/Eigen/KroneckerProduct>
#include <Eigen/Geometry>

#include <polyvec/curve-tracer/bezier_merging.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/geometry/angle.hpp>
#include <polyvec/geometry/line.hpp>
#include <polyvec/utils/string.hpp>
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/curve-tracer/curve_bezier.hpp>
#include <polyvec/shortest-path/dijkstra.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( BezierMerging )

// ===========================================
// Control points parameterization
// ===========================================

void
PointParameters::is_potracable ( bool& result, Eigen::Vector2d& oo, double& alpha, double& beta ) const {

#define undoable() do{ result = false; return; }while(0)

    // const double norm_cap = 10000;

    // if ( control_points.norm() > norm_cap ) {
    //     undoable();
    // }

    bool is_inflection_okay =  !AngleUtils::have_opposite_convexity_with_tol (
                                   control_points.col ( 0 ),
                                   control_points.col ( 1 ),
                                   control_points.col ( 2 ),
                                   control_points.col ( 3 )
                               );

    if ( !is_inflection_okay ) {
        undoable();
    }

    double lhs_at, rhs_at;
    bool do_intersect = ::polyvec::LineUtils::intersect (
                            control_points.col ( 0 ),  control_points.col ( 1 ),
                            control_points.col ( 3 ),  control_points.col ( 2 ),
                            lhs_at,
                            rhs_at );

    bool is_intersection_okay = do_intersect && ( lhs_at >= 1. )  && ( rhs_at >= 1. );

    if ( !is_intersection_okay ) {
        undoable();
    }

    oo = ::polyvec::LineUtils::line_at ( control_points.col ( 0 ),  control_points.col ( 1 ), lhs_at );
    alpha = 1. / lhs_at;
    beta = 1. / rhs_at;
    result = true;

#undef undoable
}

bool
PointParameters::is_potracable () const {
    Eigen::Vector2d oy;
    double alpha, beta;
    bool potracable;
    is_potracable ( potracable, oy, alpha, beta );
    return potracable;
}

PotraceParameters
PointParameters::as_potrace_params() const {
    bool potracable;
    Eigen::Vector2d oy;
    double alpha, beta;
    PotraceParameters ans;

    is_potracable ( potracable, oy, alpha, beta );

    if ( potracable ) {
        ans.alpha = alpha;
        ans.beta = beta;
        Mat23 p0, p1;
        //
        p0.col ( 0 ) << -1, 0;
        p0.col ( 1 ) << 0, 1;
        p0.col ( 2 ) << 1, 0;
        //
        p1.col ( 0 ) << control_points.col ( 0 );
        p1.col ( 1 ) << oy;
        p1.col ( 2 ) << control_points.col ( 3 );
        //
        find_affine_transformation ( p0, p1, ans.A, ans.b );
    } else {
        assert_break ( 0 );
    }

    return ans;
}

// ===========================================
// Potrace parameterization
// ===========================================

double
PotraceParameters::get_areaz() const {
    return 3./10* ( 2*alpha + 2*beta - alpha*beta );
}

double
PotraceParameters::get_areay() const {
    return std::abs ( polyvec::Num::determinant ( A ) ) *get_areaz();
}

PointParameters
PotraceParameters::get_zz() const {
    Mat24 ans;
    ans.col ( 0 ) << -1, 0;
    ans.col ( 1 ) << -1+alpha, alpha;
    ans.col ( 2 ) << 1-beta, beta;
    ans.col ( 3 ) << 1, 0;

    return {ans};
}

Eigen::Vector2d
PotraceParameters::get_oz() {
    return Eigen::Vector2d ( 0, 1 );
}

PointParameters
PotraceParameters::get_yy() const {
    return { ( A*get_zz().control_points ).colwise() + b};
}

Eigen::Vector2d
PotraceParameters::get_oy() const {
    return A*get_oz() + b;
}

PotraceParameters
PotraceParameters::equiparamed() const {
    PotraceParameters ans;
    ans = *this;
    ans.alpha = ans.beta = 2 - std::sqrt ( ( 2-alpha ) * ( 2-beta ) );
    return ans;
}

bool
PotraceParameters::is_ccw() const {
    return polyvec::Num::determinant ( A ) < 0;
}

// stolen from potrace <3
// calculate (p1-p0)x(p3-p2)
namespace {
	double
		_cprod(const Eigen::Vector2d& p0, const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const Eigen::Vector2d& p3) {
		double x1, y1, x2, y2;

		x1 = p1.x() - p0.x();
		y1 = p1.y() - p0.y();
		x2 = p3.x() - p2.x();
		y2 = p3.y() - p2.y();

		return x1 * y2 - x2 * y1;
	}

	// calculate the point t in [0..1] on the (convex) bezier curve
	//   (p0,p1,p2,p3) which is tangent to q1-q0. Return -1.0 if there is no
	//   solution in [0..1].
	double
		_tangent(const Eigen::Vector2d& p0, const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const Eigen::Vector2d& p3, const Eigen::Vector2d& q0,
			const Eigen::Vector2d& q1) {
		double A, B, C;   /* (1-t)^2 A + 2(1-t)t B + t^2 C = 0 */
		double a, b, c;   /* a t^2 + b t + c = 0 */
		double d, s, r1, r2;

		A = _cprod(p0, p1, q0, q1);
		B = _cprod(p1, p2, q0, q1);
		C = _cprod(p2, p3, q0, q1);

		a = A - 2 * B + C;
		b = -2 * A + 2 * B;
		c = A;

		d = b * b - 4 * a * c;

		if ((std::abs(a) < 1e-12)
			&& (std::abs(b) > 1e-12)
			&& (-c / b >= 0)
			&& (-c / b <= 1.)) {
			return -c / b;
		}

		if ((std::abs(a) < 1e-12) || (d < 1e-12)) {
			return -1.0;
		}

		s = sqrt(d);

		r1 = (-b + s) / (2 * a);
		r2 = (-b - s) / (2 * a);

		if (r1 >= 0 && r1 <= 1) {
			return r1;
		}
		else if (r2 >= 0 && r2 <= 1) {
			return r2;
		}
		else {
			return -1.0;
		}
	}
}

void
PotraceParameters::t_for_tangent ( const Eigen::Vector2d& tangent_y, double& t, bool& success ) const {
    const Mat24 yy = get_yy().control_points;
    t = _tangent ( yy.col ( 0 ), yy.col ( 1 ), yy.col ( 2 ), yy.col ( 3 ), Eigen::Vector2d::Zero(), tangent_y.normalized() );

    if ( t < 0 ) {
        success = false;
    } else {
        success = true;
    }

# if 0 // my own attempt. Potrace seems nicer.
    const double pi = ::polyvec::constants::PI;
    // Find the tangent in the reference coordiantes
    Eigen::Vector2d tangent_z = Num::solve_linear_system ( A, tangent_y );
    const Eigen::Vector2d zz = get_zz().control_points;
    assert_break ( tangent_z.norm() > 1e-10 );
    // Now find the angle of the tangent with the x axis
    // also find the begin and end angle of the bezier
    const double angle = std::atan2 ( tangent_z.y(), tangent_z.x() );
    const double begin_angle = std::atan2 ( zz.col ( 1 ).y() - zz.col ( 0 ).y(), zz.col ( 1 ).x() - zz.col ( 0 ).x() );
    const double end_angle = std::atan2 ( zz.col ( 3 ).y() - zz.col ( 2 ).y(), zz.col ( 3 ).x() - zz.col ( 2 ).x() );
    assert_break ( std::abs ( begin_angle-pi/4 ) < 1e-5 );
    assert_break ( std::abs ( end_angle+pi/4 ) < 1e-5 );
#endif
}

// ===========================================
// Functions
// ===========================================

void
find_affine_transformation ( const Mat23& points0, const Mat23& points1, Eigen::Matrix2d& A, Eigen::Vector2d& b ) {
    Eigen::Matrix<double, 6, 6> LHS;
    Eigen::Matrix<double, 6, 1> RHS;
    Eigen::Matrix<double, 6, 1> unknowns;

    Eigen::Matrix2d eye =  Eigen::Matrix2d::Identity();

    LHS.block<2, 4> ( 0, 0 ) = Eigen::kroneckerProduct ( points0.col ( 0 ).transpose(), eye );
    LHS.block<2, 4> ( 2, 0 ) = Eigen::kroneckerProduct ( points0.col ( 1 ).transpose(), eye );
    LHS.block<2, 4> ( 4, 0 ) = Eigen::kroneckerProduct ( points0.col ( 2 ).transpose(), eye );
    //
    LHS.block<2, 2> ( 0, 4 ) = eye;
    LHS.block<2, 2> ( 2, 4 ) = eye;
    LHS.block<2, 2> ( 4, 4 ) = eye;
    //
    RHS.segment<2> ( 0 ) = points1.col ( 0 );
    RHS.segment<2> ( 2 ) = points1.col ( 1 );
    RHS.segment<2> ( 4 ) = points1.col ( 2 );

    // Debugging
    //std::cout << LHS << std::endl;
    //std::cout << Num::determinant(LHS) << std::endl;

    unknowns = polyvec::Num::solve_linear_system ( LHS, RHS );

    A.col ( 0 ) = unknowns.segment<2> ( 0 );
    A.col ( 1 ) = unknowns.segment<2> ( 2 );
    b = unknowns.segment<2> ( 4 );

    // Make sure it is working
    const double error = ( ( A*points0 ).colwise() + b - points1 ).norm() ;
    assert_break ( error < 1e-10 );
}

namespace {
bool
skip_comments ( FILE* fl ) {
    char tmp[4096];
    bool is_there_more_to_read = false;
    assert_break ( fl );

    for ( ;; ) {
        const int c = fgetc ( fl );

        if ( c ==EOF ) {
            is_there_more_to_read = false;
            break;
        } else if ( c == '#' ) {
            fgets ( tmp, 4096, fl );
            continue;
        } else if ( ( c != ' ' ) && ( c != '\n' ) && ( c != '\r' ) ) {
            int success = fseek ( fl, -1, SEEK_CUR );
            assert ( success == 0 );
            is_there_more_to_read = true;
            break;
        }
    }

    return is_there_more_to_read;
}
}

void
dump_curve_sequence ( const std::vector<Eigen::Matrix2Xd>& curves, FILE* file ) {
    fprintf ( file, "# N curves \n %d \n", ( int ) curves.size() );

    for ( int i = 0 ; i < curves.size() ; ++ i ) {
        fprintf ( file, "# Curve %d, n points \n %d \n", i, ( int ) curves[i].cols() );

        for ( int j = 0 ; j < curves[i].cols() ; ++ j ) {
            fprintf ( file, "%.12g ", curves[i] ( 0, j ) );
        }

        fprintf ( file, "\n" );

        for ( int j = 0 ; j < curves[i].cols() ; ++ j ) {
            fprintf ( file, "%.12g ", curves[i] ( 1, j ) );
        }

        fprintf ( file, "\n" );
    }
}

std::vector<Eigen::Matrix2Xd>
read_curve_sequence ( FILE* file ) {
#define ENSURE(X) if(!(X)) {assert_break(0);}

    int n_curves;
    ENSURE ( skip_comments ( file ) );
    ENSURE ( fscanf ( file, "%d", &n_curves ) == 1 );

    std::vector<Eigen::Matrix2Xd> curves ( n_curves );

    for ( int i = 0 ; i < curves.size() ; ++ i ) {
        int n_points;
        ENSURE ( skip_comments ( file ) );
        ENSURE ( fscanf ( file, "%d", &n_points ) == 1 );
        curves[i].resize ( 2, n_points );

        ENSURE ( skip_comments ( file ) );

        for ( int j = 0 ; j < curves[i].cols() ; ++ j ) {
            ENSURE ( fscanf ( file, "%lf", &curves[i] ( 0, j ) ) == 1 );
        }

        ENSURE ( skip_comments ( file ) );

        for ( int j = 0 ; j < curves[i].cols() ; ++ j ) {
            ENSURE ( fscanf ( file, "%lf", &curves[i] ( 1, j ) ) == 1 );
        }
    }

    return curves;

#undef ENSURE
}

void merged_curve (
    const std::vector<Eigen::Matrix2d>& line_begin,
    const std::vector<Mat24>& beziers,
    const std::vector<Eigen::Matrix2d>& line_end,
    FILE* verbose,
    bool& doable,
    Mat24& ans_mat ) {

    PotraceParameters ans;

    auto eprintf = [verbose] ( const std::string strr ) {
        if ( verbose ) {
            fprintf ( verbose, "%s", strr.c_str() );
        }
    };

#define undoable() do{doable =false; eprintf(""); return;}while(0)

    assert_break ( line_begin.size() <= 1 );
    assert_break ( line_end.size() <= 1 );
    assert_break ( beziers.size() >= 1 );

    // ============
    // First check that each bezier should be potracable
    // ============
    for ( int i = 0 ; i < ( int ) beziers.size() ; ++i ) {
        if ( !PointParameters ( {beziers[i]} ).is_potracable() ) {
            eprintf ( StringUtils::fmt ( "curve[%d] is not potracable \n", i ) );
            undoable();
        }
    }

    // ============
    // Now check that transitions from each curve also does not
    // introduce an inflection
    // The assumption is that the tangents of all the transitions match
    // ============

    // line begin
    if ( line_begin.size() ) {
        if ( AngleUtils::have_opposite_convexity (
                    line_begin[0].col ( 0 ),
                    beziers[0].col ( 1 ),
                    beziers[0].col ( 2 ),
                    beziers[0].col ( 3 ) ) ) {
            eprintf ( StringUtils::fmt ( "line0 to bezier 1 inflected \n" ) );
            undoable();
        }
    }

    // line end
    if ( line_end.size() ) {
        if ( AngleUtils::have_opposite_convexity (
                    beziers.back().col ( 0 ),
                    beziers.back().col ( 1 ),
                    beziers.back().col ( 2 ),
                    line_end[0].col ( 1 ) ) ) {
            eprintf ( StringUtils::fmt ( "line_end to bezier end inflected \n" ) );
            undoable();
        }
    }

    // Beziers
    for ( int i = 0 ; i < ( int ) beziers.size()-1 ; ++i ) {
        if ( AngleUtils::have_opposite_convexity (
                    beziers[i].col ( 1 ),
                    beziers[i].col ( 2 ),
                    beziers[i+1].col ( 1 ),
                    beziers[i+1].col ( 2 ) ) ) {
            eprintf ( StringUtils::fmt ( "bezier[%d] to bezier[%d] inflected \n", i, i +1 ) );
            undoable();
        }
    }

    // ============
    //   Check that the first and last line intersection each other
    // ============
    Eigen::Vector2d y0, y3, oy;
    {
        Eigen::Matrix2d ray_begin;
        Eigen::Matrix2d ray_end;

        if ( line_begin.size() ) {
            ray_begin = line_begin.front();
        } else {
            ray_begin = beziers.front().leftCols<2>();
        }

        if ( line_end.size() ) {
            ray_end = line_end.front();
        } else {
            ray_end = beziers.back().rightCols<2>();
        }

        ray_end.rowwise().reverseInPlace();

        double lhs_at, rhs_at;
        const bool do_intersect = ::polyvec::LineUtils::intersect ( ray_begin.col ( 0 ), ray_begin.col ( 1 ), ray_end.col ( 0 ), ray_end.col ( 1 ), lhs_at, rhs_at );

        if ( ( !do_intersect )
                || ( lhs_at < 1. )
                || ( rhs_at < 1. ) ) {
            eprintf ( StringUtils::fmt ( "Intersection Problem \n" ) );
            undoable();
        }

        y0 = ray_begin.col ( 0 );
        y3 = ray_end.col ( 0 );
        oy = ::polyvec::LineUtils::line_at ( ray_begin.col ( 0 ), ray_begin.col ( 1 ), lhs_at );

        // This is intersecting on the other side of the universe
        if ( oy.norm() > 10000 ) {
            //polyvec::VtkCurveWriter writer;
            //writer.add_polyline( ray_begin );
            //writer.add_polyline( ray_end   );
            //writer.dump("DAMN.vtk");
            undoable();
        }
    }

    // ============
    //  Now find the area of the bezier sequence
    // ============
    double area_shape = 0;
    {
        auto find_carpet  = [&line_begin, &line_end, &beziers] {

            Eigen::Matrix2Xd carpet ( 2, beziers.size() + line_begin.size() + line_end.size() + 1 );
            int colid = 0;

            if ( line_begin.size() ) {
                carpet.col ( colid ) = line_begin.front().col ( 0 );
                ++colid;
            }

            for ( int i = 0 ; i < ( int ) beziers.size() ; ++i ) {
                carpet.col ( colid ) = beziers[i].col ( 0 );
                ++colid;
            }

            if ( line_end.size() ) {
                carpet.col ( colid ) = line_end.front().col ( 0 );
                ++colid;
                carpet.col ( colid ) = line_end.front().col ( 1 );
                ++colid;
            } else {
                carpet.col ( colid ) = beziers.back().col ( 3 );
                ++colid;
            }

            assert_break ( colid == ( int ) carpet.cols() );

            return carpet;
        }; // find carpet

        bool is_carpet_ccw;
        double area_carpet;
        Eigen::Matrix2Xd carpet = find_carpet ();
        ::polyvec::WindingNumber::compute_orientation ( carpet, is_carpet_ccw, area_carpet );

        double area_beziers = 0;

        for ( int i = 0 ; i < ( int ) beziers.size() ; ++i ) {
            area_beziers += PointParameters ( {beziers[i]} ).as_potrace_params().get_areay();
        }

        area_shape = area_carpet + area_beziers;
    }

    //
    // If we are here, we can finally create the merged bezier
    //
    {
        Mat23 p0, p1;
        //
        p0.col ( 0 ) << -1, 0;
        p0.col ( 1 ) << 0, 1;
        p0.col ( 2 ) << 1, 0;
        //
        p1.col ( 0 ) << y0;
        p1.col ( 1 ) << oy;
        p1.col ( 2 ) << y3;
        //
        find_affine_transformation ( p0, p1, ans.A, ans.b );

        const double area_ref  =  area_shape / std::abs ( polyvec::Num::determinant ( ans.A ) );

        assert_break( !std::isnan( area_ref ) );
        if ( ( area_ref > 1.19 ) ) {
            eprintf ( StringUtils::fmt ( "Area Problem \n" ) );
            undoable();
        }

        ans.alpha =   ans.beta  = 2. - sqrt ( 4-10*area_ref/3. ) ;
        assert_break (  ans.beta > 0 );

        if ( ans.beta > 1 ) {
            // DEBUGGING - TEMP, REMOVE
            //for (auto bz : beziers ) {
            //    std::cout << bz.row(0) << " " << bz.row(1) << std::endl;
            // }
            undoable();
        }

        assert_break ( area_ref > 0 );
        assert_break ( std::abs ( area_ref - ans.get_areaz() ) < 1e-4 );
        ans_mat = ans.get_yy().control_points;
        doable = true;
    }

#undef undoable
}

void
is_merge_acceptable (
    const std::vector<Mat24>& beziers,
    const Mat24& merged_bezier,
    const double tolerance,
    const double alphamax,
    double& penalty,
    bool& is_acceptable ) {

    //
    // HELPERS
    //

    auto get_distance = [] ( const PotraceParameters &bzp, ::polyvec::BezierCurve &bz, const Eigen::Vector2d& p0, const Eigen::Vector2d& p1,
    bool &proj_success, double &dist ) {
        double t;
        bzp.t_for_tangent ( p1-p0, t, proj_success );

        if ( !proj_success ) {
            return;
        }

        dist = ::polyvec::LineUtils::distance_from_point ( p0, p1, bz.pos ( t ) );
    };

    auto get_distance_signed = [] ( const PotraceParameters &bzp,  ::polyvec::BezierCurve &bz, const Eigen::Vector2d& p0, const Eigen::Vector2d& p1,
    const Eigen::Vector2d pforsign,  bool &proj_success, double &dist ) {
        double t;
        bzp.t_for_tangent ( p1-p0, t, proj_success );

        if ( !proj_success ) {
            return;
        }

        const double dist_pref =  ::polyvec::LineUtils::signed_distance_from_point ( p0, p1, pforsign );
        dist =  ::polyvec::LineUtils::signed_distance_from_point ( p0, p1, bz.pos ( t ) );

        if ( dist*dist_pref < 0 ) {
            dist = -std::abs ( dist );
        } else {
            dist = std::abs ( dist );
        }
    };

    auto check_distance_a = [tolerance] ( const double dist_a ) {
        assert_break ( dist_a >= 0 );

        if ( dist_a > tolerance ) {
            return false;
        }

        return true;
    };

    auto check_distance_b = [tolerance] ( const double dist_b ) {
        if ( dist_b < -tolerance ) {
            return false;
        }

        return true;
    };

    //
    // Loop over all beziers. Calculate the dist_a and dist_b distances
    // from the potrace paper.
    //
    penalty = 0;
    is_acceptable = true;
    ::polyvec::BezierCurve eval_ctx;
    eval_ctx.set_control_points ( merged_bezier );
    PotraceParameters project_ctx;
    project_ctx = PointParameters ( {merged_bezier} ).as_potrace_params();

    //
    // Check alpha
    //
    if ( project_ctx.alpha > alphamax ) {
        is_acceptable = false;
        return;
    }

    //
    // Check distace
    //
    for ( int i = 0 ; i < ( int ) beziers.size() ; ++i ) {

        // dist a
        const Eigen::Vector2d a = PointParameters ( {beziers[i]} ).as_potrace_params().get_oy();

        if ( i < ( int ) beziers.size() -1 ) {
            const Eigen::Vector2d anext = PointParameters ( {beziers[i+1]} ).as_potrace_params().get_oy();
            bool can_project;
            double dist_a;
            get_distance ( project_ctx, eval_ctx, a, anext, can_project, dist_a );

            if ( ( !can_project ) || ( !check_distance_a ( dist_a ) ) ) {
                is_acceptable = false;
                return;
            }

            penalty += dist_a * dist_a;
        } // end of dist a

        // dist b
        {
            const Eigen::Vector2d b = beziers[i].col ( 0 );
            const Eigen::Vector2d bnext =  beziers[i].col ( 3 );
            bool can_project;
            double dist_b;
            get_distance_signed ( project_ctx, eval_ctx, b, bnext, a, can_project, dist_b );

            if ( ( !can_project ) || ( !check_distance_b ( dist_b ) ) ) {
                is_acceptable = false;
                return;
            }

            penalty += dist_b * dist_b;
        } // end of dist b
    } // end of beziers

} //  merge_penalty()

void merge_penalty (
    const std::vector<Mat24>& beziers,
    const Mat24& merged_bezier,
    double& penalty ) {

    // Do a one sided projection of the points
    constexpr int n_samples = 10;

    penalty = 0;
    ::polyvec::BezierCurve big_eval_ctx;
    big_eval_ctx.set_control_points ( merged_bezier );

    for ( int i = 0 ; i < ( int ) beziers.size() ; ++i ) {
        ::polyvec::BezierCurve sub_eval_ctx;
        sub_eval_ctx.set_control_points ( beziers[i] );
        // This is a question . Should we multiply by length?
        const double weight = 1. / n_samples *  sub_eval_ctx.length();
        // Perhaps not
        // const double weight = 1. / n_samples;

        for ( int j = 1 ; j < ( n_samples+1 ) ; ++j ) {
            const double t_sub = 1. / ( n_samples+1 ) * j ;
            const Eigen::Vector2d point_sub = sub_eval_ctx.pos ( t_sub );
            const double t_big = big_eval_ctx.project ( point_sub );
            const Eigen::Vector2d point_big = big_eval_ctx.pos ( t_big );
            penalty += weight * ( point_big-point_sub ).norm();
        } // end of points
    }  // end of bezeirs
}

void
merge_curves (
    const std::vector<Eigen::Matrix2Xd>& curves_in, // lines of bezier control points
    const bool is_circular,
    const MergeRecursivelyOptions options,
	const std::set<Regularity::SymmetryPair>& symmetric_curves,
    std::vector<Eigen::Matrix2Xd>& curves_out,
    std::vector<std::vector<int>>& out2in  ) {

    const int n_curves = ( int ) curves_in.size();

    //
    // Check if a curve is right after a corner
    //
    auto is_line_after_corner = [&n_curves, &curves_in, &is_circular] ( const int i ) {
        if ( ( i == 0 ) && ( !is_circular ) ) {
            return false;
        } else {
            const int iprev = ( ( i-1 )+n_curves ) %n_curves;

            if ( ( curves_in[iprev].cols() == 2 )
                    && ( curves_in[i].cols() == 2 ) ) {
                return true;
            } else {
                return false;
            }
        }
    };

    //
    // Prepare the data for a merge curve
    //
    auto get_merge_data = [&n_curves, &curves_in, &is_circular]
                          ( const int i,
                            const int n_merge,
                            std::vector<Mat22>& line_begin,
                            std::vector<Mat24>& beziers,
                            std::vector<Mat22>& line_end,
    std::vector<int>& parent_ids ) {

        assert_break ( is_circular || ( n_merge+i<=n_curves ) );
        line_begin.resize ( 0 );
        line_end.resize ( 0 );
        beziers.resize ( 0 );
        parent_ids.resize ( 0 );

        for ( int offset = 0 ; offset < n_merge ; ++offset ) {
            const int curve_id = ( i + offset ) %n_curves;

            if ( ( curves_in[curve_id].cols() == 2 ) && ( offset==0 ) ) {
                line_begin.push_back ( curves_in[curve_id] );
            } else if ( ( curves_in[curve_id].cols() == 2 ) && ( offset==n_merge-1 ) ) {
                line_end.push_back ( curves_in[curve_id] );
            }
            else {
                assert_break ( curves_in[curve_id].cols() == 4 );
                beziers.push_back ( curves_in[curve_id] );
            }

            parent_ids.push_back ( curve_id );
        }

    };

    //
    // Convert a curves end or begin point into node ids
    //
    auto n_graph_nodes = [&is_circular, &curves_in, &n_curves]() {
        if ( is_circular  ) {
            return n_curves;
        } else {
            return n_curves+1;
        }
    };
    auto curve_end_node = [&is_circular, &curves_in, &n_curves] ( const int i ) {
        if ( is_circular && ( i == n_curves-1 ) ) {
            return 0;
        } else {
            return i+1;
        }
    };
    auto curve_begin_node = [&is_circular, &curves_in, &n_curves] ( const int i ) {
        return i;
    };

    //
    // All the possible merge situations
    //
    std::vector<ShortestPath::Node> graph_nodes ( n_graph_nodes() );
    std::map<std::pair<int, int>, int> mc_map;
    std::vector<double> mc_penalty;
    std::vector<std::vector<int>> mc_parent_ids;
    std::vector<Eigen::Matrix2Xd> mc_pts;

    //
    // Create all the merge candidates
    //
    {
        std::vector<Mat22> line_begin;
        std::vector<Mat24> beziers;
        std::vector<Mat22> line_end;
        ShortestPath::Node node;
        std::vector<int> parent_ids;
        Mat24 merged_bezier;
        bool is_merge_doable;
        double potrace_penalty;
        double distace_penalty;

        for ( int cbeginid = 0 ; cbeginid < n_curves ; ++cbeginid ) {
            const int max_n_merge = is_circular ? n_curves : n_curves - cbeginid;

			// find the next symmetry pair after the current curve
			auto next_symmetry = std::lower_bound(symmetric_curves.begin(), symmetric_curves.end(), Regularity::SymmetryPair(cbeginid, cbeginid, -1));
			if (next_symmetry == symmetric_curves.end())
				next_symmetry = symmetric_curves.begin(); //wrap around

			// We employ the following strategy to promote symmetries:
			// A merge is only valid if all its symmetries are completely within the
			// merge or none is (i.e., it is ok if only one curve of symmetry pairs
			// is within the merge). This is evaluated independently for each
			// participating symmetric region

			struct SymmetryInfo
			{
				// stores the number of symmetric pairs that are completely
				// contained
				int n_complete_symmetries = 0;

				// stores the number of symmetric paris where only one part is
				// contained
				int n_partial_symmetries = 0;
			};

			std::map<int, SymmetryInfo> symmetry_info;

            for ( int n_merge = 1 ; n_merge <= max_n_merge ; ++n_merge ) {

				const int cendid = (cbeginid + n_merge - 1) % n_curves;

				// check if we have new symmetries
				while (next_symmetry != symmetric_curves.end() && next_symmetry->first == cendid)
				{
					auto& info = symmetry_info[next_symmetry->region];
					// check if the other curve is already in the merge
					if (PathUtils::contains_closed(curves_in.size(), cbeginid, cendid, next_symmetry->second, is_circular))
					{
						++info.n_complete_symmetries;
						--info.n_partial_symmetries;
					}
					else
						++info.n_partial_symmetries;

					++next_symmetry;
					if (next_symmetry == symmetric_curves.end())
						next_symmetry = symmetric_curves.begin(); //wrap around
				}

                // If n_merge is just 1 then we don't have to do much
                // The fit is accepted and the error is 0
                if ( n_merge==1 ) {
                    mc_pts.push_back ( curves_in[cbeginid] );
                    mc_penalty.push_back ( 0 );
                    mc_parent_ids.push_back ( {cbeginid} );
                } else {

                    // Check that we do not pass a corner
                    const int cendidm1 = ( cbeginid+n_merge-2 ) %n_curves;

                    // Handle lines
                    if ( options.allow_merging_lines ) {
                        // If were were fitting to potrace data this should be enough
                        if (  is_line_after_corner ( cendid ) ) {
                            break;
                        }
                        // For ed's assymetric case, we also need this
                        if ( ( curves_in[cendidm1].cols() == 2 ) && ( cendidm1 != cbeginid) ) {
                            break;
                        }
                    }

                    // New strategy, should work for Ed too
                    // Simply don't let cbeginid or cendid to become line
                    // basically we are not attempting to merge lines with anything
                    if ( !options.allow_merging_lines ) {
                        if ( ( curves_in[cbeginid].cols() == 2 ) || ( curves_in[cendid].cols() == 2 ) ) {
                            break;
                        }
                    }

					bool symmetry_valid = true;
					for(auto& info : symmetry_info)
						if (info.second.n_complete_symmetries > 0 && info.second.n_partial_symmetries > 0)
							symmetry_valid = false; // violates symmetry
					if (!symmetry_valid)
						continue;

                    // Get info needed for merging
                    get_merge_data ( cbeginid, n_merge, line_begin, beziers, line_end, parent_ids );

                    // Merge the curve
                    merged_curve ( line_begin, beziers, line_end, nullptr, is_merge_doable, merged_bezier );

                    if ( !is_merge_doable ) {
                        continue;
                    }

                    // Check if acceptable
                    is_merge_acceptable ( beziers, merged_bezier, options.distance_tolerance, options.alpha_max, potrace_penalty, is_merge_doable );

                    if ( !is_merge_doable ) {
                        continue;
                    }

                    // Find the penalty
                    merge_penalty ( beziers, merged_bezier, distace_penalty );

                    // Add to the curve to nodes
                    mc_pts.push_back ( merged_bezier );
                    mc_penalty.push_back ( distace_penalty );
                    mc_parent_ids.push_back ( parent_ids );

                } // end of more than one curve

            } // end of n_merge
        } // end of begin id
    } // end of creating the merge candidates

    //
    // Now create a graph
    //
    for ( int node_id = 0 ; node_id < n_graph_nodes() ; ++node_id  ) {
        graph_nodes[node_id].v = node_id;
    }

    for ( int mcid = 0 ; mcid < ( int ) mc_parent_ids.size() ; ++mcid ) {
        const int node_from = curve_begin_node ( mc_parent_ids[mcid].front() );
        const int node_to = curve_end_node ( mc_parent_ids[mcid].back() );
        graph_nodes[node_from].add_neighbor(node_to, mc_penalty[mcid] + options.per_bezier_const );
        mc_map.insert ( std::make_pair ( std::make_pair ( node_from, node_to ), mcid ) );
    }

    // Find the shortest path
    {
        std::vector<Eigen::Index> P;
        ShortestPath::State state;

        if ( is_circular ) {
            ShortestPath::find_cycle ( state, graph_nodes, {0}, P );
            // Debugging
            // ShortestPath::print_graph ( graph_nodes, stderr );
        } else {
            ShortestPath::find ( state, graph_nodes, 0, n_graph_nodes()-1, P );
        }

        const int max_node_offset = is_circular ? ( int ) P.size() : ( int ) P.size()-1;

        curves_out.resize ( 0 );
        out2in.resize ( 0 );

        for ( int nodeoffset = 0 ; nodeoffset <  max_node_offset ; ++nodeoffset ) {
            const int node_from = (int)P[nodeoffset];
            const int node_to = (int)P[ ( nodeoffset+1 ) %P.size()];
            auto mc_id_it = mc_map.find ( std::make_pair ( node_from, node_to ) );
            assert_break ( mc_id_it != mc_map.end() );
            const int mc_id = (int)mc_id_it->second;

            curves_out.push_back ( mc_pts[ mc_id ] );
            out2in.push_back ( mc_parent_ids[ mc_id ] );
        }

    }

} // merge_recursively()

NAMESPACE_END ( BezierMerging )
NAMESPACE_END ( polyfit )


#include <optional>

#include <boost/test/unit_test.hpp>

#include <smtrat-common/smtrat-common.h>
#include <smtrat-mcsat/smtrat-mcsat.h>

#include <carl/core/Variable.h>
#include <carl-model/evaluation/ModelEvaluation.h>

#include <smtrat-mcsat/explanations/vs/VSHelper.h>

using namespace smtrat;

BOOST_AUTO_TEST_CASE(McsatVSBug) {
	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");
  carl::Variable z = carl::freshRealVariable("z");

  // 100 + -49*y^2 + -49*z^2 + 98*z*x + -98*x^2 < 0
  Poly p = Poly(Rational(100)) - Poly(Rational(49))*y*y - Poly(Rational(49))*z*z + Poly(Rational(98))*z*x - Poly(Rational(98))*x*x;
  ConstraintT c1(p, carl::Relation::LESS);

  // proof that f1 is false given a model
  Model m;
  m.assign(x, Rational(10)/7);
  m.assign(y, Rational(0));
  m.assign(z, Rational(10)/7);

  auto res = carl::model::evaluate(FormulaT(c1), m);
	BOOST_CHECK(res.isBool());
  BOOST_CHECK(!res.asBool());

  auto res2 = carl::model::evaluate(p, m);
	BOOST_CHECK(res2.isRational());
  BOOST_CHECK(res2.asRational() >= 0);

  // but VS returns true after substituting x -> 10/7 into c1
  ::vs::Substitution sub(x, SqrtEx(Poly(Rational(10)/7)), ::vs::Substitution::Type::NORMAL, carl::PointerSet<::vs::Condition>());
  smtrat::vs::DisjunctionOfConstraintConjunctions subResTmp;
  bool r = mcsat::vs::helper::substituteHelper(c1, sub, subResTmp);
  BOOST_CHECK(r);
  auto subRes = mcsat::vs::helper::doccToFormula(subResTmp);
  BOOST_CHECK(subRes.getType() != carl::FormulaType::TRUE);

  // The bug lies in splitSosDecompositions()
}


#include "sortition.h"
#include <boost/math/distributions/binomial.hpp>

uint64_t sortition_binomial_cdf_walk(double n, double p, double ratio, uint64_t money) {
  boost::math::binomial_distribution<double> dist(n, p);
  for (uint64_t j = 0; j < money; j++) {
    // Get the cdf
    double boundary = cdf(dist, j);

    // Found the correct boundary, break
    if (ratio <= boundary) {
      return j;
    }
  }
  return money;
}


/**
 * Copyright (c) 2012 Andrew Prock. All rights reserved.
 */
#include "CardDistribution.h"

#include <iostream>
#include <vector>

#include <fmt/string.h>
#include <cppext/split.h>

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

#include <pokerstove/util/combinations.h>
#include <pokerstove/peval/Card.h>

using namespace std;
using namespace pokerstove;

CardDistribution::CardDistribution()
    : _handList(1,CardSet())
    , _weights()
{
    _weights[CardSet()] = 1.0;
}

CardDistribution::CardDistribution(const CardSet& cs)
    : _handList(1,cs)
    , _weights()
{
    _weights[cs] = 1.0;
}

CardDistribution::CardDistribution(const CardDistribution& cd)
{
    *this = cd;
}

CardDistribution& CardDistribution::operator=(const CardDistribution& other)
{
    _handList = other._handList;
    _weights = other._weights;
    return *this;
}

size_t CardDistribution::size() const
{
    return _handList.size();
}

string CardDistribution::str() const
{
    string ret;
    for (size_t i=0; i<_handList.size(); i++)
    {
        const CardSet& hand = _handList[i];
        ret += (i>0?",":"") + fmt::format("{}={:.3f}", hand.str(), weight(hand));
    }
    return ret;
}

void CardDistribution::clear()
{
    _handList.clear();
    _weights.clear();
}

CardDistribution CardDistribution::data() const
{
    return *this;
}

void CardDistribution::fill(int n)
{
    CardSet cards;
    cards.fill();
    fill(cards, n);
}

void CardDistribution::fill (const CardSet& cs, int n)
{
    vector<Card> cards = cs.cards ();
    int setsize = static_cast<int>(cards.size());
    combinations hands(setsize, n);
    int vsize = boost::math::binomial_coefficient<double>(setsize, n);
    clear();
    _handList.reserve (vsize);

    do
    {
        CardSet cs;
        for (int i=0; i<n; i++)
            cs.insert(cards[hands[i]]);
        _handList.push_back(cs);
        _weights[cs] = 1.0;
    }
    while (hands.next());
}

const CardSet& CardDistribution::operator[](size_t index) const
{
    if (index >= _handList.size())
        throw std::runtime_error ("CardDistribution::operator: bounds error");
    return _handList[index];
}

const double& CardDistribution::operator[](const CardSet& hand) const
{
    static const double kStaticZero = 0.0; // for hands not in distribution
    map<CardSet,double>::const_iterator it = _weights.find(hand);
    if (it == _weights.end())
        return kStaticZero;
    return it->second;
}

double& CardDistribution::operator[](const CardSet& hand)
{
    return _weights[hand];
}

// this is a simple wrapper to make reading the code easier
const double& CardDistribution::weight(const CardSet& hand) const
{
    return (*this)[hand];
}

bool CardDistribution::parse(const std::string& input)
{
    clear();

    // trap the special "unrealized/empty" distribution which
    // has no specific cards, and is equivalent to a "random" dist
    // TODO: verify that this is the internal representation we want for a
    // random hand
    if (input == ".")
    {
        CardSet hand;
        if (hand.size() != 0)
            return false;
        _weights[hand] = 1.0;
        _handList.push_back(hand);
        return true;
    }

    vector<string> hands = cppext::split(input, string(","));
    for(const string& h : hands)
    {
        // handle the weight
        double weight = 1.0;
        if (h.find("=") != string::npos)
        {
            // trap for the case where the input ends with "="
            string::size_type weightPos = h.rfind("=")+1;
            if (weightPos == h.size())
                return false;
			weight = stod(h.substr(weightPos));
        }

        // handle the hand one card at a time.
        string handstr = h.substr(0,h.rfind("="));
        if (handstr.size()%2 != 0)
            return false;
        CardSet hand;
        for (size_t i=0; i<handstr.size(); i+=2)
        {
            Card c;
            if (!c.fromString(handstr.substr(i,i+2)))
                return false;
            if (hand.contains(c))
                return false;
            hand.insert(c);
        }

        // final check and
        if (hand.size() == 0)
            return false;
        _weights[hand] = weight;
        _handList.push_back(hand);
    }
    return true;
}

std::string CardDistribution::display() const
{
    return str();
}

void CardDistribution::removeCards(const CardSet& dead)
{
    for (size_t i=0; i<_handList.size(); i++)
        if (_handList[i].intersects(dead))
            _weights[_handList[i]] = 0.0;
}

double CardDistribution::weight() const
{
    double total = 0.0;

    for(std::pair<CardSet, double> w : _weights)
    {
        total += w.second;
    }
    return total;
}


#include "hdf5.hpp"

#include <Eigen/Dense>

#include <iostream>

void write_data() {
    Eigen::MatrixXd matrix(3, 3);
    matrix << 1, 2, 3, 4, 5, 6, 7, 8, 9;

    // open the file
    HDF5::File hf = HDF5::File("filename.hdf5", HDF5::File::Truncate);

    // write the data
    hf.write("dataset_name", matrix);

    std::cout << "Original Matrix: " << std::endl;
    std::cout << matrix << std::endl;
}

void read_data() {
    // open the file for reading
    HDF5::File hf = HDF5::File("filename.hdf5", HDF5::File::ReadOnly);

    Eigen::MatrixXd matrix;

    hf.read("dataset_name", matrix);

    std::cout << "Matrix read: " << std::endl;
    std::cout << matrix << std::endl;
}

int main() {
    write_data();
    read_data();
    return 0;
}


//
// Implements Tree-iLQR.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <ilqr/tree.hh>
#include <ilqr/ilqr_node.hh>

#include <Eigen/Dense>

#include <memory>
#include <list>
#include <vector>

namespace ilqr
{

// State-Action-Cost-Value(CostToGo).
struct SACV
{
    Eigen::VectorXd x;
    Eigen::VectorXd u;
    double c;
    double probability;
    double value;
};

// Shared pointer to a Node in the underlying Tree structure of the iLQR-Tree. Calling ->item()
// returns a shared pointer to the iLQRNode that holds the state/control/dynamics/etc.
// information.
using TreeNodePtr = std::shared_ptr<data::Node<iLQRNode>>;

class iLQRTree
{
public:
    iLQRTree(int state_dim, int control_dim);
    virtual ~iLQRTree() = default;

    // Construct a iLQRNode which represents the dynamics, cost functions as
    // well as the state and control policy at a specific time step along the
    // tree. Nominal state and control inputs are passed and used for the
    // initial Taylor expansions of the cost and dynamics functions.
    std::shared_ptr<iLQRNode> make_ilqr_node(const Eigen::VectorXd &x_star,
            const Eigen::VectorXd &u_star, const DynamicsFunc &dynamics, const
            CostFunc &cost, const double probablity);

    // Add the root node to the iLQR Tree with similar arguments to the
    // make_ilqr_node function.
    TreeNodePtr add_root(const Eigen::VectorXd &x_star, const Eigen::VectorXd
            &u_star, const DynamicsFunc &dynamics, const CostFunc &cost);

    // Add a iLQRNode as the root node to the iLQR Tree. Requires the
    // probability to be 1.
    TreeNodePtr add_root(const std::shared_ptr<iLQRNode> &ilqr_node);

    // Add a list of iLQRNodes as children under a parent. The probabilities
    // must sum to 1.
    std::vector<TreeNodePtr> add_nodes(const
            std::vector<std::shared_ptr<iLQRNode>> &ilqr_nodes, TreeNodePtr
            &parent);

    // Get the root node of the Tree.
    TreeNodePtr root();

    // Forward pass to generate new nominal points for iLQR.
    // The step-size is controlled by alpha (alpha * K).
    void forward_tree_update(const double alpha);

    // Do a full bellman backup on the tree.
    void bellman_tree_backup();

    // Returns a Tree of states-action-costs-value with expected cost-to-gos.
    data::Tree<SACV> forward_pass(const Eigen::VectorXd &x0, const TreeNodePtr &top = nullptr, const double top_alpha = 1.0);

private:
    int state_dim_ = 0;
    int control_dim_ = 0;

    data::Tree<iLQRNode> tree_;

    // Zeros matrix of size [state_dim +1] x [state_dim +1]. Used as
    // initialization for the zeros matrix.
    const QuadraticValue ZERO_VALUE_;

    // Special case for just the leaves of the tree. We can compute this by
    // giving the leaves synthetic children with $V_{T+1} = 0$.
    void control_and_value_for_leaves();

    // Backups the value matrix and control gains matrix from the children of
    // the node to get the value and control policies for the parents of the
    // children. Returns a list of all the parents.
    std::list<TreeNodePtr> backup_to_parents(const std::list<TreeNodePtr>
            &all_children);

};

} // namespace ilqr


#include <boost/test/unit_test.hpp>
#include <vector>
#include <numeric>
#include <limits>
#include <list>
#include <set>
#include <map>
#include <stack>
#include <math.h>
#include <stdio.h>

using namespace std;

int nesting_1( const string &S )
{
    stack<char> q;

    string::const_iterator I;
    for( I=S.begin(); I!=S.end(); ++I )
    {
        const char& c( *I );
        if ( c == '(' )
        {
            q.push( c );
        }
        else if ( c== ')' )
        {
            if( q.size() == 0 )
            {
                return 0;
            }

            q.pop();
        }
    }

    if ( q.size() > 0 )
        return 0;

    return 1;
}

int nesting( const string &S )
{
    int q = 0;

    string::const_iterator I;
    for( I=S.begin(); I!=S.end(); ++I )
    {
        const char& c( *I );
        if ( c == '(' )
        {
            ++q;
        }
        else if ( c== ')' )
        {
            if( q <= 0 )
            {
                return 0;
            }

            --q;
        }
    }

    if ( q > 0 )
        return 0;

    return 1;
}

BOOST_AUTO_TEST_CASE( TestRun )
{
    BOOST_CHECK_EQUAL( 1, nesting( "(()())" ) );
    BOOST_CHECK_EQUAL( 0, nesting( "(()()" ) );
}

int dominator_o1 ( const vector<int> &A )
{
    map<int,int> o;
    for( size_t i=0; i<A.size();++i )
    {
        o[ A[i] ]++;
    }

    struct greatest {
        bool operator() ( const pair<int,int>& i, const pair<int,int>& j)
        {
            return i<j;
        }
    };

    map<int,int>::iterator I =
        max_element( o.begin(), o.end(), greatest() );
    if( I == o.end() )
    {
        return -1;
    }

    int dom_i = (*I).first;
    int dom_count = (*I).second;
    if( dom_count < (int)(A.size()/2) )
        return -1;

    for( size_t i=0; i<A.size();++i )
    {
        if( A[i] == dom_i )
            return i;
    }

    return -1;
}

int dominator_o2 ( const vector<int> &A )
{
    map<int,int> o;
    for( size_t i=0; i<A.size();++i )
    {
        o[ A[i] ]++;
    }

    int dom_i = -1;
    map<int,int>::iterator I;
    for( I = o.begin(); I!= o.end(); ++I )
    {
        int dom_count = (*I).second;
        if( dom_count >= (int)(A.size()/2) )
        {
            dom_i = (*I).first;
            break;
        }
    }

    if( dom_i == -1 )
        return -1;

    for( size_t i=0; i<A.size();++i )
    {
        if( A[i] == dom_i )
            return i;
    }

    return -1;
}

int dominator ( const vector<int> &A )
{
    map<int,pair<int,int>> o;
    map<int,pair<int,int>>::iterator I;
    for( size_t i=0; i<A.size();++i )
    {
        I = o.find( A[i] );
        if( I == o.end() )
        {
            pair<map<int,pair<int,int>>::iterator,bool> P = o.insert( make_pair(A[i],make_pair(0,i)) );
            I = P.first;
        }
        (*I).second.first ++;
    }

    for( I = o.begin(); I!= o.end(); ++I )
    {
        int dom_count = (*I).second.first;
        if( dom_count >= (int)(A.size()/2) )
        {
            return (*I).second.second;
        }
    }

    return -1;
}

BOOST_AUTO_TEST_CASE( TestRun2 )
{
    {
        int a[] = { 1, 2, 3, 3, 3, 3 };
        vector<int> A( a, a + sizeof(a)/sizeof(a[0]) );
        BOOST_CHECK_EQUAL( 2, dominator( A ) );
    }
    {
        BOOST_CHECK_EQUAL( -1, dominator( vector<int>() ) );
    }
}

double weight( int a )
{
    char buf[255];
    int len = sprintf( buf, "%d", a );

    int sum = 0;
    for( int i =0; i<len; ++i )
    {
        sum += (buf[i]-'0');
    }

    return (double)sum / len;
}

int heavy_decimal_count1 ( int A,int B )
{
    int c = 0;
    for( int v=A; v<=B; ++v )
    {
        if( weight(v) > 7.0 )
        {
            c++;
        }
    }

    return c;
}

int weight_sum( int a, int& len, int& last )
{
    char buf[255];
    len = sprintf( buf, "%d", a );

    int sum = 0;
    for( int i=0; i<len; ++i )
    {
        last = (buf[i]-'0');
        sum += last;
    }

    return sum;
}

int heavy_decimal_count ( int A,int B )
{
    int c = 0;
    int len, sum, last;
    int check = 0;
    for( int v=A; v<=B; ++v )
    {
        if( check == 0 )
        {
            sum = weight_sum( v, len, last );
            if( last == 0 )
            {
                check = 9;
            }
        }
        else
        {
            sum ++;
            check--;
        }

        if( (double)sum/len > 7.0 )
        {
            c++;
        }
    }

    return c;
}

BOOST_AUTO_TEST_CASE( TestRun3 )
{
    {
        BOOST_CHECK_EQUAL( 5, heavy_decimal_count( 8675, 8691 ) );
    }
}

int binomial_coeff( int n, int k )
{
    if ( k < 0 || k > n )
        return 0;

    if ( k == 0 ) return 1;

    return binomial_coeff( n-1, k-1 ) + binomial_coeff( n-1,k );
}

class MemoizedBinomial
{
public:
	int Coeff( short n, short k )
	{
		if ( k < 0 || k > n )
			return 0;

		if ( k == 0 ) return 1;

		int key = ( n<<16 | k );
		std::map<int,int>::iterator I = m_Cache.find( key );
		if( I != m_Cache.end() )
		{
			return I->second;
		}

		int value = Coeff( n-1, k-1 ) + Coeff( n-1, k );
		m_Cache[key] = value;
		return value;
	}

private:
	std::map<int,int> m_Cache;

};

int count_decimal_up_to( int d, int n, MemoizedBinomial& binomial )
{
    int mi = -1;
    int sum = 0;
    for( int i=0; i<=d; ++i )
    {
        mi *= -1;
		sum += mi* binomial.Coeff(d,i) * binomial.Coeff( n + d - 10*i, d );
    }
    return sum;
}

BOOST_AUTO_TEST_CASE( TestRun4 )
{
	{
		MemoizedBinomial bm;
		BOOST_CHECK_EQUAL( 10, bm.Coeff( 5, 2 ) );
	}

    {
        BOOST_CHECK_EQUAL( 10, binomial_coeff( 5, 2 ) );
    }

    {
		MemoizedBinomial binomial;

        BOOST_CHECK_EQUAL( 6, count_decimal_up_to( 1, 5, binomial ) );
        BOOST_CHECK_EQUAL( heavy_decimal_count(100,199), 100 - count_decimal_up_to( 2, 21 - 1, binomial ) );
        BOOST_CHECK_EQUAL( heavy_decimal_count(1000,1999), 1000 - count_decimal_up_to( 3, 28 - 1, binomial ) );
    }
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/list.hpp>
#include <boost/hana/detail/minimal/product.hpp>

#include <boost/hana/integral.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto is = [](auto x) {
    return [=](auto y) { return equal(x, y); };
};

// the predicate needs to be compile-time
template <int i>
constexpr auto x = int_<i>;

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto p = detail::minimal::product<>;

    constexpr auto z = x<999>;

    BOOST_HANA_CONSTANT_ASSERT(span(list(), is(z)) == p(list(), list()));

    BOOST_HANA_CONSTANT_ASSERT(span(list(x<0>), is(z)) == p(list(), list(x<0>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(z), is(z)) == p(list(z), list()));

    BOOST_HANA_CONSTANT_ASSERT(span(list(x<0>, z), is(z)) == p(list(), list(x<0>, z)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(z, x<0>), is(z)) == p(list(z), list(x<0>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(x<0>, x<1>), is(z)) == p(list(), list(x<0>, x<1>)));

    BOOST_HANA_CONSTANT_ASSERT(span(list(x<0>, x<1>, x<2>), is(z)) == p(list(), list(x<0>, x<1>, x<2>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(z, x<1>, x<2>), is(z)) == p(list(z), list(x<1>, x<2>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(x<0>, z, x<2>), is(z)) == p(list(), list(x<0>, z, x<2>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(z, z, x<2>), is(z)) == p(list(z, z), list(x<2>)));
    BOOST_HANA_CONSTANT_ASSERT(span(list(z, z, z), is(z)) == p(list(z, z, z), list()));
}

int main() {
    test<List::mcd<void>>();
    (void)is;
}