/*
 Copyright (C) 2017 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/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/bond.hpp>
#include <ored/portfolio/builders/bond.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/envelope.hpp>
#include <ored/portfolio/legdata.hpp>
#include <ored/portfolio/schedule.hpp>
#include <ored/utilities/indexparser.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/volatility/swaption/swaptionconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>

using std::string;

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore::data;

// Bond test

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        asof_ = Date(3, Feb, 2016);
        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Yield, "BANK_EUR_LEND")] =
            flatRateYts(0.02);
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] = flatRateYts(0.02);
        defaultCurves_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] = flatRateDcs(0.0);
        // recoveryRates_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] =
        //     Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
        securitySpreads_[make_pair(Market::defaultConfiguration, "Security1")] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
        recoveryRates_[make_pair(Market::defaultConfiguration, "Security1")] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
        // build ibor index
        Handle<IborIndex> hEUR(ore::data::parseIborIndex(
            "EUR-EURIBOR-6M", yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")]));
        iborIndices_[make_pair(Market::defaultConfiguration, "EUR-EURIBOR-6M")] = hEUR;

        // add Eurib 6M fixing
        hEUR->addFixing(Date(1, Feb, 2016), -0.00191);
        hEUR->addFixing(Date(1, Feb, 2017), -0.00191);
        hEUR->addFixing(Date(1, Feb, 2018), -0.00191);
        hEUR->addFixing(Date(1, Feb, 2019), -0.00191);
        hEUR->addFixing(Date(31, Jan, 2019), -0.00191);
        hEUR->addFixing(Date(30, Jan, 2020), -0.00191);
    }

    TestMarket(Real defaultFlatRate) {
        asof_ = Date(3, Feb, 2016);
        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Yield, "BANK_EUR_LEND")] =
            flatRateYts(0.02);
        defaultCurves_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] = flatRateDcs(defaultFlatRate);
        // recoveryRates_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] =
        //     Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
        securitySpreads_[make_pair(Market::defaultConfiguration, "Security1")] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
        recoveryRates_[make_pair(Market::defaultConfiguration, "Security1")] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(0.00));
    }

private:
    Handle<YieldTermStructure> flatRateYts(Real forward) {
        boost::shared_ptr<YieldTermStructure> yts(new FlatForward(0, NullCalendar(), forward, ActualActual()));
        yts->enableExtrapolation();
        return Handle<YieldTermStructure>(yts);
    }
    Handle<DefaultProbabilityTermStructure> flatRateDcs(Real forward) {
        boost::shared_ptr<DefaultProbabilityTermStructure> dcs(new FlatHazardRate(asof_, forward, ActualActual()));
        return Handle<DefaultProbabilityTermStructure>(dcs);
    }
};

struct CommonVars {
    // global data
    string ccy;
    string securityId;
    string creditCurveId;
    string issuerId;
    string referenceCurveId;
    bool isPayer;
    string start;
    string end;
    string issue;
    string fixtenor;
    Calendar cal;
    string calStr;
    string conv;
    string rule;
    Size days;
    string fixDC;
    Real fixedRate;
    string settledays;
    bool isinarrears;
    Real notional;
    vector<Real> notionals;
    vector<Real> spread;

    // utilities
    boost::shared_ptr<ore::data::Bond> makeBond() {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        // build CMSSwap
        LegData fixedLegData(boost::make_shared<FixedLegData>(vector<double>(1, fixedRate)), isPayer, ccy,
                             fixedSchedule, fixDC, notionals);

        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, issue, fixedLegData));
        return bond;
    }

    boost::shared_ptr<ore::data::Bond> makeAmortizingFixedBond(string amortType, Real value, bool underflow) {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        AmortizationData amortizationData(amortType, value, start, end, fixtenor, underflow);
        LegData fixedLegData(boost::make_shared<FixedLegData>(vector<double>(1, fixedRate)), isPayer, ccy,
                             fixedSchedule, fixDC, notionals, vector<string>(), conv, false, false, false, true, "", 0,
                             "", 0, "", {amortizationData});

        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, issue, fixedLegData));
        return bond;
    }

    boost::shared_ptr<ore::data::Bond> makeAmortizingFloatingBond(string amortType, Real value, bool underflow) {
        ScheduleData floatingSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        AmortizationData amortizationData(amortType, value, start, end, fixtenor, underflow);
        LegData floatingLegData(boost::make_shared<FloatingLegData>("EUR-EURIBOR-6M", 2, false, spread), isPayer, ccy,
                                floatingSchedule, fixDC, notionals, vector<string>(), conv, false, false, false, true,
                                "", 0, "", 0, "", {amortizationData});

        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, issue, floatingLegData));
        return bond;
    }

    boost::shared_ptr<ore::data::Bond> makeAmortizingFixedBondWithChangingAmortisation(string amortType1, Real value1,
                                                                                bool underflow1, string end1,
                                                                                string amortType2, Real value2,
                                                                                bool underflow2) {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        AmortizationData amortizationData1(amortType1, value1, start, end1, fixtenor, underflow1);
        AmortizationData amortizationData2(amortType2, value2, end1, end, fixtenor, underflow2);
        LegData fixedLegData(boost::make_shared<FixedLegData>(vector<double>(1, fixedRate)), isPayer, ccy,
                             fixedSchedule, fixDC, notionals, vector<string>(), conv, false, false, false, true, "", 0,
                             "", 0, "", {amortizationData1, amortizationData2});

        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, issue, fixedLegData));
        return bond;
    }

    boost::shared_ptr<ore::data::Bond>
    makeAmortizingFloatingBondWithChangingAmortisation(string amortType1, Real value1, bool underflow1, string end1,
                                                       string amortType2, Real value2, bool underflow2) {
        ScheduleData floatingSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        FloatingLegData floatingLegRateData;
        AmortizationData amortizationData1(amortType1, value1, start, end1, fixtenor, underflow1);
        AmortizationData amortizationData2(amortType2, value2, end1, end, fixtenor, underflow2);
        LegData floatingLegData(boost::make_shared<FloatingLegData>("EUR-EURIBOR-6M", 2, false, spread), isPayer, ccy,
                                floatingSchedule, fixDC, notionals, vector<string>(), conv, false, false, false, true,
                                "", 0, "", 0, "", {amortizationData1, amortizationData2});

        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, issue, floatingLegData));
        return bond;
    }

    boost::shared_ptr<ore::data::Bond> makeZeroBond() {
        Envelope env("CP1");

        boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(
            env, issuerId, creditCurveId, securityId, referenceCurveId, settledays, calStr, notional, end, ccy, issue));
        return bond;
    }

    CommonVars()
        : ccy("EUR"), securityId("Security1"), creditCurveId("CreditCurve_A"), issuerId("CPTY_A"),
          referenceCurveId("BANK_EUR_LEND"), isPayer(false), start("20160203"), end("20210203"), issue("20160203"),
          fixtenor("1Y") {
        cal = TARGET();
        calStr = "TARGET";
        conv = "MF";
        rule = "Forward";
        fixDC = "ACT/ACT";
        fixedRate = 0.05;
        settledays = "2";
        isinarrears = false;
        notional = 10000000;
        notionals.push_back(10000000);
        spread.push_back(0.0);
    }
};

// print details of bond cashflows
void printBondSchedule(const boost::shared_ptr<ore::data::Bond>& b) {
    auto qlInstr = boost::dynamic_pointer_cast<QuantLib::Bond>(b->instrument()->qlInstrument());
    BOOST_REQUIRE(qlInstr != nullptr);
    BOOST_TEST_MESSAGE("Bond NPV=" << qlInstr->NPV() << ", Schedule:");
    Leg l = qlInstr->cashflows();
    BOOST_TEST_MESSAGE(" StartDate    EndDate     Nominal        Rate      Amount");
    for (auto const& c : l) {
        auto cpn = boost::dynamic_pointer_cast<Coupon>(c);
        if (cpn != nullptr) {
            BOOST_TEST_MESSAGE(QuantLib::io::iso_date(cpn->accrualStartDate())
                               << " " << QuantLib::io::iso_date(cpn->accrualEndDate()) << std::setw(12)
                               << cpn->nominal() << std::setw(12) << cpn->rate() << std::setw(12) << cpn->amount());
        } else {
            BOOST_TEST_MESSAGE("           " << QuantLib::io::iso_date(c->date()) << std::setw(12) << " "
                                             << std::setw(12) << " " << std::setw(12) << c->amount());
        }
    }
    BOOST_TEST_MESSAGE("");
}

// check nominal schedule of bond
void checkNominalSchedule(const boost::shared_ptr<ore::data::Bond>& b, const std::vector<Real> notionals) {
    auto qlInstr = boost::dynamic_pointer_cast<QuantLib::Bond>(b->instrument()->qlInstrument());
    BOOST_REQUIRE(qlInstr != nullptr);
    Leg l = qlInstr->cashflows();
    std::vector<Real> bondNotionals;
    for (auto const& c : l) {
        auto cpn = boost::dynamic_pointer_cast<Coupon>(c);
        if (cpn != nullptr) {
            bondNotionals.push_back(cpn->nominal());
        }
    }
    BOOST_REQUIRE_EQUAL(bondNotionals.size(), notionals.size());
    for (Size i = 0; i < notionals.size(); ++i)
        BOOST_CHECK_CLOSE(bondNotionals[i], notionals[i], 1E-4);
}

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(BondTests)

BOOST_AUTO_TEST_CASE(testZeroBond) {
    BOOST_TEST_MESSAGE("Testing Zero Bond...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::Bond> bond = vars.makeZeroBond();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";

    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    bond->build(engineFactory);

    Real npv = bond->instrument()->NPV();
    Real expectedNpv = 9048374.18;

    BOOST_CHECK_CLOSE(npv, expectedNpv, 1.0);
}

BOOST_AUTO_TEST_CASE(testAmortizingBond) {
    BOOST_TEST_MESSAGE("Testing Amortising Bonds...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Date today = Date(30, Jan, 2021);
    Settings::instance().evaluationDate() = today; // market->asofDate();

    CommonVars vars;
    vector<boost::shared_ptr<ore::data::Bond>> bonds;
    boost::shared_ptr<ore::data::Bond> bondFixedAmount = vars.makeAmortizingFixedBond("FixedAmount", 2500000, true);
    bonds.push_back(bondFixedAmount);

    boost::shared_ptr<ore::data::Bond> bondRelativeInitial =
        vars.makeAmortizingFixedBond("RelativeToInitialNotional", 0.25, true);
    bonds.push_back(bondRelativeInitial);

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";

    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    Real npvTol = 0.5;

    for (auto& b : bonds) {
        b->build(engineFactory);

        // output schedule
        printBondSchedule(b);

        Real npv = b->instrument()->NPV();
        Real expectedNpv = 0.0;

        BOOST_CHECK(std::fabs(npv - expectedNpv) < npvTol);
    }

    boost::shared_ptr<ore::data::Bond> bondRelativePrevious =
        vars.makeAmortizingFixedBond("RelativeToPreviousNotional", 0.25, true);
    bondRelativePrevious->build(engineFactory);
    printBondSchedule(bondRelativePrevious);

    boost::shared_ptr<QuantLib::Instrument> inst1 = bondRelativePrevious->instrument()->qlInstrument();
    boost::shared_ptr<QuantLib::Bond> qlBond1 = boost::dynamic_pointer_cast<QuantLib::Bond>(inst1);
    Real expectedNotional = 3164062.5;

    Real notional = qlBond1->notionals()[qlBond1->notionals().size() - 2];

    BOOST_CHECK_CLOSE(notional, expectedNotional, 1);

    boost::shared_ptr<ore::data::Bond> bondFixedAnnuity = vars.makeAmortizingFixedBond("Annuity", 2500000, true);
    bondFixedAnnuity->build(engineFactory);
    printBondSchedule(bondFixedAnnuity);

    boost::shared_ptr<QuantLib::Instrument> inst2 = bondFixedAnnuity->instrument()->qlInstrument();
    boost::shared_ptr<QuantLib::Bond> qlBond2 = boost::dynamic_pointer_cast<QuantLib::Bond>(inst2);
    expectedNotional = 1380908.447;

    notional = qlBond2->notionals()[qlBond2->notionals().size() - 2];

    BOOST_CHECK(std::fabs(notional - expectedNotional) < npvTol);

    boost::shared_ptr<ore::data::Bond> bondFloatingAnnuity = vars.makeAmortizingFloatingBond("Annuity", 2500000, true);
    bondFloatingAnnuity->build(engineFactory);
    printBondSchedule(bondFloatingAnnuity);

    boost::shared_ptr<QuantLib::Instrument> inst3 = bondFloatingAnnuity->instrument()->qlInstrument();
    boost::shared_ptr<QuantLib::Bond> qlBond3 = boost::dynamic_pointer_cast<QuantLib::Bond>(inst3);
    Real expectedAmount = 93.41;

    Real amount = qlBond3->cashflows()[qlBond3->cashflows().size() - 2]->amount();

    BOOST_CHECK(std::fabs(amount - expectedAmount) < npvTol);
}

BOOST_AUTO_TEST_CASE(testAmortizingBondWithChangingAmortisation) {
    BOOST_TEST_MESSAGE("Testing Amortising Bonds with changing amortisation...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Date today = Date(30, Jan, 2021);
    Settings::instance().evaluationDate() = today; // market->asofDate();

    // build engine factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";
    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    // test different amortization combinations
    CommonVars vars;

    // fixed rate bond test cases
    boost::shared_ptr<ore::data::Bond> bond1 = vars.makeAmortizingFixedBondWithChangingAmortisation(
        "FixedAmount", 2500000, true, "05-02-2018", "FixedAmount", 1250000, true);
    bond1->build(engineFactory);
    printBondSchedule(bond1);
    checkNominalSchedule(bond1, {1.0E7, 7.5E6, 6.25E6, 5.0E6, 3.75E6});

    boost::shared_ptr<ore::data::Bond> bond2 = vars.makeAmortizingFixedBondWithChangingAmortisation(
        "FixedAmount", 2500000, true, "05-02-2018", "RelativeToInitialNotional", 0.1, true);
    bond2->build(engineFactory);
    printBondSchedule(bond2);
    checkNominalSchedule(bond2, {1.0E7, 7.5E6, 6.5E6, 5.5E6, 4.5E6});

    boost::shared_ptr<ore::data::Bond> bond3 = vars.makeAmortizingFixedBondWithChangingAmortisation(
        "RelativeToPreviousNotional", 0.1, true, "05-02-2018", "Annuity", 1E6, true);
    bond3->build(engineFactory);
    printBondSchedule(bond3);
    checkNominalSchedule(bond3, {1.0E7, 9.0E6, 8.45247E6, 7.87393E6, 7.26645E6});

    boost::shared_ptr<ore::data::Bond> bond4 = vars.makeAmortizingFixedBondWithChangingAmortisation(
        "Annuity", 1E6, true, "05-02-2018", "RelativeToPreviousNotional", 0.1, true);
    bond4->build(engineFactory);
    printBondSchedule(bond4);
    checkNominalSchedule(bond4, {1.0E7, 9.50012E6, 8.55011E6, 7.6951E6, 6.92559E6});

    // floating rate bond test cases
    boost::shared_ptr<ore::data::Bond> bond5 = vars.makeAmortizingFloatingBondWithChangingAmortisation(
        "FixedAmount", 2500000, true, "05-02-2018", "FixedAmount", 1250000, true);
    bond5->build(engineFactory);
    printBondSchedule(bond5);
    checkNominalSchedule(bond5, {1.0E7, 7.5E6, 6.25E6, 5.0E6, 3.75E6});

    boost::shared_ptr<ore::data::Bond> bond6 = vars.makeAmortizingFloatingBondWithChangingAmortisation(
        "FixedAmount", 2500000, true, "05-02-2018", "RelativeToInitialNotional", 0.1, true);
    bond6->build(engineFactory);
    printBondSchedule(bond6);
    checkNominalSchedule(bond6, {1.0E7, 7.5E6, 6.5E6, 5.5E6, 4.5E6});

    // annuity only allowed in single block setup
    boost::shared_ptr<ore::data::Bond> bond7 = vars.makeAmortizingFloatingBondWithChangingAmortisation(
        "RelativeToPreviousNotional", 0.1, true, "05-02-2018", "Annuity", 1E6, true);
    BOOST_CHECK_THROW(bond7->build(engineFactory), QuantLib::Error);
}

BOOST_AUTO_TEST_CASE(testMultiPhaseBond) {
    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Date today = Date(30, Jan, 2021);
    Settings::instance().evaluationDate() = today; // market->asofDate();

    // build engine factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";
    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    // test multi phase bond
    CommonVars vars;
    ScheduleData schedule1(ScheduleRules("05-02-2016", "05-02-2018", "1Y", "TARGET", "F", "F", "Forward"));
    ScheduleData schedule2(ScheduleRules("05-02-2018", "05-02-2020", "6M", "TARGET", "F", "F", "Forward"));
    auto fixedLegRateData = boost::make_shared<FixedLegData>(vector<double>(1, 0.01));
    LegData legdata1(fixedLegRateData, vars.isPayer, vars.ccy, schedule1, vars.fixDC, vars.notionals);
    LegData legdata2(fixedLegRateData, vars.isPayer, vars.ccy, schedule2, vars.fixDC, vars.notionals);
    Envelope env("CP1");
    boost::shared_ptr<ore::data::Bond> bond(new ore::data::Bond(env, vars.issuerId, vars.creditCurveId, vars.securityId,
                                                                vars.referenceCurveId, vars.settledays, vars.calStr,
                                                                vars.issue, {legdata1, legdata2}));
    bond->build(engineFactory);
    printBondSchedule(bond);
    auto qlInstr = boost::dynamic_pointer_cast<QuantLib::Bond>(bond->instrument()->qlInstrument());
    BOOST_REQUIRE(qlInstr != nullptr);
    // annualy
    BOOST_REQUIRE_EQUAL(qlInstr->cashflows().size(), 7);
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[0]->date(), Date(6, Feb, 2017));
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[1]->date(), Date(5, Feb, 2018));
    // semi annually
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[2]->date(), Date(6, Aug, 2018));
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[3]->date(), Date(5, Feb, 2019));
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[4]->date(), Date(5, Aug, 2019));
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[5]->date(), Date(5, Feb, 2020));
    BOOST_CHECK_EQUAL(qlInstr->cashflows()[6]->date(), Date(5, Feb, 2020));
}

BOOST_AUTO_TEST_CASE(testBondZeroSpreadDefault) {
    BOOST_TEST_MESSAGE("Testing Bond price...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::Bond> bond = vars.makeBond();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";

    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    bond->build(engineFactory);

    Real npv = bond->instrument()->NPV();
    Real expectedNpv = 11403727.39;

    BOOST_CHECK_CLOSE(npv, expectedNpv, 1.0);
}

BOOST_AUTO_TEST_CASE(testBondCompareDefault) {
    BOOST_TEST_MESSAGE("Testing Bond price...");

    // build market
    boost::shared_ptr<Market> market1 = boost::make_shared<TestMarket>(0.0);
    boost::shared_ptr<Market> market2 = boost::make_shared<TestMarket>(0.5);
    boost::shared_ptr<Market> market3 = boost::make_shared<TestMarket>(0.99);
    Settings::instance().evaluationDate() = market1->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::Bond> bond = vars.makeBond();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Bond") = "DiscountedCashflows";
    engineData->engine("Bond") = "DiscountingRiskyBondEngine";
    map<string, string> engineparams;
    engineparams["TimestepPeriod"] = "6M";
    engineData->engineParameters("Bond") = engineparams;

    boost::shared_ptr<EngineFactory> engineFactory1 = boost::make_shared<EngineFactory>(engineData, market1);
    boost::shared_ptr<EngineFactory> engineFactory2 = boost::make_shared<EngineFactory>(engineData, market2);
    boost::shared_ptr<EngineFactory> engineFactory3 = boost::make_shared<EngineFactory>(engineData, market3);

    bond->build(engineFactory1);
    Real npv1 = bond->instrument()->NPV();
    bond->build(engineFactory2);
    Real npv2 = bond->instrument()->NPV();
    bond->build(engineFactory3);
    Real npv3 = bond->instrument()->NPV();

    BOOST_CHECK((npv1 > npv2) && (npv2 > npv3));

    //   BOOST_CHECK_CLOSE(npv, expectedNpv, 1.0);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <stdio.h>      /* printf, scanf, puts, NULL */
// #include <Eigen/Dense>
#include <algorithm>    // std::random_shuffle
#include <chrono>       // std::chrono::system_clock
#include <numeric>
#include "utils.h"
#include "robotGrid.h"

// define constants

// get rid of this hardcoded stuff
const double alphaLenRough = 7.4;
const double betaLenRough = 15;

RobotGrid::RobotGrid(double angStep, double epsilon, int seed)
    : angStep(angStep), epsilon(epsilon), seed(seed)
{
    srand(seed);

    smoothCollisions = 0;
    maxPathSteps = (int)(ceil(1000.0/angStep));
    maxDisplacement = 2*sin(angStep*M_PI/180)*(alphaLenRough+betaLenRough);

    // construct the perturbation list
    for (int ii=-1; ii<2; ii++){
        for (int jj=-1; jj<2; jj++){
            perturbArray.push_back({ii*angStep, jj*angStep});
        }
    }
}

void RobotGrid::addRobot(
    int robotID, std::string holeID, vec3 basePos, vec3 iHat, vec3 jHat,
    vec3 kHat, vec3 dxyz, double alphaLen, double alphaOffDeg,
    double betaOffDeg, double elementHeight, double scaleFac, vec2 metBetaXY,
    vec2 bossBetaXY, vec2 apBetaXY,
    std::array<vec2, 2> collisionSegBetaXY,
    bool hasApogee, double collisionBuffer, bool lefthanded
){
    // ensure robot id doesn't already exist
    if (initialized){
        throw std::runtime_error("RobotGrid is already initialized, no more robots allowed");
    }
    if (robotDict.count(robotID) > 0){
        throw std::runtime_error("Robot ID already exists");
    }
    robotDict[robotID] = std::make_shared<Robot>(
        robotID, holeID, basePos, iHat, jHat,
        kHat, dxyz, alphaLen, alphaOffDeg,
        betaOffDeg, elementHeight, scaleFac, metBetaXY,
        bossBetaXY, apBetaXY,
        collisionSegBetaXY, angStep,
        hasApogee, collisionBuffer, lefthanded
    );
    // robotDict[robotID]->setCollisionBuffer(collisionBuffer);
}

void RobotGrid::addTarget(long targetID, vec3 xyzWok, FiberType fiberType, double priority){
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before adding targets");
    }
    if (targetDict.count(targetID) > 0){
        throw std::runtime_error("Target ID already exists");
    }

    targetDict[targetID] = std::make_shared<Target>(targetID, xyzWok, fiberType, priority);
    // add target to robots and robots to target
    for (auto rPair : robotDict){
        auto r = rPair.second;
        if (isValidAssignment(r->id, targetID)){
            r->validTargetIDs.push_back(targetID);
            targetDict[targetID]->validRobotIDs.push_back(r->id);
        }
    }
}

void RobotGrid::addFiducial(int fiducialID, vec3 xyzWok, double collisionBuffer){
    if (initialized){
        throw std::runtime_error("RobotGrid is already initialized, no more fiducials allowed");
    }
    if (fiducialDict.count(fiducialID) > 0){
        throw std::runtime_error("Fiducial ID already exists");
    }
    fiducialDict[fiducialID] = std::make_shared<Fiducial>(fiducialID, xyzWok, collisionBuffer);
}

void RobotGrid::addGFA(int gfaID, std::array<vec3, 2> collisionSegWokXYZ, double collisionBuffer){
    if (initialized){
        throw std::runtime_error("RobotGrid is already initialized, no more GFAs allowed");
    }
    if (gfaDict.count(gfaID) > 0){
        throw std::runtime_error("GFA ID already exists");
    }
    gfaDict[gfaID] = std::make_shared<GFA>(gfaID, collisionSegWokXYZ, collisionBuffer);
}

void RobotGrid::initGrid(){
    // associate neighbors with fiducials and robots in grid
    double dx, dy, dist, minSep;
    if (initialized){
        throw std::runtime_error("RobotGrid is already initialized, don't do it twice!");
    }
    initialized = true;
    nRobots = robotDict.size();

    for (auto rPair1 : robotDict){
        // add fiducials (potential to collide with)
        auto r1 = rPair1.second;
        for (auto fPair : fiducialDict){
            auto fiducial = fPair.second;
            dx = r1->xPos - fiducial->x;
            dy = r1->yPos - fiducial->y;
            dist = hypot(dx, dy);
            minSep = r1->alphaLen + r1->betaLen + r1->collisionBuffer + fiducial->collisionBuffer;
            if (dist < minSep) {
                r1->addFiducialNeighbor(fiducial->id);
            }
        }
        // add GFAs (potential to collide with)
        for (auto gfaPair : gfaDict){
            auto gfa = gfaPair.second;
            vec3 ppoint = {r1->xPos, r1->yPos, r1->elementHeight};
            auto dist2 = dist3D_Point_to_Segment(
                ppoint,
                gfa->collisionSegWokXYZ[0],
                gfa->collisionSegWokXYZ[1]
            );
            auto dist = sqrt(dist2);
            minSep = r1->alphaLen + r1->betaLen + r1->collisionBuffer + gfa->collisionBuffer;
            // std::cout << "dist2 " << dist2 << " dist " << dist << " minSep " << minSep <<std::endl;
            if (dist < minSep) {
                // std::cout << "adding gfa " << gfa->id << " as neighbor to robot " << r1->id << std::endl;
                r1->addGFANeighbor(gfa->id);
            }
        }
        // initialize all to alpha beta 0,0
        r1->setAlphaBeta(0,0);
        // int r2Ind = -1;
        for (auto rPair2 : robotDict){
            auto r2 = rPair2.second;
            // r2Ind++;
            // add neighbors (potential to collide with)
            if (r1->id==r2->id){
                continue;
            }
            dx = r1->xPos - r2->xPos;
            dy = r1->yPos - r2->yPos;
            dist = hypot(dx, dy);
            minSep = r1->alphaLen + r1->betaLen + r1->collisionBuffer +
                     r2->alphaLen + r2->betaLen + r2->collisionBuffer;
            if (dist < minSep){
                // these robots are neighbors
                r1->addRobotNeighbor(r2->id);
            }
        }
    }
}

std::shared_ptr<Robot> RobotGrid::getRobot(int robotID){
    return robotDict[robotID];
}

void RobotGrid::setCollisionBuffer(double newBuffer){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->setCollisionBuffer(newBuffer);
    }
}

void RobotGrid::shrinkCollisionBuffer(double absShrink){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        double oldBuffer = r->collisionBuffer;
        r->setCollisionBuffer(oldBuffer - absShrink);
    }
}

void RobotGrid::growCollisionBuffer(double absGrow){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        double oldBuffer = r->collisionBuffer;
        r->setCollisionBuffer(oldBuffer + absGrow);
    }
}

void RobotGrid::decollideGrid(){
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before decollideGrid");
    }
    for (int ii=0; ii<1000; ii++){
        // std::cout << "n collisions " << getNCollisions() << std::endl;
        if (!getNCollisions()){
            break;
        }

        for (auto rPair : robotDict){
            auto robotID = rPair.first;
            // if robot is offline, don't try to decollide
            if (rPair.second->isOffline){
                continue;
            }
            if (isCollided(robotID)){
                decollideRobot(robotID);
            }
        }
    }

    if (getNCollisions()){
        throw std::runtime_error("Unable to decollide this pathologic grid!!!");
    }

}

void RobotGrid::smoothPaths(int points){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->smoothVelocity(points);
    }
}

void RobotGrid::simplifyPaths(){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->simplifyPath(epsilon);
    }
}

void RobotGrid::verifySmoothed(){
    smoothCollisions = 0;
    for (int ii = 0; ii < nSteps; ii++){
        for (auto rPair : robotDict){
            auto r = rPair.second;
            r->setAlphaBeta(r->interpSimplifiedAlphaPath[ii][1], r->interpSimplifiedBetaPath[ii][1]);
            // std::cout << " robot id " << r.id << std::endl;
        }
        for (auto rPair : robotDict){
            auto r = rPair.second;
            if (isCollided(r->id)){
                smoothCollidedRobots.insert(r->id);
            }
            // std::cout << " robot id " << r.id << std::endl;
        }
        smoothCollisions += getNCollisions();
    }
}

int RobotGrid::getNCollisions(){
    // return number of collisions found
    int nCollide = 0;
    for (auto rPair : robotDict){
        auto rId = rPair.first;
        if (isCollided(rId)) {
            nCollide++;
        }
    }
    return nCollide;
}

void RobotGrid::clearPaths(){
    for (auto rPair : robotDict){
        auto r = rPair.second;
        // clear any existing path
        r->alphaPath.clear();
        r->betaPath.clear();
        r->simplifiedAlphaPath.clear();
        r->simplifiedBetaPath.clear(); // sparse
        r->interpSimplifiedAlphaPath.clear();
        r->interpSimplifiedBetaPath.clear(); // dense
        r->smoothedAlphaPath.clear();
        r->smoothedBetaPath.clear();
        r->smoothAlphaVel.clear();
        r->smoothBetaVel.clear();
        r->interpAlphaX.clear();
        r->interpAlphaY.clear();
        r->interpBetaX.clear();
        r->interpBetaY.clear(); // smoothed
        r->roughAlphaX.clear();
        r->roughAlphaY.clear();
        r->roughBetaX.clear();
        r->roughBetaY.clear();
        r->scoreVec.clear();
        // r->onTargetVec.clear();
    }

}

void RobotGrid::pathGenMDP(double setGreed, double setPhobia){
    // path gen 2 steps towards alpha beta target
    // move greed and phobia to constructor?
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before pathGen");
    }
    for (auto rPair : robotDict){
        auto r = rPair.second;
        // verify that a target alpha beta has been set
        if (!r->hasDestinationAlphaBeta){
            throw std::runtime_error("One or more robots have not received target alpha/beta");
        }
    }

    greed = setGreed;
    phobia = setPhobia;
    algType = MDP;
    clearPaths();
    didFail = true;
    int ii;
    std::vector<int> robotIDs;
    for (auto rPair : robotDict){
        robotIDs.push_back(rPair.first);
    }

    for (ii=0; ii<maxPathSteps; ii++){
        std::random_shuffle(robotIDs.begin(), robotIDs.end());
        bool allAtTarget = true;
        for (auto robotID : robotIDs){
            auto r = robotDict[robotID];

            stepMDP(r, ii);
            r->scoreVec.push_back(r->score());
            if (r->score()!=0) {
                // could just check the last elemet in onTargetVec? same thing.
                // or use robot->score
                allAtTarget = false;
            }
        }

        if (allAtTarget){
            didFail = false;
            break;
        }
    }

    nSteps = ii+1;
}

void RobotGrid::pathGenGreedy(bool stopIfDeadlock, bool ignoreInitialCollisions){
    // path gen greedy steps towards alpha beta target
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before pathGen");
    }
    if (getNCollisions() > 0 && !ignoreInitialCollisions){
        throw std::runtime_error("Grid is Kaiju-collided, cannot generate paths");
    }

    for (auto rPair : robotDict){
        auto r = rPair.second;
        // verify that a target alpha beta has been set
        if (!r->hasDestinationAlphaBeta){
            throw std::runtime_error("One or more robots have not received target alpha/beta");
        }
    }
    clearPaths();
    didFail = true;
    greed = 1;
    phobia = 0;
    algType = Greedy;
    int ii;

    for (ii=0; ii<maxPathSteps; ii++){

        bool keepGoing;
        bool allAtTarget = true;
        if (stopIfDeadlock){
            keepGoing = false;
        }
        else {
            keepGoing = true;
        }

        for (auto rPair : robotDict){
            auto r = rPair.second;

            stepGreedy(r, ii);
            double robotScore = r->score();
            r->scoreVec.push_back(robotScore);
            if (robotScore!=0) {

                allAtTarget = false;

                // inspect the score vec, if it is changing
                // if it's value has decreased over the last
                // 10 steps, keep going
                if (stopIfDeadlock){
                    if (ii < 12){
                        keepGoing = true;
                    }
                    else {
                        double historicScore = r->scoreVec[ii-10];
                        if (robotScore/historicScore < 0.99){
                            keepGoing = true;
                        }
                    }
                }
            }

        }

        if (allAtTarget){
            // std::cout << "all at target" << std::endl;
            didFail = false;
            break;
        }

        if (!keepGoing){
            // std::cout << "stopped due to deadlock" << std::endl;
            break;
        }

        // next check whether or not we're deadlocked

    }

    nSteps = ii+1;
}

void RobotGrid::pathGenExplode(double deg2move){

    // path gen 2 steps towards alpha beta target
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before pathGen");
    }

    clearPaths();
    int ii;
    double steps2move = deg2move / angStep;
    for (ii=0; ii<steps2move; ii++){

        for (auto rPair : robotDict){
            auto r = rPair.second;

            stepDecollide(r, ii);
        }

    }
    // hack in destinations for all robots
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->setDestinationAlphaBeta(r->alpha, r->beta);
    }

    nSteps = ii;
    didFail = false;
}

void RobotGrid::pathGenExplodeOne(double deg2move, int robotID){

    // path gen 2 steps towards alpha beta target
    if (!initialized){
        throw std::runtime_error("Initialize RobotGrid before pathGen");
    }

    // hack in destinations for all robots
    // so robots will not move (except for the desired robot)
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->setDestinationAlphaBeta(r->alpha, r->beta);
    }

    clearPaths();
    int ii;
    double steps2move = deg2move / angStep;
    for (ii=0; ii<steps2move; ii++){

        for (auto rPair : robotDict){
            auto r = rPair.second;
            if (rPair.first == robotID){
                stepDecollide(r, ii);
            }
            else {
                // doesn't actually move because destinations
                // have been set to current positions
                // this just fills out the arrays
                stepGreedy(r, ii);
            }
        }
    }

    // hack in destination for single robot that moved

    auto r = robotDict.at(robotID);
    // auto rAlpha = r->alpha;
    // auto rBeta = r->beta;
    r->setDestinationAlphaBeta(r->alpha, r->beta);
    nSteps = ii;
    didFail = false;

}

void RobotGrid::clearTargetDict(){
    targetDict.clear(); // does clear destroy the shared_ptrs?
    // clear all robot target lists
    for (auto rPair : robotDict){
        auto r = rPair.second;
        r->validTargetIDs.clear();
        r->clearAssignment();
    }
}

std::vector<int> RobotGrid::unassignedRobots(){
    // return the ids of unassigned robots
    std::vector<int> idleRobotIDs;
    for (auto rPair : robotDict){
        auto robot = rPair.second;
        if (!robot->isAssigned()){
            idleRobotIDs.push_back(robot->id);
        }
    }
    return idleRobotIDs;
}

std::vector<int> RobotGrid::targetlessRobots(){
    // return the ids of robots that can reach no targets in targetDict
    std::vector<int> idleRobotIDs;
    for (auto rPair : robotDict){
        auto robot = rPair.second;
        if (robot->validTargetIDs.size()==0){
            idleRobotIDs.push_back(robot->id);
        }
    }
    return idleRobotIDs;
}

std::vector<long> RobotGrid::unreachableTargets(){
    // return list of targetIDs that are not reachable
    // by any robot
    if (targetDict.size()==0){
        throw std::runtime_error("unreachableTargets() failure, target list not yet set");
    }
    std::vector<long> badTargs;
    for (auto tPair : targetDict){
        auto targ = tPair.second;
        if (targ->validRobotIDs.size()==0){
            badTargs.push_back(targ->id);
        }
    }
    return badTargs;
}

std::vector<long> RobotGrid::assignedTargets(){
    // return list of targetIDs that have been assigned
    // to robots
    std::vector<long> assignedTargIDs;

    for (auto tPair : targetDict){
        auto t = tPair.second;
        if (t->isAssigned()){
            assignedTargIDs.push_back(t->id);
        }
    }

    return assignedTargIDs;
}

void RobotGrid::unassignTarget(long targID){
    // clear the the target assignment, and the
    // robot to which it's assigned
    auto target = targetDict[targID];
    if (!target->isAssigned()){
        // do nothing, target isnt assigned
        return;
    }
    auto robot = robotDict[target->assignedRobotID];
    target->clearAssignment();
    robot->clearAssignment();
}

void RobotGrid::unassignRobot(int robotID){
    // clear the the target assignment, and the
    // robot to which it's assigned
    auto robot = robotDict[robotID];
    if (!robot->isAssigned()){
        // do nothing, target isnt assigned
        return;
    }
    auto target = targetDict[robot->assignedTargetID];
    target->clearAssignment();
    robot->clearAssignment();
}

void RobotGrid::assignRobot2Target(int robotID, long targetID){
    // releases robot's previous target if present
    // releases target's previous robot if present
    auto robot = robotDict[robotID];
    auto target = targetDict[targetID];

    int ii = std::count(robot->validTargetIDs.begin(), robot->validTargetIDs.end(), targetID);
    if (ii == 0){
        throw std::runtime_error("target not valid for robot");
    }
    unassignRobot(robotID);
    unassignTarget(targetID);
    target->assignRobot(robotID);
    robot->assignTarget(targetID);
    auto ab = robot->alphaBetaFromWokXYZ(target->xyzWok, target->fiberType);
    robot->setAlphaBeta(ab[0], ab[1]);
}

bool RobotGrid::isValidAssignment(int robotID, long targetID){
    auto robot = robotDict[robotID];
    auto target = targetDict[targetID];

    if (target->fiberType == ApogeeFiber and !robot->hasApogee){
        return false;
    }

    auto ab = robot->alphaBetaFromWokXYZ(target->xyzWok, target->fiberType);
    // check alpha beta valid
    if (std::isnan(ab[0]) or std::isnan(ab[1])){
        return false;
    }

    // save current alpha beta
    double savedAlpha = robot->alpha;
    double savedBeta = robot->beta;
    robot->setAlphaBeta(ab[0], ab[1]);
    bool returnValue = true;

    auto collidedFiducials = fiducialColliders(robotID);
    if (collidedFiducials.size() != 0){
        // this target interferes with a fiducial which is immobile
        robot->setAlphaBeta(savedAlpha, savedBeta);
        returnValue = false;
    }
    auto collidedGFAs = gfaColliders(robotID);
    if (collidedGFAs.size() != 0){
        // this target interferes with a GFA which is immobile
        robot->setAlphaBeta(savedAlpha, savedBeta);
        returnValue = false;
    }
    // reset alpha beta
    robot->setAlphaBeta(savedAlpha, savedBeta);
    return returnValue;
}

bool RobotGrid::isCollided(int robotID){
    auto robotsColliding = robotColliders(robotID);
    if (robotsColliding.size() != 0){
        return true;
    }
    auto fiducialsColliding = fiducialColliders(robotID);
    if (fiducialsColliding.size() != 0){
        return true;
    }
    auto gfasColliding = gfaColliders(robotID);
    if (gfasColliding.size() != 0){
        return true;
    }
    return false;
}

std::tuple<bool, bool, bool, std::vector<int>> RobotGrid::isCollidedWithAssigned(int robotID){
    bool collided, fiducial_collided, gfa_collided;
    std::vector<int> assignedRobotsColliding;
    auto robotsColliding = robotColliders(robotID);
    if (robotsColliding.size() != 0){
        for (auto robotColliding : robotsColliding) {
            if(robotDict[robotColliding]->isAssigned()) {
	           assignedRobotsColliding.push_back(robotColliding);
            }
        }
    }
    auto fiducialsColliding = fiducialColliders(robotID);
    auto gfasColliding = gfaColliders(robotID);

    fiducial_collided = (fiducialsColliding.size() != 0);
    gfa_collided = (gfasColliding.size() != 0);

    collided = (fiducial_collided || gfa_collided || (assignedRobotsColliding.size() != 0));

    return std::make_tuple(collided, fiducial_collided, gfa_collided, assignedRobotsColliding);
}

void RobotGrid::homeRobot(int robotID){
    unassignRobot(robotID);
    auto robot = robotDict[robotID];
		robot->setAlphaBeta(0., 180.);
}

std::tuple<bool, bool, bool, std::vector<int>> RobotGrid::wouldCollideWithAssigned(int robotID, long targID){
    long currentTargetID;
    int currentRobotID;
    std::tuple<bool, bool, bool, std::vector<int>> result;

    auto robot = robotDict[robotID];
    if(robot->isAssigned())
    currentTargetID = robot->assignedTargetID;
    else
    currentTargetID = -1;

    currentRobotID = targetDict[targID]->assignedRobotID;

    assignRobot2Target(robotID, targID);
    result = isCollidedWithAssigned(robotID);

    if(currentTargetID >= 0)
    assignRobot2Target(robotID, currentTargetID);
    else
    unassignRobot(robotID);

    if(currentRobotID >= 0)
    assignRobot2Target(currentRobotID, targID);

    return result;
    }

bool RobotGrid::neighborEncroachment(std::shared_ptr<Robot> robot1){
    // score, separation2
    // look ahead and see robots getting close
    double dist2;

    // turn off neighbor encroachment if robot is disables
    if (robot1->isOffline){
        return false;
    }

    // check collisions with neighboring robots
    for (auto otherRobotID : robot1->robotNeighbors){
        auto robot2 = robotDict[otherRobotID];
        // squared distance returned
        double minDist = robot1->collisionBuffer + robot2->collisionBuffer + 3*maxDisplacement;
        dist2 = dist3D_Segment_to_Segment(
                robot2->collisionSegWokXYZ[0], robot2->collisionSegWokXYZ[1],
                robot1->collisionSegWokXYZ[0], robot1->collisionSegWokXYZ[1]
            );
        if (dist2 < (minDist*minDist)){

            return true;
        }
    }
    return false;
}

std::vector<int> RobotGrid::robotColliders(int robotID){

    std::vector<int> collidingNeighbors;
    double dist2, collideDist2, dist;
    // check collisions with neighboring robots
    auto robot1 = robotDict[robotID];

    for (auto otherRobotID : robot1->robotNeighbors){
        auto robot2 = robotDict[otherRobotID];
        // squared distance returned
        dist2 = dist3D_Segment_to_Segment(
                robot2->collisionSegWokXYZ[0], robot2->collisionSegWokXYZ[1],
                robot1->collisionSegWokXYZ[0], robot1->collisionSegWokXYZ[1]
            );
        collideDist2 = (robot1->collisionBuffer + robot2->collisionBuffer + maxDisplacement)
                       * (robot1->collisionBuffer + robot2->collisionBuffer + maxDisplacement);
        // dist = sqrt(dist2);
        // collideDist2 = (2*collisionBuffer)*(2*collisionBuffer);
        if (dist2 < collideDist2){
            // std::cout << "dist " << dist2 - collide_dist_squared << std::endl;
            collidingNeighbors.push_back(robot2->id);
        }

    }
    return collidingNeighbors;
}

std::vector<int> RobotGrid::fiducialColliders(int robotID){

    std::vector<int> collidingNeighbors;
    auto robot = robotDict[robotID];
    double dist2, collideDist2;

    for (auto fiducialID : robot->fiducialNeighbors){
        auto fiducial = fiducialDict[fiducialID];
        // squared distance
        dist2 = dist3D_Point_to_Segment(
                fiducial->xyzWok, robot->collisionSegWokXYZ[0],
                robot->collisionSegWokXYZ[1]
                );
        collideDist2 =  (robot->collisionBuffer+fiducial->collisionBuffer) *
                        (robot->collisionBuffer+fiducial->collisionBuffer);

        if (dist2 < collideDist2){
            collidingNeighbors.push_back(fiducial->id);
        }
    }
    return collidingNeighbors;

}

std::vector<int> RobotGrid::gfaColliders(int robotID){

    std::vector<int> collidingNeighbors;
    auto robot = robotDict[robotID];
    double dist2, collideDist2;

    for (auto gfaID : robot->gfaNeighbors){
        auto gfa = gfaDict[gfaID];
        dist2 = dist3D_Segment_to_Segment(
                gfa->collisionSegWokXYZ[0], gfa->collisionSegWokXYZ[1],
                robot->collisionSegWokXYZ[0], robot->collisionSegWokXYZ[1]
            );

        collideDist2 =  (robot->collisionBuffer+gfa->collisionBuffer) *
                        (robot->collisionBuffer+gfa->collisionBuffer);

        if (dist2 < collideDist2){
            collidingNeighbors.push_back(gfa->id);
        }
    }
    return collidingNeighbors;

}

std::vector<int> RobotGrid::getCollidedRobotRobotList(){
    std::vector<int> robotIDList;
    for (auto rPair : robotDict){
        auto robotID = rPair.first;
        auto colliders = robotColliders(robotID);
        if (!colliders.empty()){
            robotIDList.push_back(robotID);
        }
    }
    return robotIDList;
}

std::vector<int> RobotGrid::getCollidedRobotFiducialList(){
    std::vector<int> robotIDList;
    for (auto rPair : robotDict){
        auto robotID = rPair.first;
        auto colliders = fiducialColliders(robotID);
        if (!colliders.empty()){
            robotIDList.push_back(robotID);
        }
    }
    return robotIDList;

}

std::vector<int> RobotGrid::getCollidedRobotGFAList(){
    std::vector<int> robotIDList;
    for (auto rPair : robotDict){
        auto robotID = rPair.first;
        auto colliders = gfaColliders(robotID);
        if (!colliders.empty()){
            robotIDList.push_back(robotID);
        }
    }
    return robotIDList;
}

std::vector<int> RobotGrid::getCollidedRobotList(){
    std::vector<int> robotIDList;
    for (auto rPair : robotDict){
        auto robotID = rPair.first;
        if (isCollided(robotID)){
            robotIDList.push_back(robotID);
        }
    }
    return robotIDList;
}

bool RobotGrid::throwAway(int robotID){
    // return true if worked
    // robot gets new alpha beta position
    // if return is false robot is unchanged
    auto robot = robotDict[robotID];
    auto currAlpha = robot->alpha;
    auto currBeta = robot->beta;
    // attempt to keep beta as small as possible
    // loop over it first
    for (int ii=0; ii<100000; ii++){
        robot->setXYUniform();
        if (!isCollided(robotID)){
            return true;
        }
    }
    // couldn't find replacement!
    robot->setAlphaBeta(currAlpha, currBeta);
    return false;
}

void RobotGrid::decollideRobot(int robotID){
    // warning, if decollide doesn't work
    // no error is raised!!!
    // remove assigned target if present
    unassignRobot(robotID);
    auto robot = robotDict[robotID];
    for (int ii=0; ii<1000; ii++){
        robot->setXYUniform();
        // nDecollide ++;
        if (!isCollided(robotID)){
            // std::cout << "decollide successful " << std::endl;
            break;
        }
    }

    // may want to fail hard if we still collided?
    // if (isCollided(robot)){
    //     throw std::runtime_error("Unable do decollide robot!!!");
    // }
}

std::vector<int> RobotGrid::deadlockedRobots(){
    std::vector<int> deadlockedRobotIDs;

    for (auto rPair : robotDict){
        auto robot = rPair.second;
        if (algType == Fold){
            if (robot->alpha != 0 or robot->beta != 180){
                deadlockedRobotIDs.push_back(robot->id);
            }
        }
        else {
            if (robot->score() != 0){
                deadlockedRobotIDs.push_back(robot->id);
            }
            else if (neighborEncroachment(robot)){
                deadlockedRobotIDs.push_back(robot->id);
            }
        }
    }
    return deadlockedRobotIDs;
}

// handle things near limits of robot motion
vec2 handleLimits(double currAlpha, double currBeta, double nextAlpha, double nextBeta){
    vec2 nextAlphaBeta;

    // handle alpha lims
    // if robots currently beyond limits
    // don't allow them to go further beyond limits
    if (currAlpha < 0 && nextAlpha < currAlpha){
        nextAlpha = currAlpha; // don't move further the wrong direction
    }

    else if (currAlpha > 360 && nextAlpha > currAlpha){
        nextAlpha = currAlpha;
    }
    // robots currently in bounds
    else if (nextAlpha > 360){
        nextAlpha = 360;
    }
    else if (nextAlpha < 0){
        nextAlpha = 0;
    }

    if (currBeta < 0 && nextBeta < currBeta){
        nextBeta = currBeta; // don't move further the wrong direction
    }

    else if (currBeta > 360 && nextBeta > currBeta){
        nextBeta = currBeta;
    }
    // robots currently in bounds
    else if (nextBeta > 360){
        nextBeta = 360;
    }
    else if (nextBeta < 0){
        nextBeta = 0;
    }

    nextAlphaBeta[0] = nextAlpha;
    nextAlphaBeta[1] = nextBeta;
    return nextAlphaBeta;

    // if robots moving beyond limits, don't let them do it

}

void RobotGrid::stepDecollide(std::shared_ptr<Robot> robot, int stepNum){
    double currAlpha = robot->alpha;
    double currBeta = robot->beta;
    vec2 alphaPathPoint;
    vec2 betaPathPoint;
    vec2 temp;
    alphaPathPoint[0] = stepNum;
    betaPathPoint[0] = stepNum;

    double maxDist = -1e9; // to be maximized
    double bestAlpha = 0;  // solved in loop
    double bestBeta = 0;
    // this is for keeping track of last step
    // only updates if robot hasn't reached fold
    robot->lastStepNum = stepNum;

    for (auto dAlphaBeta : perturbArray){
        double nextAlpha = currAlpha + dAlphaBeta[0];
        double nextBeta = currBeta + dAlphaBeta[1];

        vec2 nextAlphaBeta = handleLimits(currAlpha, currBeta, nextAlpha, nextBeta);

        nextAlpha = nextAlphaBeta[0];
        nextBeta =nextAlphaBeta[1];

        robot->setAlphaBeta(nextAlpha, nextBeta);

        auto collideDist = minCollideDist(robot->id);
        if (collideDist > maxDist){
            maxDist = collideDist;
            bestAlpha = nextAlpha;
            bestBeta = nextBeta;
        }
    }

    robot->setAlphaBeta(bestAlpha, bestBeta);
    alphaPathPoint[1] = bestAlpha;
    betaPathPoint[1] = bestBeta;
    robot->alphaPath.push_back(alphaPathPoint);
    robot->betaPath.push_back(betaPathPoint);

    // add alpha/beta xy points

    temp[0] = stepNum;
    temp[1] = robot->collisionSegWokXYZ[0][0]; // xAlphaEnd

    robot->roughAlphaX.push_back(temp);

    temp[1] = robot->collisionSegWokXYZ[0][1]; // yAlphaEnd
    robot->roughAlphaY.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[0]; // xBetaEnd
    robot->roughBetaX.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[1]; // yBetaEnd
    robot->roughBetaY.push_back(temp);

    return;
}

double RobotGrid::minCollideDist(int robotID){

    double dist2, dist;
    // check collisions with neighboring robots
    double minDist = 1e9;  // to be minimized
    auto robot1 = robotDict[robotID];

    // search through robot neighbors for min dist
    for (auto otherRobotID : robot1->robotNeighbors){
        auto robot2 = robotDict[otherRobotID];
        // squared distance returned
        dist2 = dist3D_Segment_to_Segment(
                robot2->collisionSegWokXYZ[0], robot2->collisionSegWokXYZ[1],
                robot1->collisionSegWokXYZ[0], robot1->collisionSegWokXYZ[1]
            );
        dist = sqrt(dist2) - robot2->collisionBuffer - robot1->collisionBuffer;
        if (dist < minDist){
            minDist = dist;
        }
    }

    for (auto fiducialID : robot1->fiducialNeighbors){
        auto fiducial = fiducialDict[fiducialID];
        // squared distance
        dist2 = dist3D_Point_to_Segment(
                fiducial->xyzWok, robot1->collisionSegWokXYZ[0],
                robot1->collisionSegWokXYZ[1]
                );
        dist = sqrt(dist2) - fiducial->collisionBuffer - robot1->collisionBuffer;
        if (dist < minDist){
            minDist = dist;
        }
    }

    for (auto gfaID : robot1->gfaNeighbors){
        auto gfa = gfaDict[gfaID];
        // squared distance
        dist2 = dist3D_Segment_to_Segment(
                gfa->collisionSegWokXYZ[0], gfa->collisionSegWokXYZ[1],
                robot1->collisionSegWokXYZ[0], robot1->collisionSegWokXYZ[1]
                );
        dist = sqrt(dist2) - gfa->collisionBuffer - robot1->collisionBuffer;
        if (dist < minDist){
            minDist = dist;
        }
    }

    return minDist;
}

void RobotGrid::stepGreedy(std::shared_ptr<Robot> robot, int stepNum){

    double score;
    double currAlpha = robot->alpha;
    double currBeta = robot->beta;
    double bestAlpha, bestBeta, bestScore, nextAlpha, nextBeta;
    bestAlpha = currAlpha;
    bestBeta = currBeta;
    bestScore = 1e16; // to be minimized

    vec2 alphaPathPoint;
    vec2 betaPathPoint;
    vec2 temp;

    alphaPathPoint[0] = stepNum;
    betaPathPoint[0] = stepNum;

    if (stepNum==0 || robot->score()==0){
        // at target or on first step, don't move
        alphaPathPoint[1] = currAlpha;
        betaPathPoint[1] = currBeta;
        robot->alphaPath.push_back(alphaPathPoint);
        robot->betaPath.push_back(betaPathPoint);

        // note make collision segment just two points

        temp[0] = stepNum;
        temp[1] = robot->collisionSegWokXYZ[0][0]; // xAlphaEnd
        robot->roughAlphaX.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ[0][1]; // yAlphaEnd
        robot->roughAlphaY.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ.back()[0]; // xBetaEnd
        robot->roughBetaX.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ.back()[1]; // yBetaEnd
        robot->roughBetaY.push_back(temp);

        return;
    }

    robot->lastStepNum = stepNum;

    std::random_shuffle(perturbArray.begin(), perturbArray.end());
    // check all move combinations for each axis
    for (auto dAlphaBeta : perturbArray){
        nextAlpha = currAlpha + dAlphaBeta[0];
        nextBeta = currBeta + dAlphaBeta[1];
        // careful not to overshoot
        if (currAlpha > robot->destinationAlpha and nextAlpha <= robot->destinationAlpha){
            nextAlpha = robot->destinationAlpha;
        }
        if (currAlpha < robot->destinationAlpha and nextAlpha >= robot->destinationAlpha){
            nextAlpha = robot->destinationAlpha;
        }
        if (currBeta > robot->destinationBeta and nextBeta <= robot->destinationBeta){
            nextBeta = robot->destinationBeta;
        }
        if (currBeta < robot->destinationBeta and nextBeta >= robot->destinationBeta){
            nextBeta = robot->destinationBeta;
        }
        // handle limits of travel
        // can probably ditch this as target
        // must be in range anyways
        vec2 nextAlphaBeta = handleLimits(currAlpha, currBeta, nextAlpha, nextBeta);
        nextAlpha = nextAlphaBeta[0];
        nextBeta =nextAlphaBeta[1];

        robot->setAlphaBeta(nextAlpha, nextBeta);
        score = robot->score();

        if (!isCollided(robot->id)){
            if (score < bestScore){
                bestScore = score;
                bestAlpha = nextAlpha;
                bestBeta = nextBeta;

            }

            else if (score == bestScore and randomSample() >= 0.5){
                // flip a coin to see whether to accept
                bestScore = score;
                bestAlpha = nextAlpha;
                bestBeta = nextBeta;
            }
        }
    } // end loop over perturbations

    // set alpha beta to best found option
    robot->setAlphaBeta(bestAlpha, bestBeta);
    alphaPathPoint[1] = bestAlpha;
    betaPathPoint[1] = bestBeta;
    robot->alphaPath.push_back(alphaPathPoint);
    robot->betaPath.push_back(betaPathPoint);

    temp[0] = stepNum;
    temp[1] = robot->collisionSegWokXYZ[0][0]; // xAlphaEnd
    robot->roughAlphaX.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ[0][1]; // yAlphaEnd
    robot->roughAlphaY.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[0]; // xBetaEnd
    robot->roughBetaX.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[1]; // yBetaEnd
    robot->roughBetaY.push_back(temp);

}

void RobotGrid::stepMDP(std::shared_ptr<Robot> robot, int stepNum){

    double score, dist2, localEnergy;
    double nextAlpha, nextBeta;
    bool doPhobia;
    double currAlpha = robot->alpha;
    double currBeta = robot->beta;
    double bestAlpha, bestBeta, bestScore;
    bestAlpha = currAlpha;
    bestBeta = currBeta;
    bestScore = 1e16; // to be minimized

    vec2 alphaPathPoint;
    vec2 betaPathPoint;
    vec2 temp;

    alphaPathPoint[0] = stepNum;
    betaPathPoint[0] = stepNum;

    if (stepNum==0 || (robot->score()==0 && !neighborEncroachment(robot))){
        // either at first step, or done folding no one knocking don't move
        alphaPathPoint[1] = currAlpha;
        betaPathPoint[1] = currBeta;
        robot->alphaPath.push_back(alphaPathPoint);
        robot->betaPath.push_back(betaPathPoint);

        // note make collision segment just two points

        temp[0] = stepNum;
        temp[1] = robot->collisionSegWokXYZ[0][0]; // xAlphaEnd
        robot->roughAlphaX.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ[0][1]; // yAlphaEnd
        robot->roughAlphaY.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ.back()[0]; // xBetaEnd
        robot->roughBetaX.push_back(temp);
        temp[1] = robot->collisionSegWokXYZ.back()[1]; // yBetaEnd
        robot->roughBetaY.push_back(temp);

        return;
    }

    robot->lastStepNum = stepNum;
    // begin looping over all possible moves
    // shuffle move options to ensure they are visited
    // in no particular order
    std::random_shuffle(perturbArray.begin(), perturbArray.end());

    // decide whether we're minimizing phobia
    // or minimizing score

    doPhobia = randomSample() < phobia;

    for (auto dAlphaBeta : perturbArray){
        nextAlpha = currAlpha + dAlphaBeta[0];
        nextBeta = currBeta + dAlphaBeta[1];
        // careful not to overshoot
        if (currAlpha > robot->destinationAlpha and nextAlpha <= robot->destinationAlpha){
            nextAlpha = robot->destinationAlpha;
        }
        if (currAlpha < robot->destinationAlpha and nextAlpha >= robot->destinationAlpha){
            nextAlpha = robot->destinationAlpha;
        }
        if (currBeta > robot->destinationBeta and nextBeta <= robot->destinationBeta){
            nextBeta = robot->destinationBeta;
        }
        if (currBeta < robot->destinationBeta and nextBeta >= robot->destinationBeta){
            nextBeta = robot->destinationBeta;
        }
        // handle limits of travel
        vec2 nextAlphaBeta = handleLimits(currAlpha, currBeta, nextAlpha, nextBeta);
        nextAlpha = nextAlphaBeta[0];
        nextBeta =nextAlphaBeta[1];

        robot->setAlphaBeta(nextAlpha, nextBeta);
        // score is min possible steps till goal
        localEnergy = 0;
        bool didCollide = false;

        // compute robot's local energy, and check for collision
        for (auto otherRobotID : robot->robotNeighbors){
            auto otherRobot = robotDict[otherRobotID];
            dist2 = dist3D_Segment_to_Segment(
                otherRobot->collisionSegWokXYZ[0], otherRobot->collisionSegWokXYZ[1],
                robot->collisionSegWokXYZ[0], robot->collisionSegWokXYZ[1]
            );

            localEnergy += 1/dist2;
            double collideDist2 = (robot->collisionBuffer + otherRobot->collisionBuffer + maxDisplacement) *
                                  (robot->collisionBuffer + otherRobot->collisionBuffer + maxDisplacement);
            if (dist2 < collideDist2){
                // this is not a viable move option
                // go on to next try
                didCollide = true;
                if (otherRobot->score() < robot->score()){
                    otherRobot->nudge = true;
                }
            }
        }

        if (didCollide){
            // don't consider this a viable option
            // move to next option
            break;
        }

        if (doPhobia){
            score = localEnergy;
        }
        else {
            score = robot->score();
        }

        if (score < bestScore and randomSample() < greed){
        // almost always pick a better score
            bestScore = score;
            bestAlpha = nextAlpha;
            bestBeta = nextBeta;
        }
        else if (score == bestScore and randomSample() > 0.5){
            // if score is same switch to new
            // state with 0.5 probability
            bestScore = score;
            bestAlpha = nextAlpha;
            bestBeta = nextBeta;
        }
    } // end loop over perturbations

    // set alpha beta to best found option
    robot->setAlphaBeta(bestAlpha, bestBeta);
    alphaPathPoint[1] = bestAlpha;
    betaPathPoint[1] = bestBeta;
    robot->alphaPath.push_back(alphaPathPoint);
    robot->betaPath.push_back(betaPathPoint);

    temp[0] = stepNum;
    temp[1] = robot->collisionSegWokXYZ[0][0]; // xAlphaEnd
    robot->roughAlphaX.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ[0][1]; // yAlphaEnd
    robot->roughAlphaY.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[0]; // xBetaEnd
    robot->roughBetaX.push_back(temp);
    temp[1] = robot->collisionSegWokXYZ.back()[1]; // yBetaEnd
    robot->roughBetaY.push_back(temp);
    robot->nudge = false;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#ifndef REALTIME_MF_realsense_HPP_INCLUDED
#define REALTIME_MF_realsense_HPP_INCLUDED

#include <string>

#include <Eigen/Dense>

#include <jsCore/timerLog.hpp>

#include <mmf/optimizationSO3.hpp>
#include <mmf/optimizationSO3_approx.hpp>
#include <mmf/optimizationSO3_vmf.hpp>

#include <cudaPcl/openniSmoothNormalsGpu.hpp>
#include <rgbdGrabber/realSenseGrabber.hpp>

#include <rtmf/rtmf.hpp>

using namespace Eigen;
using namespace std;

class RealtimeMF_realsense : public rgbdGrabber::RealSenseGrabber
{
  public:
    RealtimeMF_realsense(shared_ptr<RealtimeMF>& pRtmf, uint32_t w, uint32_t h, uint32_t fps);
    virtual ~RealtimeMF_realsense();

    virtual void rgbd_cb(const uint8_t* rgb, const uint16_t* depth);

  protected:

   shared_ptr<RealtimeMF> pRtmf_;
   cv::Mat normalsImg_;
   cv::Mat rgb_;

};

// -------------------------------- impl -----------------------------------

RealtimeMF_realsense::RealtimeMF_realsense(shared_ptr<RealtimeMF>& pRtmf,
  uint32_t w, uint32_t h, uint32_t fps)
  : rgbdGrabber::RealSenseGrabber(w,h,fps), pRtmf_(pRtmf)
{};

RealtimeMF_realsense::~RealtimeMF_realsense()
{
}

void RealtimeMF_realsense::rgbd_cb(const uint8_t* rgb, const uint16_t* depth)
{
  rgb_ = cv::Mat(h_,w_,CV_8UC3,const_cast<uint8_t*>(rgb));
  pRtmf_->compute(depth,w_,h_);
//  normalsImg_ = pRtmf_->normalsImg_;
//  cout<<pRtmf_->cRmf()<<endl;
  cv::Mat Iseg;
  cout<<"visualize Normals"<<endl;
  {
    Iseg = pRtmf_->overlaySeg(rgb_);
  }
  cv::imshow("seg",Iseg);
  cv::waitKey(1);
}

#endif


#include <boost/hana/concept/euclidean_ring.hpp>


/*
 * Copyright 2015 David A. Boyuka II
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * test-setops-simplify.cpp
 *
 *  Created on: August 6, 2014
 *      Author: David A. Boyuka II
 */

#include <cstdint>
#include <cassert>
#include <cstdlib>
#include <vector>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/dynamic_bitset.hpp>

#include "pique/region/region-encoding.hpp"
#include "pique/setops/setops.hpp"

using RU = RegionEncoding::RegionUniformity;
using NSO = NArySetOperation;
using SSO = SimplifiedSetOp;

static void test_simplify(NSO op, std::vector< RU > operands, SSO expected_result) {
	assert(SSO::simplify(op, operands) == expected_result);
}

int main(int argc, char **argv) {
	test_simplify(
		NSO::UNION,
		{ RU::MIXED, RU::MIXED },
		SSO(RU::MIXED, NSO::UNION, false)
	);
	test_simplify(
		NSO::UNION,
		{ RU::FILLED, RU::MIXED },
		SSO(RU::FILLED, NSO::UNION, false)
	);
	test_simplify(
		NSO::UNION,
		{ RU::MIXED, RU::FILLED },
		SSO(RU::FILLED, NSO::UNION, false)
	);
	test_simplify(
		NSO::UNION,
		{ RU::EMPTY, RU::MIXED },
		SSO(RU::MIXED, NSO::UNION, false)
	);
	test_simplify(
		NSO::UNION,
		{ RU::EMPTY, RU::FILLED },
		SSO(RU::FILLED, NSO::UNION, false)
	);

	test_simplify(
		NSO::INTERSECTION,
		{ },
		SSO(RU::FILLED, NSO::INTERSECTION, false)
	);
	test_simplify(
		NSO::INTERSECTION,
		{ RU::MIXED },
		SSO(RU::MIXED, NSO::INTERSECTION, false)
	);
	test_simplify(
		NSO::INTERSECTION,
		{ RU::MIXED, RU::EMPTY, RU::MIXED },
		SSO(RU::EMPTY, NSO::INTERSECTION, false)
	);

	test_simplify(
		NSO::DIFFERENCE,
		{ RU::MIXED },
		SSO(RU::MIXED, NSO::DIFFERENCE, false)
	);
	test_simplify(
		NSO::DIFFERENCE,
		{ RU::EMPTY, RU::MIXED },
		SSO(RU::EMPTY, NSO::DIFFERENCE, false)
	);
	test_simplify(
		NSO::DIFFERENCE,
		{ RU::FILLED, RU::MIXED },
		SSO(RU::MIXED, NSO::UNION, true)
	);

	test_simplify(
		NSO::SYMMETRIC_DIFFERENCE,
		{ RU::FILLED, RU::MIXED },
		SSO(RU::MIXED, NSO::SYMMETRIC_DIFFERENCE, true)
	);
	test_simplify(
		NSO::SYMMETRIC_DIFFERENCE,
		{ RU::FILLED, RU::MIXED, RU::EMPTY, RU::FILLED },
		SSO(RU::MIXED, NSO::SYMMETRIC_DIFFERENCE, false )
	);
}


#ifdef WITH_AD3
#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/external/ad3.hxx>
#include <param/ad3_param.hxx>

using namespace boost::python;

template<class GM,class ACC>
void export_ad3(){
   using namespace boost::python;
   import_array();
   append_subnamespace("solver");

   std::string srName = semiRingName  <typename GM::OperatorType,ACC >() ;

   // setup
   InfSetup setup;
   setup.algType     = "dual decomposition";
   setup.guarantees  = "global optimal if solverType='ac3_ilp'";
   setup.examples   = ">>> parameter = opengm.InfParam(solverType='ac3_ilp')\n"
                      ">>> inference = opengm.inference.AStar(gm=gm,accumulator='minimizer',parameter=parameter)\n"
                      "\n\n";

   typedef opengm::external::AD3Inf<GM, ACC>  PyInf;

   // export enums
   const std::string enumName1=std::string("_Ad3SolverType")+srName;
   enum_<typename PyInf::SolverType> (enumName1.c_str())
      .value("ad3_lp",   PyInf::AD3_LP)
      .value("ad3_ilp",  PyInf::AD3_ILP)
      .value("psdd_lp",  PyInf::PSDD_LP)
   ;

   // export parameter
   exportInfParam<PyInf>("_Ad3");
   // export inference
   class_< PyInf>("_Ad3",init<const GM & >())
   .def(InfSuite<PyInf,false>(std::string("Ad3"),setup))
   ;
}

template void export_ad3<opengm::python::GmAdder, opengm::Minimizer>();
template void export_ad3<opengm::python::GmAdder, opengm::Maximizer>();
//template void export_ad3<GmMultiplier, opengm::Minimizer>();
//template void export_ad3<GmMultiplier, opengm::Maximizer>();
#endif


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <string>
#include <thread>

#include <limits>

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

#include "Tudat/Basics/testMacros.h"
#include "Tudat/SimulationSetup/tudatSimulationHeader.h"
#include "Tudat/Astrodynamics/ObservationModels/linkTypeDefs.h"
#include "Tudat/Astrodynamics/ObservationModels/simulateObservations.h"
#include "Tudat/SimulationSetup/EstimationSetup/orbitDeterminationManager.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGroundStations.h"

namespace tudat
{
namespace unit_tests
{
BOOST_AUTO_TEST_SUITE( test_multi_arc_state_estimation )

//Using declarations.
using namespace tudat::observation_models;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::orbital_element_conversions;
using namespace tudat::ephemerides;
using namespace tudat::propagators;
using namespace tudat::basic_astrodynamics;

template< typename ObservationScalarType = double , typename TimeType = double , typename StateScalarType  = double >
Eigen::VectorXd  executeParameterEstimation(
        const int linkArcs )
{
    //Load spice kernels.f
    std::string kernelsPath = input_output::getSpiceKernelPath( );
    spice_interface::loadStandardSpiceKernels( );

    //Define setting for total number of bodies and those which need to be integrated numerically.
    //The first numberOfNumericalBodies from the bodyNames vector will be integrated numerically.

    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Mars" );
    bodyNames.push_back( "Sun" );
    bodyNames.push_back( "Moon" );

    // Specify initial time
    TimeType initialEphemerisTime = TimeType( 1.0E7 );
    TimeType finalEphemerisTime = TimeType( 6.0E7 );
    double maximumTimeStep = 3600.0;

    double buffer = 10.0 * maximumTimeStep;

    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );
    bodySettings[ "Earth" ]->ephemerisSettings-> resetMakeMultiArcEphemeris( true );
    bodySettings[ "Moon" ]->ephemerisSettings->resetFrameOrigin( "Sun" );
    bodySettings[ "Mars" ]->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
                "ECLIPJ2000", "IAU_Mars",
                spice_interface::computeRotationQuaternionBetweenFrames(
                    "ECLIPJ2000", "IAU_Mars", initialEphemerisTime ),
                initialEphemerisTime, 2.0 * mathematical_constants::PI /
                ( physical_constants::JULIAN_DAY + 40.0 * 60.0 ) );
    NamedBodyMap bodyMap = createBodies( bodySettings );

    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Create ground stations
    std::pair< std::string, std::string > grazStation = std::pair< std::string, std::string >( "Earth", "" );
    std::pair< std::string, std::string > mslStation = std::pair< std::string, std::string >( "Mars", "MarsStation" );

    createGroundStation( bodyMap.at( "Mars" ), "MarsStation", ( Eigen::Vector3d( ) << 100.0, 0.5, 2.1 ).finished( ),
                         coordinate_conversions::geodetic_position );

    std::vector< std::pair< std::string, std::string > > groundStations;
    groundStations.push_back( grazStation );
    groundStations.push_back( mslStation );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
    accelerationsOfEarth[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfEarth[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Earth" ] = accelerationsOfEarth;

    // Set bodies for which initial state is to be estimated and integrated.
    std::vector< std::string > bodiesToEstimate;
    bodiesToEstimate.push_back( "Earth" );
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Earth" );
    unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( );

    // Define propagator settings.
    std::vector< std::string > centralBodies;
    std::map< std::string, std::string > centralBodyMap;

    centralBodies.resize( numberOfNumericalBodies );
    for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
    {
        centralBodies[ i ] = "SSB";
    }

    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, bodiesToIntegrate, centralBodies );

    std::vector< double > integrationArcStartTimes;
    std::vector< double > integrationArcEndTimes;

    std::vector< double > integrationArcLimits;

    double integrationStartTime = initialEphemerisTime + 1.0E4;
    double integrationEndTime = finalEphemerisTime - 1.0E4;
    double arcDuration = 1.6E7;
    double arcOverlap = 2.0E4;

    double currentStartTime = integrationStartTime, currentEndTime = integrationStartTime + arcDuration;

    do
    {
        integrationArcLimits.push_back( currentStartTime );
        integrationArcEndTimes.push_back( currentEndTime );
        integrationArcStartTimes.push_back( currentStartTime );
        currentStartTime = currentEndTime - arcOverlap;
        currentEndTime = currentStartTime + arcDuration;
    }
    while( currentEndTime < integrationEndTime );
    integrationArcLimits.push_back( currentStartTime + arcOverlap );

    // Set parameters that are to be estimated.
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back( std::make_shared< ArcWiseInitialTranslationalStateEstimatableParameterSettings< StateScalarType > >(
                                  "Earth", integrationArcStartTimes ) );
    parameterNames.push_back( std::make_shared< EstimatableParameterSettings >
                              ( "Mars", constant_rotation_rate ) );
    parameterNames.push_back(  std::make_shared< EstimatableParameterSettings >
                               ( "Mars", rotation_pole_position ) );

    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< StateScalarType > > parametersToEstimate =
            createParametersToEstimate< StateScalarType >( parameterNames, bodyMap );

    // Define links in simulation.
    std::vector< LinkEnds > linkEnds2;
    linkEnds2.resize( 2 );
    linkEnds2[ 0 ][ transmitter ] = grazStation;
    linkEnds2[ 0 ][ receiver ] = mslStation;

    linkEnds2[ 1 ][ receiver ] = grazStation;
    linkEnds2[ 1 ][ transmitter ] = mslStation;

    observation_models::ObservationSettingsMap observationSettingsMap;
    observationSettingsMap.insert( std::make_pair( linkEnds2[ 0 ], std::make_shared< ObservationSettings >(
                                       one_way_range ) ) );
    observationSettingsMap.insert( std::make_pair( linkEnds2[ 1 ], std::make_shared< ObservationSettings >(
                                       one_way_range ) ) );

    // Define integrator settings.
    std::shared_ptr< IntegratorSettings< TimeType > > integratorSettings =
            std::make_shared< IntegratorSettings< TimeType > >
            ( rungeKutta4, TimeType( initialEphemerisTime - 4.0 * maximumTimeStep ), 3600.0 );

    std::vector< std::shared_ptr< SingleArcPropagatorSettings< StateScalarType > > > propagatorSettingsList;
    for( unsigned int i = 0; i < integrationArcStartTimes.size( ); i++ )
    {
        Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > currentInitialState =
                getInitialStateOfBody< TimeType, StateScalarType>(
                    bodiesToIntegrate.at( 0 ), centralBodies.at( 0 ), bodyMap, integrationArcStartTimes.at( i ) );
        propagatorSettingsList.push_back(
                    std::make_shared< TranslationalStatePropagatorSettings< StateScalarType > >
                    ( centralBodies, accelerationModelMap, bodiesToIntegrate,
                      currentInitialState,
                      integrationArcEndTimes.at( i ) ) );
    }
    std::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings =
            std::make_shared< MultiArcPropagatorSettings< StateScalarType > >( propagatorSettingsList, linkArcs );

    // Create orbit determination object.
    OrbitDeterminationManager< ObservationScalarType, TimeType > orbitDeterminationManager =
            OrbitDeterminationManager< ObservationScalarType, TimeType >(
                bodyMap, parametersToEstimate,
                observationSettingsMap, integratorSettings, propagatorSettings );

    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > initialParameterEstimate =
            parametersToEstimate->template getFullParameterValues< StateScalarType >( );

    TimeType observationTime;
    int numberOfObservationsPerArc = 5000;
    double timeBuffer = 9000.0;

    std::vector< TimeType > initialObservationTimes;
    initialObservationTimes.resize( numberOfObservationsPerArc * integrationArcStartTimes.size( ) );

    for( unsigned int i = 0; i < integrationArcLimits.size( ) - 1; i++ )
    {
        double currentTimeStep = ( integrationArcLimits[ i + 1 ] - integrationArcLimits[ i ] - 2.0 * timeBuffer ) /
                static_cast< double >( numberOfObservationsPerArc - 1 );
        observationTime = integrationArcLimits[ i ] + timeBuffer;
        for( int j = 0; j < numberOfObservationsPerArc; j++ )
        {
            initialObservationTimes[ j + i * numberOfObservationsPerArc ] = observationTime;
            observationTime += currentTimeStep;
        }
    }

    std::map< ObservableType, std::map< LinkEnds, std::pair< std::vector< TimeType >, LinkEndType > > > measurementSimulationInput;
    std::map< LinkEnds, std::pair< std::vector< TimeType >, LinkEndType > > singleObservableSimulationInput;

    singleObservableSimulationInput[ linkEnds2[ 0 ] ] = std::make_pair( initialObservationTimes, receiver );
    measurementSimulationInput[ one_way_range ] = singleObservableSimulationInput;

    typedef Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > ObservationVectorType;
    typedef std::map< LinkEnds, std::pair< ObservationVectorType, std::pair< std::vector< TimeType >, LinkEndType > > > SingleObservablePodInputType;
    typedef std::map< ObservableType, SingleObservablePodInputType > PodInputDataType;

    PodInputDataType observationsAndTimes = simulateObservations< ObservationScalarType, TimeType >(
                measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( )  );

    Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > truthParameters = initialParameterEstimate;

    for( unsigned int i = 0; i < numberOfNumericalBodies * integrationArcStartTimes.size( ); i++ )
    {
        initialParameterEstimate[ 0 + 6 * i ] += 1.0E0;
        initialParameterEstimate[ 1 + 6 * i ] += 1.0E0;
        initialParameterEstimate[ 2 + 6 * i ] += 1.0E0;
        initialParameterEstimate[ 3 + 6 * i ] += 1.0E-5;
        initialParameterEstimate[ 4 + 6 * i ] += 1.0E-5;
        initialParameterEstimate[ 5 + 6 * i ] += 1.0E-5;
    }
    for( unsigned int i = numberOfNumericalBodies * integrationArcStartTimes.size( );
         i < static_cast< unsigned int >( initialParameterEstimate.rows( ) ); i++ )
    {
        initialParameterEstimate[ i ] *= ( 1.0 + 1.0E-6 );
    }

    parametersToEstimate->resetParameterValues( initialParameterEstimate );

    std::shared_ptr< PodInput< ObservationScalarType, TimeType > > podInput =
            std::make_shared< PodInput< ObservationScalarType, TimeType > >(
                observationsAndTimes, ( initialParameterEstimate ).rows( ) );

    std::shared_ptr< PodOutput< StateScalarType, TimeType > > podOutput = orbitDeterminationManager.estimateParameters(
                podInput );

    return ( podOutput->parameterEstimate_ - truthParameters ).template cast< double >( );
}

BOOST_AUTO_TEST_CASE( test_MultiArcStateEstimation )
{
    // Execute test for linked arcs and separate arcs.
    for( unsigned int testCase = 0; testCase < 2; testCase++ )
    {
#if( BUILD_EXTENDED_PRECISION_PROPAGATION_TOOLS )
        Eigen::VectorXd parameterError = executeParameterEstimation< long double, tudat::Time, long double >(
                    testCase );
        int numberOfEstimatedArcs = ( parameterError.rows( ) - 3 ) / 6;

        std::cout <<"Estimation error: "<< parameterError.transpose( ) << std::endl;
        for( int i = 0; i < numberOfEstimatedArcs; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( parameterError( i * 6 + j ) ), 1E-4 );
                BOOST_CHECK_SMALL( std::fabs( parameterError( i * 6 + j + 3 ) ), 1.0E-10  );
            }
        }

        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 3 ) ), 1.0E-20 );
        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 2 ) ), 1.0E-12 );
        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 1 ) ), 1.0E-12 );
#else
        Eigen::VectorXd parameterError = executeParameterEstimation< double, double, double >(
                    testCase );
        int numberOfEstimatedArcs = ( parameterError.rows( ) - 3 ) / 6;

        std::cout << parameterError.transpose( ) << std::endl;
        for( int i = 0; i < numberOfEstimatedArcs; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( parameterError( i * 6 + j ) ), 1E-1 );
                BOOST_CHECK_SMALL( std::fabs( parameterError( i * 6 + j + 3 ) ), 1.0E-7  );
            }
        }

        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 3 ) ), 1.0E-17 );
        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 2 ) ), 1.0E-9 );
        BOOST_CHECK_SMALL( std::fabs( parameterError( parameterError.rows( ) - 1 ) ), 1.0E-9 );
#endif
    }

}

BOOST_AUTO_TEST_SUITE_END( )

}

}