/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/assign.hpp>
// clang-format off
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
// clang-format on
#include <boost/variant.hpp>

#include <test/capfloormarketdata.hpp>
#include <test/toplevelfixture.hpp>
#include <test/yieldcurvemarketdata.hpp>

#include <ql/instruments/makecapfloor.hpp>
#include <ql/pricingengines/capfloor/bacheliercapfloorengine.hpp>
#include <ql/pricingengines/capfloor/blackcapfloorengine.hpp>
#include <ql/time/date.hpp>
#include <qle/math/flatextrapolation.hpp>
#include <qle/termstructures/capfloorhelper.hpp>
#include <qle/termstructures/capfloortermvolcurve.hpp>
#include <qle/termstructures/piecewiseatmoptionletcurve.hpp>

using namespace QuantExt;
using namespace QuantLib;
using namespace boost::unit_test_framework;

namespace bdata = boost::unit_test::data;
typedef QuantLib::BootstrapHelper<QuantLib::OptionletVolatilityStructure> helper;
using boost::assign::list_of;
using boost::assign::map_list_of;
using std::boolalpha;
using std::fixed;
using std::map;
using std::ostream;
using std::pair;
using std::setprecision;
using std::string;
using std::vector;

namespace {

// Variables to be used in the test
struct CommonVars : public qle::test::TopLevelFixture {

    // Constructor
    CommonVars()
        : referenceDate(5, Feb, 2016), settlementDays(0), calendar(TARGET()), bdc(Following),
          dayCounter(Actual365Fixed()), accuracy(1.0e-12), tolerance(1.0e-10) {

        // Reference date
        Settings::instance().evaluationDate() = referenceDate;

        // Set cap floor ibor index to EUR-EURIBOR-6M and attach a forwarding curve
        iborIndex = boost::make_shared<Euribor6M>(testYieldCurves.forward6M);
    }

    // Valuation date for the test
    Date referenceDate;

    // Variables used in the optionlet volatility structure creation
    Natural settlementDays;
    Calendar calendar;
    BusinessDayConvention bdc;
    DayCounter dayCounter;

    // Accuracy for optionlet stripping
    Real accuracy;

    // Test tolerance for comparing the NPVs
    Real tolerance;

    // Cap floor ibor index
    boost::shared_ptr<IborIndex> iborIndex;

    // EUR discount curve test data from file test/yieldcurvemarketdata.hpp
    YieldCurveEUR testYieldCurves;
};

// Struct to hold a cap floor volatility column and some associated meta data
struct AtmVolData {
    vector<Period> tenors;
    vector<Real> volatilities;
    VolatilityType type;
    Real displacement;
};

// Needed for data driven test case below as it writes out the VolatilityColumn
ostream& operator<<(ostream& os, const AtmVolData& vc) {
    return os << "Atm volatility data with volatility type: " << vc.type << ", shift: " << vc.displacement;
}

// From the EUR cap floor test volatility data in file test/capfloormarketdata.hpp, create a vector of
// AtmVolData which will be the data in the data driven test below
vector<AtmVolData> generateAtmVolData() {

    // Will hold the generated ATM volatility data
    vector<AtmVolData> data;

    // EUR cap floor test volatility data from file test/capfloormarketdata.hpp
    CapFloorVolatilityEUR testVols;

    // All the ATM data has shared tenors
    AtmVolData datum;
    datum.tenors = testVols.atmTenors;

    // The normal ATM volatility data
    datum.volatilities = testVols.nAtmVols;
    datum.type = Normal;
    datum.displacement = 0.0;
    data.push_back(datum);

    // The shifted lognormal volatilities with shift 1
    datum.volatilities = testVols.slnAtmVols_1;
    datum.type = ShiftedLognormal;
    datum.displacement = testVols.shift_1;
    data.push_back(datum);

    // The shifted lognormal volatilities with shift 2
    datum.volatilities = testVols.slnAtmVols_2;
    datum.type = ShiftedLognormal;
    datum.displacement = testVols.shift_2;
    data.push_back(datum);

    return data;
}

// Interpolation types for the data driven test case
typedef boost::variant<Linear, BackwardFlat, QuantExt::LinearFlat, Cubic, QuantExt::CubicFlat> InterpolationType;

// BackwardFlat does not work well with interpolation on term cap floors so this allows us to exclude it
class InterpDataset {
public:
    enum { arity = 1 };

    InterpDataset() {
        samples_.push_back(boost::make_tuple(InterpolationType(Linear()), true));
        samples_.push_back(boost::make_tuple(InterpolationType(QuantExt::LinearFlat()), true));
        samples_.push_back(boost::make_tuple(InterpolationType(Cubic()), true));
        samples_.push_back(boost::make_tuple(InterpolationType(QuantExt::CubicFlat()), true));
        samples_.push_back(boost::make_tuple(InterpolationType(BackwardFlat()), true));
        samples_.push_back(boost::make_tuple(InterpolationType(Linear()), false));
        samples_.push_back(boost::make_tuple(InterpolationType(QuantExt::LinearFlat()), false));
        samples_.push_back(boost::make_tuple(InterpolationType(Cubic()), false));
        samples_.push_back(boost::make_tuple(InterpolationType(QuantExt::CubicFlat()), false));
    }

    bdata::size_t size() const { return samples_.size(); }

    typedef vector<boost::tuple<InterpolationType, bool> >::const_iterator iterator;
    iterator begin() const { return samples_.begin(); }

private:
    vector<boost::tuple<InterpolationType, bool> > samples_;
};

// If the built optionlet structure in the test has a floating or fixed reference date
vector<bool> isMovingValues = list_of(true)(false);

// If the optionlet structure has a flat first period or not
vector<bool> flatFirstPeriodValues = list_of(true)(false);

// So that I can reuse below
string to_string(const InterpolationType& interpolationType) {
    string result;
    switch (interpolationType.which()) {
    case 0:
        result = "Linear";
        break;
    case 1:
        result = "BackwardFlat";
        break;
    case 2:
        result = "LinearFlat";
        break;
    case 3:
        result = "Cubic";
        break;
    case 4:
        result = "CubicFlat";
        break;
    default:
        BOOST_FAIL("Unexpected interpolation type");
    }
    return result;
}

} // namespace

// Needed for BOOST_DATA_TEST_CASE below
// https://stackoverflow.com/a/33965517/1771882
namespace boost {

namespace test_tools {
namespace tt_detail {
template <> struct print_log_value<boost::tuple<InterpolationType, bool> > {
    void operator()(ostream& os, const boost::tuple<InterpolationType, bool>& tp) {
        os << to_string(boost::get<0>(tp)) << ", " << boolalpha << boost::get<1>(tp);
    }
};
} // namespace tt_detail
} // namespace test_tools

namespace unit_test {
namespace data {
namespace monomorphic {
// Registering InterpDataset as a dataset
template <> struct is_dataset<InterpDataset> : boost::mpl::true_ {};
} // namespace monomorphic
} // namespace data
} // namespace unit_test

} // namespace boost

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(PiecewiseAtmOptionletCurveTests)

BOOST_DATA_TEST_CASE_F(CommonVars, testPiecewiseAtmOptionletStripping,
                       bdata::make(generateAtmVolData()) * InterpDataset() * bdata::make(isMovingValues) *
                           bdata::make(flatFirstPeriodValues),
                       atmVolData, interpSample, isMoving, flatFirstPeriod) {

    BOOST_TEST_MESSAGE("Testing piecewise optionlet stripping of ATM cap floor curve");

    InterpolationType interpolationType = boost::get<0>(interpSample);
    bool interpOnOptionlets = boost::get<1>(interpSample);

    BOOST_TEST_MESSAGE("Test inputs are:");
    BOOST_TEST_MESSAGE("  Quote volatility type: " << atmVolData.type);
    BOOST_TEST_MESSAGE("  Quote displacement: " << atmVolData.displacement);
    BOOST_TEST_MESSAGE("  Interpolation type: " << to_string(interpolationType));
    BOOST_TEST_MESSAGE("  Interp on optionlets: " << boolalpha << interpOnOptionlets);
    BOOST_TEST_MESSAGE("  Floating reference date: " << boolalpha << isMoving);
    BOOST_TEST_MESSAGE("  Flat first period: " << boolalpha << flatFirstPeriod);

    // Store each cap floor instrument in the strike column and its NPV using the flat cap floor volatilities
    vector<boost::shared_ptr<CapFloor> > instruments(atmVolData.tenors.size());
    vector<Real> flatNpvs(atmVolData.tenors.size());

    // Store the ATM volatility quotes and handles
    vector<boost::shared_ptr<SimpleQuote> > volQuotes(atmVolData.tenors.size());
    vector<Handle<Quote> > volHandles(atmVolData.tenors.size());

    BOOST_TEST_MESSAGE("The input values at each tenor are:");
    for (Size i = 0; i < atmVolData.tenors.size(); i++) {

        // Get the relevant quote value
        Real volatility = atmVolData.volatilities[i];
        volQuotes[i] = boost::make_shared<SimpleQuote>(volatility);
        volHandles[i] = Handle<Quote>(volQuotes[i]);

        // Create the ATM cap instrument and store its price using the quoted flat volatility
        instruments[i] = MakeCapFloor(CapFloor::Cap, atmVolData.tenors[i], iborIndex, 0.01);
        Rate atm = instruments[i]->atmRate(**testYieldCurves.discountEonia);
        instruments[i] = MakeCapFloor(CapFloor::Cap, atmVolData.tenors[i], iborIndex, atm);
        if (atmVolData.type == ShiftedLognormal) {
            instruments[i]->setPricingEngine(boost::make_shared<BlackCapFloorEngine>(
                testYieldCurves.discountEonia, volatility, dayCounter, atmVolData.displacement));
        } else {
            instruments[i]->setPricingEngine(
                boost::make_shared<BachelierCapFloorEngine>(testYieldCurves.discountEonia, volatility, dayCounter));
        }
        flatNpvs[i] = instruments[i]->NPV();

        BOOST_TEST_MESSAGE("  (Cap/Floor, Tenor, Strike, Volatility, Flat NPV) = (Cap, "
                           << atmVolData.tenors[i] << ", " << atm << ", " << fixed << setprecision(13) << volatility
                           << ", " << flatNpvs[i] << ")");
    }

    // Create the ATM optionlet curve, with the given interpolation type.
    // Doesn't matter what interpolation we use here for the cap floor term surface so just use the same
    // interpolation as the optionlet curve being stripped
    VolatilityType curveVolatilityType = Normal;
    Real curveDisplacement = 0.0;
    boost::shared_ptr<CapFloorTermVolCurve> cftvc;
    boost::shared_ptr<OptionletVolatilityStructure> ovCurve;
    switch (interpolationType.which()) {
    case 0:
        if (isMoving) {
            BOOST_TEST_MESSAGE("Using Linear interpolation with a moving reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<Linear> >(
                settlementDays, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<Linear> >(
                settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        } else {
            BOOST_TEST_MESSAGE("Using Linear interpolation with a fixed reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<Linear> >(
                referenceDate, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<Linear> >(
                referenceDate, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        }
        break;
    case 1:
        if (isMoving) {
            BOOST_TEST_MESSAGE("Using BackwardFlat interpolation with a moving reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<BackwardFlat> >(
                settlementDays, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<BackwardFlat> >(
                settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        } else {
            BOOST_TEST_MESSAGE("Using BackwardFlat interpolation with a fixed reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<BackwardFlat> >(
                referenceDate, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<BackwardFlat> >(
                referenceDate, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        }
        break;
    case 2:
        if (isMoving) {
            BOOST_TEST_MESSAGE("Using LinearFlat interpolation with a moving reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<LinearFlat> >(
                settlementDays, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<LinearFlat> >(
                settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        } else {
            BOOST_TEST_MESSAGE("Using LinearFlat interpolation with a fixed reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<LinearFlat> >(
                referenceDate, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<LinearFlat> >(
                referenceDate, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        }
        break;
    case 3:
        if (isMoving) {
            BOOST_TEST_MESSAGE("Using Cubic interpolation with a moving reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<Cubic> >(
                settlementDays, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<Cubic> >(
                settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        } else {
            BOOST_TEST_MESSAGE("Using Cubic interpolation with a fixed reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<Cubic> >(
                referenceDate, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<Cubic> >(
                referenceDate, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        }
        break;
    case 4:
        if (isMoving) {
            BOOST_TEST_MESSAGE("Using CubicFlat interpolation with a moving reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<CubicFlat> >(
                settlementDays, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<CubicFlat> >(
                settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        } else {
            BOOST_TEST_MESSAGE("Using CubicFlat interpolation with a fixed reference date");
            cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<CubicFlat> >(
                referenceDate, calendar, bdc, atmVolData.tenors, volHandles, dayCounter, flatFirstPeriod);
            ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<CubicFlat> >(
                referenceDate, cftvc, iborIndex, testYieldCurves.discountEonia, flatFirstPeriod, atmVolData.type,
                atmVolData.displacement, curveVolatilityType, curveDisplacement, interpOnOptionlets);
        }
        break;
    default:
        BOOST_FAIL("Unexpected interpolation type");
    }
    Handle<OptionletVolatilityStructure> hovs(ovCurve);

    // Price each cap floor instrument using the piecewise optionlet curve and check it against the flat NPV
    BOOST_TEST_MESSAGE("The stripped values and differences at each tenor are:");
    Real strippedNpv;
    for (Size i = 0; i < atmVolData.tenors.size(); i++) {

        // Price the instrument using the stripped optionlet structure
        if (ovCurve->volatilityType() == ShiftedLognormal) {
            instruments[i]->setPricingEngine(
                boost::make_shared<BlackCapFloorEngine>(testYieldCurves.discountEonia, hovs));
        } else {
            instruments[i]->setPricingEngine(
                boost::make_shared<BachelierCapFloorEngine>(testYieldCurves.discountEonia, hovs));
        }
        strippedNpv = instruments[i]->NPV();

        BOOST_TEST_MESSAGE("  (Cap/Floor, Tenor, Strike, Volatility, Flat NPV, Stripped NPV, Flat - Stripped) = ("
                           << instruments[i]->type() << ", " << atmVolData.tenors[i] << ", "
                           << instruments[i]->capRates()[0] << ", " << fixed << setprecision(13)
                           << atmVolData.volatilities[i] << ", " << flatNpvs[i] << ", " << strippedNpv << ", "
                           << (flatNpvs[i] - strippedNpv) << ")");

        BOOST_CHECK_SMALL(fabs(flatNpvs[i] - strippedNpv), tolerance);
    }

    // Pick 10Y (arbitrary choice - 5th element) ATM vol, bump it and ensure stripping still works.
    BOOST_TEST_MESSAGE("Testing that stripping still works after bumping volatility quote");
    Size idx = 4;
    Volatility bumpedVol = volQuotes[idx]->value() * 1.10;
    volQuotes[idx]->setValue(bumpedVol);
    strippedNpv = instruments[idx]->NPV();
    if (atmVolData.type == ShiftedLognormal) {
        instruments[idx]->setPricingEngine(boost::make_shared<BlackCapFloorEngine>(
            testYieldCurves.discountEonia, bumpedVol, dayCounter, atmVolData.displacement));
    } else {
        instruments[idx]->setPricingEngine(
            boost::make_shared<BachelierCapFloorEngine>(testYieldCurves.discountEonia, bumpedVol, dayCounter));
    }
    Real newFlatNpv = instruments[idx]->NPV();
    BOOST_TEST_MESSAGE("  (Cap/Floor, Tenor, Strike, Volatility, Flat NPV, Stripped NPV, Flat - Stripped) = ("
                       << instruments[idx]->type() << ", " << atmVolData.tenors[idx] << ", "
                       << instruments[idx]->capRates()[0] << ", " << fixed << setprecision(13) << bumpedVol << ", "
                       << newFlatNpv << ", " << strippedNpv << ", " << (newFlatNpv - strippedNpv) << ")");
    BOOST_CHECK_GT(newFlatNpv, flatNpvs[idx]);
    BOOST_CHECK_SMALL(fabs(newFlatNpv - strippedNpv), tolerance);

    BOOST_TEST_MESSAGE("Test extrapolation settings with out of range date");
    Date oorDate = ovCurve->maxDate() + 1 * Months;
    BOOST_CHECK_NO_THROW(ovCurve->volatility(oorDate, 0.01, true));
    BOOST_CHECK_THROW(ovCurve->volatility(oorDate, 0.01), Error);
    ovCurve->enableExtrapolation();
    BOOST_CHECK_NO_THROW(ovCurve->volatility(oorDate, 0.01));

    BOOST_TEST_MESSAGE("Test term structure stripping still works after changing evaluation date");
    Date newDate = calendar.advance(referenceDate, 1 * Months);
    Settings::instance().evaluationDate() = newDate;

    for (Size i = 0; i < atmVolData.tenors.size(); i++) {

        Real volatility = volQuotes[i]->value();

        // Cap floor set up is different depending on whether we are testing the moving term structure or not.
        // Empty startDate, i.e. moving, means that cap floor will be relative to the global evaluation date.
        // If not moving, we keep the instrment's original start date.
        Date startDate;
        if (!isMoving) {
            startDate = iborIndex->fixingCalendar().adjust(referenceDate);
            startDate = iborIndex->fixingCalendar().advance(startDate, iborIndex->fixingDays() * Days);
        }
        instruments[i] =
            MakeCapFloor(CapFloor::Cap, atmVolData.tenors[i], iborIndex, 0.01).withEffectiveDate(startDate, true);
        Rate atm = instruments[i]->atmRate(**testYieldCurves.discountEonia);
        instruments[i] =
            MakeCapFloor(CapFloor::Cap, atmVolData.tenors[i], iborIndex, atm).withEffectiveDate(startDate, true);

        if (atmVolData.type == ShiftedLognormal) {
            instruments[i]->setPricingEngine(boost::make_shared<BlackCapFloorEngine>(
                testYieldCurves.discountEonia, volatility, dayCounter, atmVolData.displacement));
        } else {
            instruments[i]->setPricingEngine(
                boost::make_shared<BachelierCapFloorEngine>(testYieldCurves.discountEonia, volatility, dayCounter));
        }
        newFlatNpv = instruments[i]->NPV();

        // Price the instrument using the stripped optionlet structure
        if (ovCurve->volatilityType() == ShiftedLognormal) {
            instruments[i]->setPricingEngine(
                boost::make_shared<BlackCapFloorEngine>(testYieldCurves.discountEonia, hovs));
        } else {
            instruments[i]->setPricingEngine(
                boost::make_shared<BachelierCapFloorEngine>(testYieldCurves.discountEonia, hovs));
        }
        strippedNpv = instruments[i]->NPV();

        BOOST_TEST_MESSAGE("  (Cap/Floor, Tenor, Strike, Volatility, Flat NPV, Stripped NPV, Flat - Stripped) = ("
                           << instruments[i]->type() << ", " << atmVolData.tenors[i] << ", "
                           << instruments[i]->capRates()[0] << ", " << fixed << setprecision(13)
                           << volQuotes[i]->value() << ", " << newFlatNpv << ", " << strippedNpv << ", "
                           << (newFlatNpv - strippedNpv) << ")");

        BOOST_CHECK_SMALL(fabs(newFlatNpv - strippedNpv), tolerance);
    }
}

BOOST_FIXTURE_TEST_CASE(testAtmStrippingExceptions, CommonVars) {

    BOOST_TEST_MESSAGE("Testing ATM stripping exception behaviour");

    // Use the normal ATM volatility test data
    CapFloorVolatilityEUR testVols;
    vector<Period> tenors = testVols.atmTenors;
    vector<Real> volatilities = testVols.nAtmVols;
    VolatilityType type = Normal;
    Real displacement = 0.0;

    // Store the ATM volatility quotes and handles
    vector<boost::shared_ptr<SimpleQuote> > volQuotes(tenors.size());
    vector<Handle<Quote> > volHandles(tenors.size());
    for (Size i = 0; i < tenors.size(); i++) {
        Real volatility = volatilities[i];
        volQuotes[i] = boost::make_shared<SimpleQuote>(volatility);
        volHandles[i] = Handle<Quote>(volQuotes[i]);
    }

    // Get configuration values for bootstrap
    Real globalAccuracy = 1e-10;
    bool dontThrow = false;

    // Cap floor term curve
    boost::shared_ptr<CapFloorTermVolCurve> cftvc = boost::make_shared<InterpolatedCapFloorTermVolCurve<LinearFlat> >(
        settlementDays, calendar, bdc, tenors, volHandles, dayCounter, true);

    // Piecewise curve
    VolatilityType curveVolatilityType = Normal;
    Real curveDisplacement = 0.0;
    bool interpOnOptionlets = true;
    boost::shared_ptr<OptionletVolatilityStructure> ovCurve =
        boost::make_shared<PiecewiseAtmOptionletCurve<LinearFlat> >(
            settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, true, type, displacement,
            curveVolatilityType, curveDisplacement, interpOnOptionlets, LinearFlat(),
            QuantExt::IterativeBootstrap<
                PiecewiseAtmOptionletCurve<LinearFlat, QuantExt::IterativeBootstrap>::optionlet_curve>(
                accuracy, globalAccuracy, dontThrow));

    // Checks
    Period oneYear = 1 * Years;
    Period fiveYears = 5 * Years;
    Period eightYears = 8 * Years;
    Volatility oneYearVol;
    Volatility fiveYearVol;
    Volatility eightYearVol;
    BOOST_CHECK_NO_THROW(oneYearVol = ovCurve->volatility(oneYear, 0.01, true));
    BOOST_TEST_MESSAGE("1Y vol: " << oneYearVol);
    BOOST_CHECK_NO_THROW(fiveYearVol = ovCurve->volatility(fiveYears, 0.01, true));
    BOOST_TEST_MESSAGE("5Y vol: " << fiveYearVol);
    BOOST_CHECK_NO_THROW(eightYearVol = ovCurve->volatility(eightYears, 0.01, true));
    BOOST_TEST_MESSAGE("8Y vol: " << eightYearVol);

    // Increase the 5Y volatility to introduce an exception. Can't bootstrap the 7Y and 10Y caps as a result
    volQuotes[2]->setValue(2 * volQuotes[2]->value());
    BOOST_CHECK_THROW(ovCurve->volatility(fiveYears, 0.01, true), Error);

    // Rebuild ovCurve with dontThrow set to true
    dontThrow = true;
    ovCurve = boost::make_shared<PiecewiseAtmOptionletCurve<LinearFlat> >(
        settlementDays, cftvc, iborIndex, testYieldCurves.discountEonia, true, type, displacement, curveVolatilityType,
        curveDisplacement, interpOnOptionlets, LinearFlat(),
        QuantExt::IterativeBootstrap<
            PiecewiseAtmOptionletCurve<LinearFlat, QuantExt::IterativeBootstrap>::optionlet_curve>(
            accuracy, globalAccuracy, dontThrow));

    // Check bootstrap passes and check the values.
    // - the 1Y optionlet volatility should not have been affected
    // - the 5Y optionlet volatility should have increased
    Volatility testVol = 0.0;
    BOOST_CHECK_NO_THROW(testVol = ovCurve->volatility(oneYear, 0.01, true));
    BOOST_CHECK_SMALL(fabs(testVol - oneYearVol), tolerance);
    BOOST_TEST_MESSAGE("1Y vol after bump using previous: " << testVol);
    BOOST_CHECK_NO_THROW(testVol = ovCurve->volatility(fiveYears, 0.01, true));
    BOOST_CHECK_GT(testVol, fiveYearVol);
    BOOST_TEST_MESSAGE("5Y vol after bump using previous: " << testVol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

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

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

#ifndef BOOST_GEOMETRY_STRATEGY_AREA_RESULT_HPP
#define BOOST_GEOMETRY_STRATEGY_AREA_RESULT_HPP

#include <type_traits>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/strategies/area/services.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategy/area.hpp>

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/sequence.hpp>
#include <boost/geometry/util/type_traits.hpp>

#include <boost/variant/variant_fwd.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace area
{

template
<
    typename Geometry,
    typename Strategy,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategy>::value
>
struct area_result
{
    typedef decltype(std::declval<Strategy>().area(std::declval<Geometry>())) strategy_type;
    typedef typename strategy_type::template result_type<Geometry>::type type;
};

template <typename Geometry, typename Strategy>
struct area_result<Geometry, Strategy, false>
{
    typedef typename Strategy::template result_type<Geometry>::type type;
};

template
<
    typename Geometry,
    bool IsGeometry = util::is_geometry<Geometry>::value
>
struct default_area_result
    : area_result
        <
            Geometry,
            typename geometry::strategies::area::services::default_strategy
                <
                    Geometry
                >::type
        >
{};

// Workaround for VS2015
#if defined(_MSC_VER) && (_MSC_VER < 1910)
template
<
    typename Geometry,
    bool IsGeometry = util::is_geometry<Geometry>::value
>
struct coordinate_type
    : geometry::coordinate_type<Geometry>
{};
template <typename Geometry>
struct coordinate_type<Geometry, false>
{
    typedef int type;
};
template <typename Geometry>
struct default_area_result<Geometry, false>
{
    typedef int type;
};
#endif

template <typename Curr, typename Next>
struct more_precise_coordinate_type
    : std::is_same
        <
            typename coordinate_type<Curr>::type,
            typename geometry::select_most_precise
                <
                    typename coordinate_type<Curr>::type,
                    typename coordinate_type<Next>::type
                >::type
        >
{};

template <typename Curr, typename Next>
struct more_precise_default_area_result
    : std::is_same
        <
            typename default_area_result<Curr>::type,
            typename geometry::select_most_precise
                <
                    typename default_area_result<Curr>::type,
                    typename default_area_result<Next>::type
                >::type
        >
{};

}} // namespace detail::area
#endif //DOXYGEN_NO_DETAIL

/*!
\brief Meta-function defining return type of area function
\ingroup area
\note The return-type is defined by Geometry and Strategy
 */
template
<
    typename Geometry,
    typename Strategy = default_strategy
>
struct area_result
    : detail::area::area_result<Geometry, Strategy>
{};

template <BOOST_VARIANT_ENUM_PARAMS(typename T), typename Strategy>
struct area_result<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>, Strategy>
    : geometry::area_result
        <
            typename util::select_pack_element
                <
                    detail::area::more_precise_coordinate_type,
                    BOOST_VARIANT_ENUM_PARAMS(T)
                >::type,
            Strategy
        >
{};

template <typename Geometry>
struct area_result<Geometry, default_strategy>
    : detail::area::default_area_result<Geometry>
{};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct area_result<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>, default_strategy>
    : detail::area::default_area_result
        <
            typename util::select_pack_element
                <
                    detail::area::more_precise_default_area_result,
                    BOOST_VARIANT_ENUM_PARAMS(T)
                >::type
        >
{};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_AREA_RESULT_HPP


#include "catch.hpp"

#include "mp/following_detection.hpp"
#include "mp/following_graph.hpp"

#include <boost/functional/hash.hpp>
#include <cereal/archives/json.hpp>

#include <unordered_set>

using namespace mp;
using namespace std;

unordered_set<string> get_name_set(const following_graph &g)
{
    unordered_set<string> names;
    for (auto vertex : make_iter_range(vertices(g))) {
        names.insert(g[vertex].name);
    }
    return names;
}

// Must use boost::hash since STL does not give a hash impl for tuple.
using edge_set_type = unordered_set<tuple<string, string>, boost::hash<tuple<string, string>>>;

edge_set_type get_edge_set(const following_graph &g)
{
    edge_set_type set;
    for (auto edge : make_iter_range(edges(g))) {
        auto s = source(edge, g);
        auto t = target(edge, g);
        set.insert(make_tuple(g[s].name, g[t].name));
    }
    return set;
}

TEST_CASE("graph construction", "[following-graph]")
{
    following_data data;
    data.devices = {"A", "B", "C"};
    data.begin_timestamp = 0;
    data.end_timestamp = 0;
    data.duration = 0;
    data.timestamps = {
        {
            // Timestamp 0
            0,
            {
                {0, 1, -5, following_type::following},
                {2, 0,  2, following_type::leading},
            }
        }
    };

    // Out of range:
    REQUIRE_THROWS_AS(following_graph_at(data, -1), std::logic_error);
    REQUIRE_THROWS_AS(following_graph_at(data,  1), std::logic_error);

    following_graph g = following_graph_at(data, 0);
    auto names = get_name_set(g);
    auto expected_names = unordered_set<string>{"A", "B", "C"};

    auto edges = get_edge_set(g);
    auto expected_edges = edge_set_type{make_tuple("A", "B"), make_tuple("A", "C")};

    REQUIRE(names == expected_names);
    REQUIRE(edges == expected_edges);
}

TEST_CASE("graph serialization", "[following-graph]")
{
    following_graph g;
    {
        auto a = add_vertex({"A"}, g);
        auto b = add_vertex({"B"}, g);
        auto c = add_vertex({"C"}, g);

        add_edge(a, b, {}, g);
        add_edge(c, b, {}, g);
        add_edge(a, c, {}, g);
    }

    std::stringstream s;

    // Serialize
    {
        cereal::JSONOutputArchive ar(s);
        ar(cereal::make_nvp("graph", g));
    }

    // Deserialize
    following_graph test;
    {
        cereal::JSONInputArchive ar(s);
        ar(cereal::make_nvp("graph", test));
    }

    REQUIRE(get_name_set(test) == get_name_set(g));
    REQUIRE(get_edge_set(test) == get_edge_set(g));
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "ConversionUtilities.h"

#include <assert.h>

#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

#pragma warning(push, 0)
#include <igl/vertex_triangle_adjacency.h>
#pragma warning(pop)

#include <Urho3D/Core/Variant.h>
#include <Urho3D/Container/Vector.h>

#pragma warning(disable : 4244)

using Urho3D::Variant;
using Urho3D::VariantMap;
using Urho3D::VariantType;
using Urho3D::VariantVector;
using Urho3D::Vector3;

// Extract data stored in IGL matrices V, F into VariantVector

VariantVector IglExtractVertices(
	const Eigen::MatrixXf& V
)
{
	VariantVector earlyRet;

	if (V.rows() == 0) {
		std::cout << "ERROR: IglExtractVertices --- V.rows() == 0" << std::endl;
		return earlyRet;
	}
	if (V.cols() != 3) {
		int vcols = V.cols();
		std::cout << "ERROR: IglExtractVertices --- V.cols() != 3" << std::endl;
		return earlyRet;
	}

	VariantVector vertices;

	for (unsigned i = 0; i < V.rows(); ++i) {
		Vector3 vert(V(i, 0), V(i, 1), V(i, 2));
		vertices.Push(Variant(vert));
	}

	return vertices;
}

VariantVector IglExtractFaces(
	const Eigen::MatrixXi& F
)
{
	VariantVector faces;

	for (unsigned i = 0; i < F.rows(); ++i) {
		faces.Push(Variant(F(i, 0)));
		faces.Push(Variant(F(i, 1)));
		faces.Push(Variant(F(i, 2)));
	}

	return faces;
}

VariantVector IglExtractFaces(
	const Eigen::MatrixXi& F,
	unsigned numVertices
)
{
	VariantVector earlyRet;

	if (F.rows() == 0) {
		std::cout << "ERROR: IglExtractFaces --- F.rows() == 0" << std::endl;
		return earlyRet;
	}

	if (F.cols() != 3) {
		std::cout << "ERROR: IglExtractFaces --- F.rows() != 3" << std::endl;
		return earlyRet;
	}

	VariantVector faces;

	for (unsigned i = 0; i < F.rows(); ++i) {
		for (unsigned j = 0; j < 3; ++j) {
			int vertIndex = F(i, j);
			if (vertIndex < 0 || vertIndex > numVertices - 1) {
				std::cout << "ERROR: IglExtractFaces --- vertIndex out of range" << std::endl;
				return earlyRet;
			}
			faces.Push(Variant(F(i, j)));
		}
	}

	return faces;
}

Eigen::MatrixXd IglFloatToDouble(
	const Eigen::MatrixXf& V
)
{
	Eigen::MatrixXd Vd(V.rows(), V.cols());
	for (unsigned i = 0; i < V.rows(); ++i) {
		Vd(i, 0) = double(V(i, 0));
		Vd(i, 1) = double(V(i, 1));
		Vd(i, 2) = double(V(i, 2));
	}

	return Vd;
}

Eigen::MatrixXf IglDoubleToFloat(
	const Eigen::MatrixXd& V
)
{
	Eigen::MatrixXf Vf(V.rows(), V.cols());
	for (unsigned i = 0; i < V.rows(); ++i) {
		Vf(i, 0) = (float)V(i, 0);
		Vf(i, 1) = (float)V(i, 1);
		Vf(i, 2) = (float)V(i, 2);
	}

	return Vf;
}

VariantVector IglComputeFaceNormals(
	const Eigen::MatrixXf& V,
	const Eigen::MatrixXi& F
)
{
	VariantVector normals;

	for (unsigned i = 0; i < F.rows(); ++i) {
		Eigen::RowVector3f u = V.row(F(i, 1)) - V.row(F(i, 0));
		Eigen::RowVector3f v = V.row(F(i, 2)) - V.row(F(i, 0));

		//Eigen::RowVector3f n = u.cross(v);
		Eigen::RowVector3f n = u.cross(v);
		// does Urho expect this normalized?

		Vector3 normal(n(0), n(1), n(2));

		normals.Push(Variant(normal));
	}

	return normals;
}

VariantVector IglComputeVertexNormals(
	const Eigen::MatrixXf& V,
	const Eigen::MatrixXi& F
)
{
	VariantVector vertexNormals;
	VariantVector faceNormals = IglComputeFaceNormals(V, F);
	assert(faceNormals.Size() == F.rows());

	std::vector<std::vector<int> > VF;
	std::vector<std::vector<int> > VFi;

	igl::vertex_triangle_adjacency(V, F, VF, VFi);

	// for each vertex index i, compute the normal there
	// VF.size() == V.rows()
	for (unsigned i = 0; i < VF.size(); ++i) {

		// normal associated to vertex i
		Vector3 normal(0.0f, 0.0f, 0.0f);

		// normal will be the straight average of the normals of faces adjacent to vertex
		for (unsigned j = 0; j < VF[i].size(); ++j) {
			int adj = VF[i][j];
			normal = normal + faceNormals[adj].GetVector3();
		}
		if (VF[i].size() > 0) {
			normal = (1.0f / VF[i].size()) * normal;
		}
		vertexNormals.Push(normal);
	}

	return vertexNormals;
}

Urho3D::Variant IglMatricesToMesh(
	const Eigen::MatrixXf& V,
	const Eigen::MatrixXi& F
)
{
	Variant earlyRet;

	VariantVector outVertices;
	outVertices = IglExtractVertices(V);
	if (outVertices.Size() == 0) {
		std::cout << "ERROR: IglMatricesToMesh --- outVertices.Size() == 0" << std::endl;
		return earlyRet;
	}

	unsigned numVertices = V.rows();
	VariantVector outFaces = IglExtractFaces(F, numVertices);
	if (outFaces.Size() == 0) {
		std::cout << "ERROR: IglMatricesToMesh --- outFaces.Size() == 0" << std::endl;
		return earlyRet;
	}

	VariantMap meshMap;
	meshMap["vertices"] = outVertices;
	meshMap["faces"] = outFaces;

	Variant outMesh(meshMap);

	return outMesh;
}

bool IglMeshToMatrices(
	const Variant& mesh,
	Eigen::MatrixXf& V,
	Eigen::MatrixXi& F
)
{
	VariantMap meshMap = mesh.GetVariantMap();
	if (meshMap.Empty()) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- mesh.GetVariantMap() returns empty" << std::endl;
		return false;
	}

	// meshMap is not empty
	Variant vertices = meshMap["vertices"];
	if (vertices.GetType() != VariantType::VAR_VARIANTVECTOR) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- vertices.GetType() != VAR_VARIANTVECTOR" << std::endl;
		return false;
	}
	Variant faces = meshMap["faces"];
	if (faces.GetType() != VariantType::VAR_VARIANTVECTOR) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- faces.GetType() != VAR_VARIANTVECTOR" << std::endl;
		return false;
	}

	VariantVector vertexList = vertices.GetVariantVector();
	if (vertexList.Size() == 0) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- vertexList.Size() == 0" << std::endl;
		return false;
	}
	unsigned numVertices = vertexList.Size();
	Eigen::MatrixXf W(numVertices, 3);
	for (unsigned i = 0; i < numVertices; ++i) {
		Variant vertexVariant = vertexList[i];
		if (vertexVariant.GetType() != VariantType::VAR_VECTOR3) {
			// V, F untouched
			std::cout << "ERROR: IglMeshToMatrices --- vertexVariant.GetType() != VAR_VECTOR3, index=" << i << std::endl;
			return false;
		}
		Vector3 vert = vertexVariant.GetVector3();
		W.row(i) = Eigen::RowVector3f(vert.x_, vert.y_, vert.z_);
	}
	// vertex information is good; if face information checks out, W is ready to be copied to V

	VariantVector faceList = faces.GetVariantVector();
	if (faceList.Size() == 0) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- faceList.Size() == 0" << std::endl;
		return false;
	}
	if (faceList.Size() % 3 != 0) {
		// V, F untouched
		std::cout << "ERROR: IglMeshToMatrices --- faceList.Size() % 3 != 0" << std::endl;
		return false;
	}
	unsigned numFaces = faceList.Size() / 3;
	Eigen::MatrixXi G(numFaces, 3);
	for (unsigned i = 0; i < numFaces; ++i) {
		for (unsigned j = 0; j < 3; ++j) {
			Variant faceIndexVar = faceList[3 * i + j];
			if (faceIndexVar.GetType() != VariantType::VAR_INT) {
				// V, F untouched
				std::cout << "ERROR: IglMeshMatrices -- faceIndexVar.GetType() != VAR_INT, index=" << 3 * i + j << std::endl;
				return false;
			}
			int faceIndex = faceIndexVar.GetInt();
			if (faceIndex < 0 || faceIndex > numVertices - 1) {
				// V, F untouched
				std::cout << "ERROR: IglMeshMatrices -- faceIndex out of range" << std::endl;
				return false;
			}
			G(i, j) = faceIndex;
		}
	}
	// G is ready

	/*
	Potential crash causing data NOT checked for:
	  -- face with repeated vertex, i.e., 0 0 0 or 0 1 1
	  -- bad mesh GEOMETRY, i.e., self-intersections, non-manifold
	*/
	V.setZero(W.rows(), 3);
	V = W;
	F.setZero(G.rows(), 3);
	F = G;

	return true;
}

// If data stored in mesh passes basic tests then this function essentially does
//   vertexList = mesh.GetVariantMap()["vertices"].GetVariantVector();
//   faceList = mesh.GetVariantMap()["faces"].GetVariantVector();
// and returns true;
// Otherwise Clears() vertexList and faceList and returns false.
// Basic tests are
//   (1) non-zero vertex count
//   (2) non-zero face count; consistent face count, i.e., list length divisible by 3
//   (3) all high level VariantType values are correct, that is,
//       mesh stores a VariantMap with the correct keys, etc.,
//       BUT there is NO check that
//         vertexList[0].GetType() == VAR_VECTOR3
//         faceList[0].GetType() == VAR_INT
//       etc.
bool ExtractMeshData(
	const Urho3D::Variant& mesh,
	Urho3D::VariantVector& vertexList,
	Urho3D::VariantVector& faceList
)
{
	// extract the map
	if (mesh.GetType() != VariantType::VAR_VARIANTMAP) {
		std::cerr << "ERROR: ExtractMeshData --- mesh.GetType() != VAR_VARIANTMAP\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	VariantMap meshMap = mesh.GetVariantMap();

	// extract vertexList
	Variant vertices = meshMap["vertices"];
	if (vertices.GetType() != VariantType::VAR_VARIANTVECTOR) {
		std::cerr << "ERROR: ExtractMeshData --- vertices.GetType() != VAR_VARIANTVECTOR\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	vertexList.Clear();
	vertexList = vertices.GetVariantVector();
	if (vertexList.Size() == 0) {
		std::cerr << "ERROR: ExtractMeshData --- copyVertexList.Size() == 0\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	for (unsigned i = 0; i < vertexList.Size(); ++i) {
		if (vertexList[i].GetType() != VariantType::VAR_VECTOR3) {
			std::cerr << "ERROR: ExtractMeshData --- vertexList[i].GetType() != VAR_VECTOR3, i=" << i << "\n";
			vertexList.Clear();
			faceList.Clear();
			return false;
		}
	}

	// extract faceList
	Variant faces = meshMap["faces"];
	if (faces.GetType() != VariantType::VAR_VARIANTVECTOR) {
		std::cerr << "ERROR: ExtractMeshData --- faces.GetType() != VAR_VARIANTVECTOR\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	faceList.Clear();
	faceList = faces.GetVariantVector();
	if (faceList.Size() == 0) {
		std::cerr << "ERROR: ExtractMeshData --- faceList.Size() == 0\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	if (faceList.Size() % 3 != 0) {
		std::cerr << "ERROR: ExtractMeshData --- faceList.Size() % 3 != 0\n";
		vertexList.Clear();
		faceList.Clear();
		return false;
	}
	for (unsigned i = 0; i < faceList.Size(); ++i) {
		if (faceList[i].GetType() != VariantType::VAR_INT) {
			std::cerr << "ERROR: ExtractMeshData --- faceList[i].GetType() != VAR_INT, i=" << i << "\n";
			vertexList.Clear();
			faceList.Clear();
			return false;
		}
	}
	unsigned numVertices = vertexList.Size();
	for (unsigned i = 0; i < faceList.Size(); i += 3) {
		unsigned i0 = (unsigned)faceList[i].GetInt();
		unsigned i1 = (unsigned)faceList[i + 1].GetInt();
		unsigned i2 = (unsigned)faceList[i + 2].GetInt();

		if (i0 == i1 || i1 == i2 || i2 == i0) {
			std::cerr << "ERROR: ExtractMeshData --- repeated vertex indices in face\n";
			vertexList.Clear();
			faceList.Clear();
			return false;
		}

		if (
			(i0 < 0 || i0 > numVertices - 1) ||
			(i1 < 0 || i1 > numVertices - 1) ||
			(i2 < 0 || i2 > numVertices - 1)
			)
		{
			std::cerr << "ERROR: ExtractMeshData --- vertex indices out of range\n";
			vertexList.Clear();
			faceList.Clear();
			return false;
		}
	}

	return true;
}

void Matrix3x4ToEigen(const Urho3D::Matrix3x4& transform, Eigen::MatrixXf& rotation, Eigen::RowVector3f& translation)
{
	Eigen::MatrixXf eigenRotation;
	Eigen::RowVector3f eigenTranslation;

	eigenRotation.setZero(3, 3);

	eigenRotation(0, 0) = transform.m00_;
	eigenRotation(0, 1) = transform.m01_;
	eigenRotation(0, 2) = transform.m02_;
	eigenRotation(1, 0) = transform.m10_;
	eigenRotation(1, 1) = transform.m11_;
	eigenRotation(1, 2) = transform.m12_;
	eigenRotation(2, 0) = transform.m20_;
	eigenRotation(2, 1) = transform.m21_;
	eigenRotation(2, 2) = transform.m22_;

	eigenTranslation(0) = transform.m03_;
	eigenTranslation(1) = transform.m13_;
	eigenTranslation(2) = transform.m23_;

	rotation = eigenRotation;
	translation = eigenTranslation;

}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

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

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

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

/*
 *  IgbFiletoMatrix_Plugin.cc
 *
 *  Written by:
 *   Karli Gillette
 *   Department of Bioengineering
 *   University of Utah
 *
 */

#include <Core/IEPlugin/IgbFileToMatrix_Plugin.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Utils/Legacy/StringUtil.h>
#include <Core/Logging/LoggerInterface.h>

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

#include <iostream>
#include <fstream>
#include <sstream>

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Datatypes;

MatrixHandle SCIRun::IgbFileMatrix_reader(LoggerHandle pr, const char *filename)
{
  std::string line;
  std::vector<std::string> strs;
  int x_size=0;
  int t_size=0;
  int count=0;

  std::ifstream myfile(filename);
  if (myfile.is_open())
  {
    for (int i=0; i<5; i++)
    {
      std::getline(myfile,line);
      boost::split(strs,line,boost::is_any_of(":, "));
      size_t sz = line.size();

      for (int p=0;p<sz;p++)
      {
        if (boost::iequals(strs[p], "x"))
        {
          x_size=atoi(strs[p+1].c_str());
          count += 1;
        }
        if (boost::iequals(strs[p], "t"))
        {
          t_size=atoi(strs[p+1].c_str());
          count += 1;
        }
        if (count == 2)
          break;
      }
  	}
    myfile.close();
  }

  std::streamoff length=0;
  std::streamsize numbyts=0;

  std::ifstream is;
  is.open(filename, std::ios::in | std::ios::binary );
  if (is.is_open())
  {
    // get length of file:
    is.seekg(0, std::ios::end);
    length = is.tellg();
    is.seekg(1024, std::ios::beg);
    length -= 1024;

    std::vector<float> vec;
    vec.resize(length);
    is.read((char*)&vec[0],length);

    if (!is)
    {
      numbyts = is.gcount();
      std::cerr << "Error reading binary data. Number of bytes read: " << numbyts << std::endl;
    }
    is.close();

    auto result(boost::make_shared<DenseMatrix>(x_size,t_size));

    for(size_t p=0;p<t_size;p++ )
    {
      for(size_t pp=0;pp<x_size;pp++ )
      {
        (*result)(pp, p) = vec[(p*x_size)+pp];
      }
    }

    return result;
  }
  return nullptr;
}


#include <boost/gil/image_processing/scaling.hpp>
#include <boost/gil/image.hpp>

#include <boost/core/lightweight_test.hpp>

#include <iostream>

namespace gil = boost::gil;

bool are_equal(gil::rgb8_view_t expected, gil::rgb8_view_t actual) {
    if (expected.dimensions() != actual.dimensions())
        return false;

    for (long int y = 0; y < expected.height(); ++y)
    {
        for (long int x = 0; x < expected.width(); ++x)
        {
            if (expected(x, y) != actual(x, y))
            {
                return false;
            }
        }
    }

    return true;
}

void test_lanczos_black_image()
{

    const gil::point_t input_dimensions(20, 20);
    const gil::point_t output_dimensions(input_dimensions.x / 2, input_dimensions.y / 2);
    gil::rgb8_image_t image(input_dimensions, gil::rgb8_pixel_t(0, 0, 0), 0);
    // fill with values other than 0
    gil::rgb8_image_t output_image(
        output_dimensions,
        gil::rgb8_pixel_t(100, 100, 100),
        0
    );
    gil::rgb8_image_t expected(
        output_dimensions,
        gil::rgb8_pixel_t(0, 0, 0),
        0
    );

    auto view = gil::view(image);
    auto output_view = gil::view(output_image);
    auto expected_view = gil::view(expected);
    gil::scale_lanczos(view, output_view, 5);
    BOOST_TEST(are_equal(expected_view,output_view));
}

void test_lanczos_response_on_zero()
{
    //random value for a
    BOOST_TEST(gil::lanczos(0, 2) == 1);
}

int main()
{
    test_lanczos_black_image();
    test_lanczos_response_on_zero();
    return boost::report_errors();
}


#include <boost/compute/functional/popcount.hpp>


// -*- mode: c++ -*-
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, JSK Lab
 *  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/o2r other materials provided
 *     with the distribution.
 *   * Neither the name of the JSK Lab nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT 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 <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <ctime>
#include <boost/random.hpp>
#include <string>
#include <typeinfo>
#include <sstream>
#include<stdio.h>
#include <sys/time.h>

void usage(char** argv)
{
  std::cerr << "Usage: " << argv[0] << " Iteration Number-of-points Max-iterations Gaussian-noise-variance Outlier-threshold" << std::endl;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr generatePoints(const int num_points, const double variance)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  boost::mt19937 gen = boost::mt19937(static_cast<unsigned long>(time(0)));
  boost::normal_distribution<> dst(0.0, sqrt(variance));
  boost::variate_generator<
    boost::mt19937&,
    boost::normal_distribution<> > rand(gen, dst);

  const double dx = 0.01;
  cloud->points.resize(num_points * num_points);
  for (size_t i = 0; i < num_points; i++) {
    for (size_t j = 0; j < num_points; j++) {
      pcl::PointXYZ p;
      p.x = i * dx;
      p.y = j * dx;
      p.z = rand();
      cloud->points[i * num_points + j] = p;
    }
  }
  return cloud;
}

int toInt(char* argv)
{
  std::stringstream ss(argv);
  int val = 0;
  ss >> val;
  return val;
}

double toDouble(char* argv)
{
  std::stringstream ss(argv);
  double val = 0;
  ss >> val;
  return val;
}

int main(int argc, char** argv)
{
  if (argc != 6) {
    usage(argv);
    return 1;
  }
  int iteration = toInt(argv[1]);
  int number_of_points = toInt(argv[2]);
  int max_iterations = toInt(argv[3]);
  double variance = toDouble(argv[4]);
  double outlier_threshold = toDouble(argv[5]);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = generatePoints(number_of_points, variance);
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients (true);
  seg.setRadiusLimits(0.0, std::numeric_limits<double>::max ()); // 0.3?
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (outlier_threshold);
  seg.setInputCloud(cloud);
  seg.setMaxIterations (max_iterations);
  seg.setModelType (pcl::SACMODEL_PLANE);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  struct timeval s, e;
  gettimeofday(&s, NULL);
  for (size_t i = 0; i < iteration; i++) {
    seg.segment (*inliers, *coefficients);
  }
  gettimeofday(&e, NULL);
  // Total, each, iteration, number_of_points, max_iterations, variance, outlier_threshold
  double time = (e.tv_sec - s.tv_sec) + (e.tv_usec - s.tv_usec)*1.0E-6;
  printf("%lf,%lf,%d,%d,%d,%f,%f\n", time, time / iteration,
         iteration, number_of_points * number_of_points, max_iterations, variance, outlier_threshold);
  std::cerr << "Took " << time << " sec (" << time / iteration << " sec for each)" << std::endl;
  return 0;
}


#ifndef TRACKER_STATUS_BAYES_HPP
#define TRACKER_STATUS_BAYES_HPP

#include <memory>
#include <unordered_map>

#include <Eigen/Dense>

#include "open_ptrack/recognition/registered_face.hpp"

/**
 * @brief the face recognition status of a tracker
 */
class TrackerStatusBayes {
public:
  typedef std::shared_ptr<TrackerStatusBayes> Ptr;

  TrackerStatusBayes(int tracker_id)
    : tracker_id(tracker_id),
      face_ids(1),
      counts(1),
      priori(1)
  {
    face_ids[0] = -1;
    counts[0] = 0;
    priori[0] = 0.1;
  }

  void addFace(const std::shared_ptr<Eigen::VectorXf>& feature) {
    features.push_back(feature);
  }

  int getTrackerId() const {
    return tracker_id;
  }

  const std::vector<std::shared_ptr<Eigen::VectorXf>>& getFeatures() const {
    return features;
  }

  std::pair<Eigen::VectorXi, Eigen::VectorXd> createPrioriVector(const Eigen::VectorXi& new_face_ids) {
    Eigen::VectorXi new_counts(new_face_ids.size());
    Eigen::VectorXd new_probs(new_face_ids.size());

    double eps = 0.1;

    int cursor = 0;
    for(int i=0; i<new_probs.size(); i++) {
      int id = new_face_ids[i];
      while(cursor != priori.size() && face_ids[cursor] < id) {
        cursor ++;
      }

      if(cursor == priori.size() || face_ids[cursor] != id) {
        new_probs[i] = eps;
        new_counts[i] = 0;
      } else {
        new_probs[i] = priori[cursor];
        new_counts[i] = counts[cursor];
      }
    }

    return std::make_pair(new_counts, new_probs);
  }

  void update(const Eigen::VectorXi& face_ids, const Eigen::VectorXi& counts, const Eigen::VectorXd& posterior) {
    this->face_ids = face_ids;
    this->counts = counts;
    this->priori = posterior;
  }

private:
  int tracker_id;
  std::vector<std::shared_ptr<Eigen::VectorXf>> features;

  Eigen::VectorXi face_ids;   // IDs of the faces
  Eigen::VectorXi counts;     // the numbers of observations for each face
  Eigen::VectorXd priori;     // the priori probabilities for each face
};

#endif // BAYES_INFERENCE_STATUS_HPP


#include <boost/numeric/odeint/integrate/max_step_checker.hpp>


#pragma once
/**
 * @file kdtree.hpp
 * @author Thomas Grandits
 * @brief Header of the KD-Tree implementation (CPU & GPU)
 * @version 0.1
 * @date 2020-12-16
 *
 * @copyright Copyright (c) 2020
 *
 */

#include <iostream>     // std::cout
#include <iterator>     // std::back_inserter
#include <vector>       // std::vector
#include <algorithm>    // std::copy
#include <array>
#include <assert.h>     /* assert */
#include <cmath>
#include <tuple>
#include <numeric>
#include <functional>
#include "nndistance.hpp"

#include <Eigen/Dense>

//https://stackoverflow.com/questions/32014839/how-to-use-a-cuda-class-header-file-in-both-cpp-and-cuda-modules
#ifdef __CUDACC__
#define CUDA_HOSTDEV __host__ __device__
#else
#define CUDA_HOSTDEV
#endif

typedef uint8_t dim_t;
typedef uint32_t point_i_t;
typedef uint32_t point_i_knn_t;
typedef point_i_t tree_ind_t;
typedef int level_t;
typedef uint8_t slot_t;

template <typename T, dim_t dims>
using Vec = Eigen::Matrix<T, dims, 1>;

/**
 * @brief PingPongBuffer consisting of two actual buffers that can be easily accessed and swapped
 *
 * @tparam T Type of the PingPongBuffer
 */
template <typename T>
struct PingPongBuffer
{
    //std::array<T*, 2> buffers;
    T* buffers[2];
    slot_t current_slot = 0;

    CUDA_HOSTDEV PingPongBuffer(){}
    CUDA_HOSTDEV PingPongBuffer(T* ping, T* pong){buffers[0] = ping; buffers[1] = pong;}

    CUDA_HOSTDEV inline T* getCurrentSlot() { return buffers[current_slot]; }
    CUDA_HOSTDEV inline slot_t getCurrentSlotInd() const { return current_slot; }
    CUDA_HOSTDEV inline T* getPongSlot() { return buffers[(current_slot + 1) % 2]; }
    CUDA_HOSTDEV inline void increment()  { current_slot = (current_slot + 1) % 2; }
    CUDA_HOSTDEV inline T* getCurrentSlotAndIncrement() { T* buf = getCurrentSlot();  increment(); return buf;}
};

/**
 * @brief Computes the total number of nodes necessary for a KD-Tree with the given levels
 *
 * @tparam T Type of the levels
 * @param levels Number of levels of the tree
 * @return T Number of total nodes of the tree
 */
template <typename T>
inline T compTotalNrNodes(const T levels)
{
    std::vector<T> range(levels);
    std::iota(range.begin(), range.end(), 0);

    return std::accumulate(range.begin(), range.end(), 0, [](const T accum, const T cur_level){return accum + std::pow<T>(2, cur_level);});
}

/**
 * @brief Computes the total number of leaves necessary for a KD-Tree with the given levels
 *
 * @tparam T Type of the levels
 * @param levels Number of levels of the tree
 * @return CUDA_HOSTDEV T Number of total leaves of the tree
 */
template <typename T>
CUDA_HOSTDEV inline T compTotalNrLeaves(const T levels)
{
    return std::pow<T>(2, levels);
}

/**
 * @brief Enum to tag which nodes have been visited when traversing the tree
 *
 */
enum NodeTag : unsigned char
{
    uncharted = 0,
    //visited = 1,
    left_visited = 1,
    right_visited = 2,
    left_right_visited = left_visited | right_visited
};

/**
 * @brief Enum to specify in which direction an algorithm should continue traversing the tree
 *
 */
enum NodeDirection : unsigned char
{
    up = 0,
    left = 1,
    right = 2,
    finished = 3
};

/**
 * @brief Specifies data associated to each leaf of the tree
 *
 * @tparam T Type of the leaf
 * @tparam dims Dimensionality of the contained points
 */
template <typename T, dim_t dims>
struct PartitionLeaf
{
    std::array<T, dims>* data;
    point_i_t nr_points;
    point_i_t offset;

    PartitionLeaf(std::array<T, dims>* data_, const point_i_t nr_points_, const point_i_t offset_)
    : data(data_), nr_points(nr_points_), offset(offset_){}

    PartitionLeaf(){}
};

/**
 * @brief Partition info present at each node, denoting the split-axis and the median used for the partitioning
 *
 * @tparam T Type of the median
 */
template <typename T>
struct Partition //: PartitionAligned<T>
{
    dim_t axis_split;
    T median;

    Partition(const dim_t axis_split_, const T median_) : axis_split(axis_split_), median(median_){}
    Partition(){}
};

template <typename T>
using tree_visit_f = std::function<NodeDirection(const Partition<T>&, const NodeTag, const tree_ind_t, const level_t)>;

/**
 * @brief Contains all partition info associated with a KD-Tree:
 *        All partitions, all leaves, levels of the tree and the underyling structured points, as well as the shuffled indices are all managed here.
 *
 * @tparam T Type of the structured points and all underlying data
 * @tparam dims Dimensionality of the points
 * @tparam delete_partitions If true, deletes all associated partition data once the tree is destructed
 */
template <typename T, dim_t dims, bool delete_partitions = true>
struct PartitionInfo
{
    Partition<T>* partitions;
    PartitionLeaf<T, dims>* leaves;
    level_t levels;
    std::array<T, dims>* structured_points = NULL;
    point_i_t* shuffled_inds = NULL;
    const point_i_t nr_points = 0;
    const tree_ind_t nr_partitions, nr_leaves;

    PartitionInfo(std::vector<Partition<T>>&& parts, std::vector<PartitionLeaf<T, dims>>&& leaves_, point_i_t* shuffled_inds_, const point_i_t nr_points_);

    PartitionInfo(PartitionInfo&& to_move) : partitions(std::move(to_move.partitions)), leaves(std::move(to_move.leaves)), levels(to_move.levels),
        structured_points(to_move.structured_points), shuffled_inds(to_move.shuffled_inds), nr_points(to_move.nr_points),
        nr_partitions(to_move.nr_partitions), nr_leaves(to_move.nr_leaves)
    {
        to_move.partitions = NULL;
        to_move.leaves = NULL;
        to_move.structured_points = NULL;
        to_move.shuffled_inds = NULL;
    }

    PartitionInfo(const PartitionInfo<T, dims>& to_copy) : partitions(to_copy.partitions), leaves(to_copy.leaves), levels(to_copy.levels),
        structured_points(to_copy.structured_points), shuffled_inds(to_copy.shuffled_inds), nr_points(to_copy.nr_points),
        nr_partitions(to_copy.nr_partitions), nr_leaves(to_copy.nr_leaves)
    {
        if(delete_partitions)
            throw std::runtime_error("Copies are not meant to do this here");
    }

    CUDA_HOSTDEV ~PartitionInfo()
    {
        if(delete_partitions)
        {
            delete partitions;
            delete leaves;
        }
    }
};

template <typename T, dim_t dims>
using PartitionInfoDevice = PartitionInfo<T, dims, false>;

/**
 * @brief Helper class to effectively traverse the KD-tree without any recursions.
 *
 * @tparam T Type of the points
 * @tparam dims Dimensionality of the points
 */
template <typename T, dim_t dims>
class TreeTraversal
{
//protected:
public:
    PartitionInfo<T, dims>* partition_info = NULL;

    NodeTag* visited_info;
    tree_ind_t current_lin_ind;
    level_t current_level;
    point_i_t nr_nodes;

public:

    /**
     * @brief Resets all positions and tags, effectively reinitializing the object without the need to destruct it.
     *
     * @return CUDA_HOSTDEV
     */
    CUDA_HOSTDEV void resetPositionAndTags()
    {
        std::fill(visited_info, visited_info + nr_nodes, NodeTag::uncharted);
        current_lin_ind = current_level = 0;
    }

    TreeTraversal(PartitionInfo<T, dims>* partition_info_) : partition_info(partition_info_), nr_nodes(partition_info_->nr_partitions)
    {
        //visited_info = std::move(std::vector<NodeTag>(compTotalNrNodes(partition_info->levels), NodeTag::uncharted));
        visited_info = new NodeTag[nr_nodes];
        std::fill(visited_info, visited_info + nr_nodes, NodeTag::uncharted);
        resetPositionAndTags();
    }

    CUDA_HOSTDEV TreeTraversal(){}

    CUDA_HOSTDEV TreeTraversal(PartitionInfoDevice<T, dims>* partition_info_) :
    //TODO: This could cause serious problems down the road!!!
    partition_info(reinterpret_cast<PartitionInfo<T, dims>*>(partition_info_)),
    //partition_info_d(partition_info_),
    nr_nodes(partition_info_->nr_partitions)
    {
        //visited_info = std::move(std::vector<NodeTag>(compTotalNrNodes(partition_info->levels), NodeTag::uncharted));
        visited_info = new NodeTag[nr_nodes];
        std::fill(visited_info, visited_info + nr_nodes, NodeTag::uncharted);
        current_lin_ind = current_level = 0;
    }

    CUDA_HOSTDEV ~TreeTraversal()
    {
        delete visited_info;
    }

    template <typename T_ind>
    static T_ind compParentInd(const T_ind lin_ind) { return (lin_ind - 1)/2; }

    template <typename T_ind>
    static T_ind compLeftChildInd(const T_ind lin_ind) { return lin_ind * 2 + 1; }

    template <typename T_ind>
    static T_ind compRightChildInd(const T_ind lin_ind) { return lin_ind * 2 + 2; }

    template <typename T_ind>
    static T_ind compLevel(T_ind lin_ind)
    {
        if(lin_ind == 0)
            return 1;

        T_ind level = 0;
        while((lin_ind = compParentInd(lin_ind)) != 0)
            level++;

        return level + 2;
    }

    template <typename T_ind, typename T_ind2>
    static inline T_ind compLeftLeafInd(const T_ind lin_ind, const T_ind2 nr_partitions) { return compLeftChildInd(lin_ind) - nr_partitions; }

    template <typename T_ind, typename T_ind2>
    static inline T_ind compRightLeafInd(const T_ind lin_ind, const T_ind2 nr_partitions) { return compLeftLeafInd(lin_ind, nr_partitions) + 1; }

    template <typename T_ind>
    static inline T_ind compLeftLeafInd(const T_ind lin_ind) { return compLeftLeafInd(lin_ind, compTotalNrNodes(compLevel(lin_ind))); }

    template <typename T_ind>
    static inline T_ind compRightLeafInd(const T_ind lin_ind) { return compLeftLeafInd(lin_ind) + 1; }

    CUDA_HOSTDEV inline tree_ind_t compLeftLeafInd(){ return compLeftLeafInd(current_lin_ind); }
    CUDA_HOSTDEV inline tree_ind_t compRighLeafInd(){ return compRightLeafInd(current_lin_ind); }
    CUDA_HOSTDEV inline level_t getTotalLevels() const {return partition_info->levels;}
    CUDA_HOSTDEV inline tree_ind_t getCurrentLinearIndex() const {return current_lin_ind;}
    CUDA_HOSTDEV inline level_t getCurrentLevel() const {return current_level;}
    CUDA_HOSTDEV inline NodeTag getCurrentTag() const {return visited_info[current_lin_ind];}
    CUDA_HOSTDEV //Partition<T>& getCurrentPartition() {return partition_info->partitions[current_lin_ind];}
    CUDA_HOSTDEV inline const Partition<T>& getCurrentConstPartition() const {return partition_info->partitions[current_lin_ind];}
    CUDA_HOSTDEV inline void moveToParent(){ assert(current_level != 0 && current_lin_ind != 0); current_lin_ind = compParentInd(current_lin_ind); current_level -= 1; }
    CUDA_HOSTDEV inline void moveToLeftChild(){ assert(current_level < partition_info->levels);  current_lin_ind = compLeftChildInd(current_lin_ind); current_level += 1; }
    CUDA_HOSTDEV inline void moveToRightChild(){ assert(current_level < partition_info->levels);  current_lin_ind = compRightChildInd(current_lin_ind); current_level += 1; }
    CUDA_HOSTDEV inline bool isLeafParent() const { return current_level == partition_info->levels-1; }
    CUDA_HOSTDEV inline const PartitionLeaf<T, dims>& getLeftLeaf() const{ assert(isLeafParent()); return partition_info->leaves[compLeftLeafInd(current_lin_ind, partition_info->nr_partitions)]; }
    CUDA_HOSTDEV inline const PartitionLeaf<T, dims>& getRightLeaf() const{ assert(isLeafParent()); return partition_info->leaves[compRightLeafInd(current_lin_ind, partition_info->nr_partitions)]; }
    CUDA_HOSTDEV inline void setCurrentNodeTag(const NodeTag new_tag){ visited_info[current_lin_ind] = new_tag; }

    static_assert(NodeTag::left_right_visited == 3 && NodeTag::uncharted == 0, "The binary computation does not work without these values");
    CUDA_HOSTDEV inline NodeTag moveTagToBinaryPosition(const NodeTag tag) const
    {
        const unsigned char bit_idx  = current_lin_ind % 4;
        return static_cast<NodeTag>(tag << (bit_idx*2));
    }
    CUDA_HOSTDEV inline NodeTag getBinaryIndMask() const { return moveTagToBinaryPosition(NodeTag::left_right_visited);}
    CUDA_HOSTDEV inline NodeTag getCurrentTagBinary() const
    {
        const auto byte_idx = current_lin_ind / 4;
        const unsigned char bit_idx  = current_lin_ind % 4;
        const unsigned char byte_data = visited_info[byte_idx];
        const unsigned char binary_bitmask = NodeTag::left_right_visited << (bit_idx*2);
        const unsigned char data = ( (byte_data & binary_bitmask) >> (bit_idx*2)) & NodeTag::left_right_visited;

        return static_cast<NodeTag>(data);
    }
    CUDA_HOSTDEV inline void setCurrentNodeTagBinary(const NodeTag new_bits)
    {
        const auto byte_idx = current_lin_ind / 4;
        const NodeTag old_byte = visited_info[byte_idx];
        const NodeTag modified_mask = moveTagToBinaryPosition(NodeTag::left_right_visited); //getBinaryIndMask();
        const NodeTag unmodified_mask = static_cast<NodeTag>(~modified_mask);
        const unsigned char bit_idx  = current_lin_ind % 4;
        visited_info[byte_idx] = static_cast<NodeTag>((unmodified_mask & old_byte) | (moveTagToBinaryPosition(new_bits) & modified_mask));
    }

    /**
     * @brief Traverses the tree with which the object was constructed and calls a function at each node.
     *
     * @param direction_f Function to call at each node. This function will receive the current node along with some positional information.
     *                    The function has to return a direction in which the tree should be further traversed. Finished when the function
     *                    returns NodeDirection::finished.
     * @return CUDA_HOSTDEV
     */
    CUDA_HOSTDEV void traverseTree(const tree_visit_f<T>& direction_f)
    {
        NodeDirection next_dir;
        while((next_dir = direction_f(getCurrentConstPartition(), getCurrentTag(), current_lin_ind, current_level))
                        != NodeDirection::finished)
        {
            switch(next_dir)
            {
                case NodeDirection::up:
                    moveToParent(); break;
                case NodeDirection::left:
                    moveToLeftChild(); break;
                case NodeDirection::right:
                    moveToRightChild(); break;
            }
        }
    }
};

/**
 * @brief Computes the median along given axis for the ordered points
 *
 * @tparam T Type of the points
 * @tparam dims Dimensionality of the points
 * @param points_ordered Points for which the median should be computed. Note that the points are required to be ordered
 *                       in the respective dimension. This should be ensured after the tree was already constructed.
 * @param nr_points Number of points present in the array
 * @param current_axis Axis for which the median should be computed
 * @return T The median
 */
template <typename T, dim_t dims>
inline T compMedian(const std::array<T, dims>* points_ordered, const point_i_t nr_points, const dim_t current_axis)
{
    assert(nr_points > 1);
    const point_i_t half_ind = nr_points / 2;
    if(nr_points % 2 == 1)
        return points_ordered[half_ind][current_axis];
    else
        return 0.5 * (points_ordered[half_ind][current_axis] + points_ordered[half_ind-1][current_axis]);
}

/**
 * @brief Creates the KD-Tree's partitions using a recursive algorithms
 *
 * @tparam T Type of the points
 * @tparam dims Dimensionality of the points
 * @param partitions Vector of already generated (possibly empty) partitions, which will be initialized
 * @param leaves Empty vector of leaves that will be generated when creating the KD-tree
 * @param lin_ind Current node index (initially 0)
 * @param structured_points Pointer to the array of underlying ordered points
 * @param shuffled_inds Pointer to the array of underlying indices of the ordered points
 * @param nr_points Number of total points
 * @param levels Levels of the final KD-Tree
 * @param current_axis Current dimension in which we will split (initially 0)
 * @param arr_offset Current recursive offset of in the structured points array (initially 0)
 */
template <typename T, dim_t dims>
void createPartitionRecursive(
                                std::vector<Partition<T>>& partitions,
                                std::vector<PartitionLeaf<T, dims>>& leaves,
                                const tree_ind_t lin_ind,
                                std::array<T, dims>* structured_points,
                                point_i_t* shuffled_inds, const point_i_t nr_points,
                                const int levels, const dim_t current_axis, const point_i_t arr_offset)
{
    typedef TreeTraversal<T, dims> tree_t;
    if(nr_points == 0)
        throw std::runtime_error("Error: Ran out of points while building KD-Tree. Either you required too many levels, or you used a lot of coplanar points");

    //Track global indices
    std::vector<point_i_t> idx(nr_points);
    std::iota(idx.begin(), idx.end(), 0);
    std::sort(idx.begin(), idx.end(),
    [current_axis, &structured_points](const point_i_t lhs, const point_i_t rhs)
    { return structured_points[lhs][current_axis] < structured_points[rhs][current_axis]; });

    //Reorder in the original
    //Work on copy and then copy back
    std::vector<std::array<T, dims>> structured_points_copy;
    std::vector<point_i_t> shuffle_copy(shuffled_inds, shuffled_inds + nr_points);
    for(size_t elem_i = 0; elem_i < nr_points; elem_i++)
        shuffled_inds[elem_i] = shuffle_copy[idx[elem_i]];

    std::sort(structured_points, structured_points + nr_points,
    [current_axis](const std::array<T, dims>& lhs, const std::array<T, dims>& rhs){return lhs[current_axis] < rhs[current_axis];});

    const T median = compMedian<T, dims>(structured_points, nr_points, current_axis);

    std::array<T, dims>* points_lower = structured_points;
    std::array<T, dims>* points_higher = structured_points + nr_points/2;
    const point_i_t nr_points_lower = nr_points/2;
    const point_i_t nr_points_higher = nr_points - nr_points_lower; //TODO: For duplicates, this may cause alterations...

    assert((nr_points - nr_points_lower) > 0);

    if(levels == 0)
    {
        assert(tree_t::compLeftLeafInd(lin_ind) == leaves.size());
        leaves.push_back(std::move(PartitionLeaf<T, dims>(points_lower, nr_points_lower, arr_offset)));

        assert(tree_t::compRightLeafInd(lin_ind) == leaves.size());
        leaves.push_back(std::move(PartitionLeaf<T, dims>(points_higher, nr_points_higher, arr_offset + nr_points_lower)));
        partitions[lin_ind] = std::move(Partition<T>(current_axis, median));
    }
    else
    {
        createPartitionRecursive<T, dims>(
            partitions, leaves, TreeTraversal<T, dims>::compLeftChildInd(lin_ind),
            points_lower, shuffled_inds, nr_points_lower, levels-1,
                (current_axis + 1) % dims, arr_offset);
        createPartitionRecursive<T, dims>(
            partitions, leaves, TreeTraversal<T, dims>::compRightChildInd(lin_ind),
            points_higher, shuffled_inds + nr_points_lower, nr_points_higher, levels-1,
                (current_axis + 1) % dims, arr_offset + nr_points_lower);

        Partition<T> partition(current_axis, median);
        partitions[lin_ind] = std::move(partition);
    }
}

/**
 * @brief Recursively generates a KD-Tree using \ref createPartitionRecursive 'createPartitionRecursive'
 *
 * @tparam T Type of the points
 * @tparam dims Dimensionality of the points
 * @param points_flat Array of points
 * @param nr_points Number of points
 * @param levels KD-Tree levels
 * @return PartitionInfo<T, dims> The PartitionInfo containing the information regarding the KD-Tree
 */
template <typename T, dim_t dims>
PartitionInfo<T, dims> createKDTree(const T* points_flat,
                                        const point_i_t nr_points, const int levels)
{
    assert(levels >= 1);
    std::vector<PartitionLeaf<T, dims>> leaves; //(compTotalNrLeaves(levels));
    std::vector<Partition<T>> partitions(compTotalNrNodes(levels), Partition<T>(-1, -1));

    point_i_t* orig_inds = new point_i_t[nr_points];
    std::iota(orig_inds, orig_inds + nr_points, 0); //Initializing
    //std::cout << "Creating main partition: " << nr_points << ", " << levels << std::endl;
    const std::array<T, dims>* points = reinterpret_cast<const std::array<T, dims>*>(points_flat);
    //std::vector<std::array<T, dims>> structured_points(points, points + nr_points);
    std::array<T, dims>* structured_points = new std::array<T, dims>[nr_points];
    std::copy(points, points + nr_points, structured_points);
    createPartitionRecursive<T, dims>(partitions, leaves, 0,
                                        structured_points,
                                        orig_inds, nr_points, levels-1, 0, 0);
    //partitions[0] = new MainPartition<T, dims>(std::move(partitions[0]));
    /*MainPartition<T, dims> main_partition = MainPartition<T, dims>(std::move(
                        ),
                                        structured_points, orig_inds, nr_points, levels);*/

    //Sanity checks
    #ifndef NDEBUG
    point_i_t offset = 0;
    for(int i = 0; i < leaves.size(); i++)
    {
        assert(leaves[i].offset == offset);
        offset += leaves[i].nr_points;
    }
    for(int i = 1; i < leaves.size(); i++)
    {
        assert((leaves[i-1].data + leaves[i-1].nr_points) == leaves[i].data);
    }
    #endif

    assert(leaves.size() == compTotalNrLeaves(levels));

    return std::move(PartitionInfo<T, dims>(std::move(partitions), std::move(leaves), /*structured_points,*/ orig_inds, nr_points)); //std::move(main_partition);
}

/**
 * @brief Computes the difference between two points (lhs - rhs)
 *
 * @tparam T Type of the points
 * @tparam T_calc Type of the resulting difference
 * @tparam dims Dimensionality of the points
 * @param lhs Array of points
 * @param rhs Array of points
 * @return std::array<T_calc, dims> The difference between the lhs and rhs
 */
template
<typename T, typename T_calc, dim_t dims>
inline CUDA_HOSTDEV std::array<T_calc, dims> compDiff(const std::array<T, dims>& lhs, const std::array<T, dims>& rhs)
{
    std::array<T_calc, dims> diffs;

    for(dim_t dim_i = 0; dim_i < dims; dim_i++)
    {
        diffs[dim_i] = static_cast<T_calc>(lhs[dim_i]) - static_cast<T_calc>(rhs[dim_i]);
    }

    return std::move(diffs);
}

/**
 * @brief Computes the sum of squared elem
 *
 * @tparam T Type of the array
 * @tparam dims Number of points
 * @param x Array of values
 * @return T sum(x^2)
 */
template
<typename T, dim_t dims>
inline CUDA_HOSTDEV T compQuadrSum(const std::array<T, dims>& x)
{
    return std::accumulate(x.begin(), x.end(), 0., [](const T accum, const T elem){return accum + elem*elem;});
}

/**
 * @brief Computes the element-wise quadratic euclidean distance between lhs and rhs, i.e. ||lhs_i - rhs_i||
 *
 * @tparam T Type of the array
 * @tparam T_calc Type of the calculation
 * @tparam dims Dimensionality of the points
 * @param lhs Left-hand-side of the calculation
 * @param lhs Right-hand-side of the calculation
 * @return T_calc ||lhs_i - rhs_i||
 */
template
<typename T, typename T_calc, dim_t dims>
inline CUDA_HOSTDEV T_calc compQuadrDist(const std::array<T, dims>& lhs, const std::array<T, dims>& rhs)
{
    std::array<T_calc, dims> diffs = compDiff<T, T_calc, dims>(lhs, rhs);
    return compQuadrSum<T_calc, dims>(diffs);
}

/**
 * @brief Computes the distance to the projected point onto the median of the current dimension. This is used to see if we need to further
 *        traverse the tree or already found the KNNs, which is true in case the projection is farther away than
 *        the distance to the current KNN most far away.
 *
 * @tparam T Type of the array
 * @tparam dims Dimensionality of the points
 * @param point Original point for which to search the KNN
 * @param point_proj The original point, already projected on possible multiple previous dimensions. Note that in the simplest case
 *                   this is equal to point.
 * @param Partition<T> Node information of the current axis/dimension split that we want to compute the projection in
 * @return T Resulting distance of the current projection
 */
template
<typename T, dim_t dims>
inline CUDA_HOSTDEV T projectionDist(const std::array<T, dims>& point, const std::array<T, dims>& point_proj, const Partition<T>& partition)
{
    std::array<T, dims> proj_vec = compDiff<T, T, dims>(point_proj, point);
    const dim_t current_axis = partition.axis_split;
    proj_vec[current_axis] += partition.median - point_proj[current_axis];
    return compQuadrSum<T, dims>(proj_vec);
}

/**
 * @brief See \ref projectionDist 'projectionDist'
 */
template
<typename T, dim_t dims>
inline CUDA_HOSTDEV T projectionDist(const Vec<T, dims>& point, const Vec<T, dims>& point_proj, const Partition<T>& partition)
{
    Vec<T, dims> proj_vec = (point_proj - point);
    const dim_t current_axis = partition.axis_split;
    proj_vec[current_axis] += partition.median - point_proj[current_axis];
    return proj_vec.squaredNorm();
}

/**
 * @brief Checks if we need to compute the distances to any of the points in the partition. Achieved by using \ref projectionDist 'projectionDist'.
 */
template
<typename T, typename T_calc, dim_t dims>
inline CUDA_HOSTDEV bool partitionNecessary(const std::array<T, dims>& point, const std::array<T, dims>& point_proj,
                                const Partition<T>& partition, const T current_worst_dist)
{
    const auto proj_dist = projectionDist<T, dims>(point, point_proj, partition);
    return proj_dist < current_worst_dist;
}

/**
 * @brief See \ref partitionNecessary 'partitionNecessary'.
 */
template
<typename T, typename T_calc, dim_t dims>
inline CUDA_HOSTDEV bool partitionNecessary(const Vec<T, dims>& point, const Vec<T, dims>& point_proj,
                                const Partition<T>& partition, const T current_worst_dist)
{
    const auto proj_dist = projectionDist<T, dims>(point, point_proj, partition);
    return proj_dist < current_worst_dist;
}

/**
 * @brief Computes the quadratic distances between point to all points in partition_leaf and updates the current knn, marked by best_dists and best_idx.
 *
 * @param point Query point for which the KNN should be calculated
 * @param partition_leaf Partition for which all quadratic distances should be computed
 * @param best_dists Quadr. distances to the currently assumed KNNs (starts with infinity). Has a length of K
 * @param best_idx Indices of the currently assumed KNNs (starts with -1). Has a length of K
 * @param nr_nns_searches How many nearest neighbors will be searched (=K)
 *
 */
template
<typename T, typename T_calc, dim_t dims>
void compQuadrDistLeafPartition(const std::array<T, dims>& point, const PartitionLeaf<T, dims>& partition_leaf,
                                    T* best_dists, point_i_knn_t* best_idx,
									const point_i_knn_t nr_nns_searches)
{
	const std::array<T, dims>* partition_data = partition_leaf.data;
    const point_i_t partition_size = partition_leaf.nr_points;
	const point_i_t partition_offset = partition_leaf.offset;
    for(point_i_t ref_i = 0; ref_i < partition_size; ref_i++)
    {
        const T_calc dist = compQuadrDist<T, T_calc, dims>(point, partition_data[ref_i]);
        const auto insertion_idx = knnInsertionDynamic<T_calc>(dist, best_dists, nr_nns_searches);
        if(insertion_idx < nr_nns_searches)
        {
            const auto best_dists_end = best_dists + nr_nns_searches;
            const auto best_idx_end = best_idx + nr_nns_searches;
            assert(best_dists + insertion_idx < best_dists_end);
            assert(best_idx + insertion_idx < best_idx_end);
            //Shift elements to the right
            //std::cout << k << ", " << d << "insert into " << insertion_idx << std::endl;
            //std::move_backward(best_dists + insertion_idx, best_dists +  nr_nns_searches - 1, best_dists.end());
            //std::move_backward(best_idx.begin() + insertion_idx, best_idx.end() - 1, best_idx.end());
            moveBackward(best_dists + insertion_idx, best_dists_end - 1, best_dists_end);
            moveBackward(best_idx + insertion_idx, best_idx_end - 1, best_idx_end);
            best_dists[insertion_idx] = dist;
            best_idx[insertion_idx] = ref_i + partition_offset;
        }
    }
}

/**
 * @brief See \ref compQuadrDistLeafPartition 'compQuadrDistLeafPartition'.
 */
template
<typename T, typename T_calc, dim_t dims>
CUDA_HOSTDEV void compQuadrDistLeafPartition(const Vec<T, dims>& point, const PartitionLeaf<T, dims>& partition_leaf,
                                    T* best_dists, point_i_knn_t* best_idx,
									const point_i_knn_t nr_nns_searches)
{
    //printf("compQuadrDistLeafPartition: %x, ", partition_leaf.data);
    //printf("%d, ", partition_leaf.nr_points);
    //printf("%d\n", partition_leaf.offset);
	const Vec<T, dims>* partition_data = reinterpret_cast<Vec<T, dims>*>(partition_leaf.data);
    const point_i_t partition_size = partition_leaf.nr_points;
	const point_i_t partition_offset = partition_leaf.offset;
    for(point_i_t ref_i = 0; ref_i < partition_size; ref_i++)
    {
        const T_calc dist = (point - partition_data[ref_i]).squaredNorm();
        const auto insertion_idx = knnInsertionDynamic<T_calc>(dist, best_dists, nr_nns_searches);
        if(insertion_idx < nr_nns_searches)
        {
            const auto best_dists_end = best_dists + nr_nns_searches;
            const auto best_idx_end = best_idx + nr_nns_searches;
            assert(best_dists + insertion_idx < best_dists_end);
            assert(best_idx + insertion_idx < best_idx_end);
            //Shift elements to the right
            //std::cout << k << ", " << d << "insert into " << insertion_idx << std::endl;
            //std::move_backward(best_dists + insertion_idx, best_dists +  nr_nns_searches - 1, best_dists.end());
            //std::move_backward(best_idx.begin() + insertion_idx, best_idx.end() - 1, best_idx.end());
            moveBackward(best_dists + insertion_idx, best_dists_end - 1, best_dists_end);
            moveBackward(best_idx + insertion_idx, best_idx_end - 1, best_idx_end);
            best_dists[insertion_idx] = dist;
            best_idx[insertion_idx] = ref_i + partition_offset;
        }
    }
}

/**
 * @brief See \ref compQuadrDistLeafPartition 'compQuadrDistLeafPartition'.
 */
template
<typename T, typename T_calc, dim_t dims>
void compQuadrDistPartition(const std::array<T, dims>& point, const PartitionLeaf<T, dims>& partition_leaf,
                                    std::vector<T>& best_dists, std::vector<point_i_knn_t>& best_idx,
									const point_i_knn_t nr_nns_searches);

/**
 * @brief Recursively traverses the node and calls compQuadrDistLeafPartition on the leaves.
 */
template
<typename T, typename T_calc, dim_t dims>
void compQuadrDistNode(const std::array<T, dims>& point, const Partition<T>& partition,
                                    std::vector<T>& best_dists, std::vector<point_i_knn_t>& best_idx,
                                    const int current_level, const std::array<T, dims>& point_proj, const point_i_knn_t nr_nns_searches);

/**
 * @brief Computes the KNN on the KD-Tree defined by partition_info. Iteratively calls compQuadrDistNode on the main node for each point in points_query.
 *        The resulting quadr. distances and indices of the KNNs will be stored in dist [M, K] in a C-ordering fashion.
 */
template
<typename T, typename T_calc, dim_t dims>
void KDTreeKNNSearch(PartitionInfo<T, dims>& partition_info,
                    const point_i_t nr_query,
                    const std::array<T, dims>* points_query, T * dist, point_i_knn_t* idx, const point_i_knn_t nr_nns_searches);


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2015 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_GEOMETRY_IS_VALID_HPP
#define MAPNIK_GEOMETRY_IS_VALID_HPP

#include <boost/version.hpp>

// only Boost >= 1.58 contains the is_valid function
#if BOOST_VERSION >= 105800

#include <mapnik/geometry.hpp>
#include <mapnik/geometry_adapters.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>
#include <boost/geometry/algorithms/validity_failure_type.hpp>

namespace mapnik { namespace geometry {

namespace detail {

struct geometry_is_valid
{
    using result_type = bool;

    template <typename T>
    result_type operator() (geometry<T> const& geom) const
    {
        return mapnik::util::apply_visitor(*this, geom);
    }

    result_type operator() (geometry_empty const& ) const
    {
        return true;
    }

    template <typename T>
    result_type operator() (geometry_collection<T> const& collection) const
    {
        for (auto const& geom : collection)
        {
            if ( !(*this)(geom)) return false;
        }
        return true;
    }

    template <typename T>
    result_type operator() (point<T> const& pt) const
    {
        return boost::geometry::is_valid(pt);
    }

    template <typename T>
    result_type operator() (line_string<T> const& line) const
    {
        return boost::geometry::is_valid(line);
    }

    template <typename T>
    result_type operator() (polygon<T> const& poly) const
    {
        return boost::geometry::is_valid(poly);
    }

    template <typename T>
    result_type operator() (multi_point<T> const& multi_pt) const
    {
        return boost::geometry::is_valid(multi_pt);
    }

    template <typename T>
    result_type operator() (multi_line_string<T> const& multi_line) const
    {
        return boost::geometry::is_valid(multi_line);
    }

    template <typename T>
    result_type operator() (multi_polygon<T> const& multi_poly) const
    {
        return boost::geometry::is_valid(multi_poly);
    }
};

struct geometry_is_valid_reason
{
    using result_type = bool;

    boost::geometry::validity_failure_type & failure_;

    geometry_is_valid_reason(boost::geometry::validity_failure_type & failure):
        failure_(failure) {}

    template <typename T>
    result_type operator() (geometry<T> const& geom) const
    {
        return mapnik::util::apply_visitor(*this, geom);
    }

    result_type operator() (geometry_empty const& ) const
    {
        failure_ = boost::geometry::no_failure;
        return true;
    }

    template <typename T>
    result_type operator() (geometry_collection<T> const& collection) const
    {
        for (auto const& geom : collection)
        {
            if ( !(*this)(geom)) return false;
        }
        return true;
    }

    template <typename T>
    result_type operator() (point<T> const& pt) const
    {
        return boost::geometry::is_valid(pt, failure_);
    }

    template <typename T>
    result_type operator() (line_string<T> const& line) const
    {
        return boost::geometry::is_valid(line, failure_);
    }

    template <typename T>
    result_type operator() (polygon<T> const& poly) const
    {
        return boost::geometry::is_valid(poly, failure_);
    }

    template <typename T>
    result_type operator() (multi_point<T> const& multi_pt) const
    {
        return boost::geometry::is_valid(multi_pt, failure_);
    }

    template <typename T>
    result_type operator() (multi_line_string<T> const& multi_line) const
    {
        return boost::geometry::is_valid(multi_line, failure_);
    }

    template <typename T>
    result_type operator() (multi_polygon<T> const& multi_poly) const
    {
        return boost::geometry::is_valid(multi_poly, failure_);
    }
};

struct geometry_is_valid_string
{
    using result_type = bool;

    std::string & message_;

    geometry_is_valid_string(std::string & message):
        message_(message) {}

    template <typename T>
    result_type operator() (geometry<T> const& geom) const
    {
        return mapnik::util::apply_visitor(*this, geom);
    }

    result_type operator() (geometry_empty const& ) const
    {
        message_ = "Geometry is valid";
        return true;
    }

    template <typename T>
    result_type operator() (geometry_collection<T> const& collection) const
    {
        for (auto const& geom : collection)
        {
            if ( !(*this)(geom)) return false;
        }
        return true;
    }

    template <typename T>
    result_type operator() (point<T> const& pt) const
    {
        return boost::geometry::is_valid(pt, message_);
    }

    template <typename T>
    result_type operator() (line_string<T> const& line) const
    {
        return boost::geometry::is_valid(line, message_);
    }

    template <typename T>
    result_type operator() (polygon<T> const& poly) const
    {
        return boost::geometry::is_valid(poly, message_);
    }

    template <typename T>
    result_type operator() (multi_point<T> const& multi_pt) const
    {
        return boost::geometry::is_valid(multi_pt, message_);
    }

    template <typename T>
    result_type operator() (multi_line_string<T> const& multi_line) const
    {
        return boost::geometry::is_valid(multi_line, message_);
    }

    template <typename T>
    result_type operator() (multi_polygon<T> const& multi_poly) const
    {
        return boost::geometry::is_valid(multi_poly, message_);
    }
};

}

template <typename T>
inline bool is_valid(T const& geom)
{
    return detail::geometry_is_valid() (geom);
}

template <typename T>
inline bool is_valid(mapnik::geometry::geometry<T> const& geom)
{
    return util::apply_visitor(detail::geometry_is_valid(), geom);
}

template <typename T>
inline bool is_valid(T const& geom, boost::geometry::validity_failure_type & failure)
{
    return detail::geometry_is_valid_reason(failure) (geom);
}

template <typename T>
inline bool is_valid(mapnik::geometry::geometry<T> const& geom,
                     boost::geometry::validity_failure_type & failure)
{
    return util::apply_visitor(detail::geometry_is_valid_reason(failure), geom);
}

template <typename T>
inline bool is_valid(T const& geom, std::string & message)
{
    return detail::geometry_is_valid_string(message) (geom);
}

template <typename T>
inline bool is_valid(mapnik::geometry::geometry<T> const& geom,
                     std::string & message)
{
    return util::apply_visitor(detail::geometry_is_valid_string(message), geom);
}

}}

#endif // BOOST_VERSION >= 1.58
#endif // MAPNIK_GEOMETRY_IS_VALID_HPP