/*
 Copyright (C) 2018 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.
*/

/*! \file qle/termstructures/pricecurve.hpp
    \brief Interpolated price curve
*/

#ifndef quantext_price_curve_hpp
#define quantext_price_curve_hpp

#include <boost/algorithm/cxx11/is_sorted.hpp>

#include <qle/termstructures/pricetermstructure.hpp>

#include <ql/currency.hpp>
#include <ql/math/comparison.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/interpolatedcurve.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

namespace QuantExt {

//! Interpolated price curve
/*! Class representing a curve of projected prices in the future.

    \warning for consistency, if curve is constructed by inferring times from dates
             using a given day counter, pass the same day counter to the constructor

    \ingroup termstructures
*/
template <class Interpolator>
class InterpolatedPriceCurve : public PriceTermStructure,
                               public QuantLib::LazyObject,
                               protected QuantLib::InterpolatedCurve<Interpolator> {
public:
    //! \name Constructors
    //@{
    //! Curve constructed from periods and prices. No conventions are applied in getting to a date from a period.
    InterpolatedPriceCurve(const std::vector<QuantLib::Period>& tenors, const std::vector<QuantLib::Real>& prices,
                           const QuantLib::DayCounter& dc, const QuantLib::Currency& currency,
                           const Interpolator& interpolator = Interpolator());

    //! Curve constructed from periods and quotes. No conventions are applied in getting to a date from a period.
    InterpolatedPriceCurve(const std::vector<QuantLib::Period>& tenors,
                           const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
                           const QuantLib::DayCounter& dc, const QuantLib::Currency& currency,
                           const Interpolator& interpolator = Interpolator());

    //! Curve constructed from dates and prices
    InterpolatedPriceCurve(const QuantLib::Date& referenceDate, const std::vector<QuantLib::Date>& dates,
                           const std::vector<QuantLib::Real>& prices, const QuantLib::DayCounter& dc,
                           const QuantLib::Currency& currency, const Interpolator& interpolator = Interpolator());

    //! Curve constructed from dates and quotes
    InterpolatedPriceCurve(const QuantLib::Date& referenceDate, const std::vector<QuantLib::Date>& dates,
                           const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
                           const QuantLib::DayCounter& dc, const QuantLib::Currency& currency,
                           const Interpolator& interpolator = Interpolator());
    //@}

    //! \name Observer interface
    //@{
    void update();
    //@}

    //! \name LazyObject interface
    //@{
    void performCalculations() const;
    //@}

    //! \name TermStructure interface
    //@{
    QuantLib::Date maxDate() const;
    QuantLib::Time maxTime() const;
    //@}

    //! \name PriceTermStructure interface
    //@{
    QuantLib::Time minTime() const;
    std::vector<QuantLib::Date> pillarDates() const;
    const QuantLib::Currency& currency() const { return currency_; }
    //@}

    //! \name Inspectors
    //@{
    const std::vector<QuantLib::Time>& times() const { return this->times_; }
    const std::vector<QuantLib::Real>& prices() const { return this->data_; }
    //@}

protected:
    //! \name PriceTermStructure implementation
    //@{
    QuantLib::Real priceImpl(QuantLib::Time t) const;
    //@}

private:
    const QuantLib::Currency currency_;
    std::vector<QuantLib::Handle<QuantLib::Quote> > quotes_;
    std::vector<QuantLib::Period> tenors_;
    mutable std::vector<QuantLib::Date> dates_;

    void initialise();
    void populateDatesFromTenors() const;
    void convertDatesToTimes();
    void getPricesFromQuotes() const;
};

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(const std::vector<QuantLib::Period>& tenors,
                                                             const std::vector<QuantLib::Real>& prices,
                                                             const QuantLib::DayCounter& dc,
                                                             const QuantLib::Currency& currency,
                                                             const Interpolator& interpolator)
    : PriceTermStructure(0, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                               std::vector<QuantLib::Time>(tenors.size()), prices,
                                                               interpolator),
      currency_(currency), tenors_(tenors), dates_(tenors.size()) {

    QL_REQUIRE(boost::algorithm::is_sorted(tenors_.begin(), tenors_.end()), "Tenors must be sorted");
    populateDatesFromTenors();
    initialise();
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(
    const std::vector<QuantLib::Period>& tenors, const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes,
    const QuantLib::DayCounter& dc, const QuantLib::Currency& currency, const Interpolator& interpolator)
    : PriceTermStructure(0, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                               std::vector<QuantLib::Time>(tenors.size()),
                                                               std::vector<QuantLib::Real>(quotes.size()),
                                                               interpolator),
      currency_(currency), quotes_(quotes), tenors_(tenors), dates_(tenors.size()) {

    QL_REQUIRE(boost::algorithm::is_sorted(tenors_.begin(), tenors_.end()), "Tenors must be sorted");
    populateDatesFromTenors();
    initialise();

    // Observe the quotes
    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        registerWith(quotes[i]);
    }
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(const QuantLib::Date& referenceDate,
                                                             const std::vector<QuantLib::Date>& dates,
                                                             const std::vector<QuantLib::Real>& prices,
                                                             const QuantLib::DayCounter& dc,
                                                             const QuantLib::Currency& currency,
                                                             const Interpolator& interpolator)
    : PriceTermStructure(referenceDate, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                                           std::vector<QuantLib::Time>(dates.size()),
                                                                           prices, interpolator),
      currency_(currency), dates_(dates) {

    convertDatesToTimes();
    initialise();
}

template <class Interpolator>
InterpolatedPriceCurve<Interpolator>::InterpolatedPriceCurve(
    const QuantLib::Date& referenceDate, const std::vector<QuantLib::Date>& dates,
    const std::vector<QuantLib::Handle<QuantLib::Quote> >& quotes, const QuantLib::DayCounter& dc,
    const QuantLib::Currency& currency, const Interpolator& interpolator)
    : PriceTermStructure(referenceDate, QuantLib::NullCalendar(), dc), QuantLib::InterpolatedCurve<Interpolator>(
                                                                           std::vector<QuantLib::Time>(dates.size()),
                                                                           std::vector<QuantLib::Real>(quotes.size()),
                                                                           interpolator),
      currency_(currency), quotes_(quotes), dates_(dates) {

    convertDatesToTimes();
    initialise();

    // Observe the quotes
    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        registerWith(quotes[i]);
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::update() {

    QuantLib::LazyObject::update();

    // TermStructure::update() update part
    if (moving_) {
        updated_ = false;
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::performCalculations() const {
    // Calculations need to be performed if the curve is tenor based
    if (!tenors_.empty()) {
        populateDatesFromTenors();
        this->interpolation_.update();
    }

    // Calculations need to be performed if the curve depends on quotes
    if (!quotes_.empty()) {
        getPricesFromQuotes();
        this->interpolation_.update();
    }
}

template <class Interpolator> QuantLib::Date InterpolatedPriceCurve<Interpolator>::maxDate() const {
    calculate();
    return dates_.back();
}

template <class Interpolator> QuantLib::Time InterpolatedPriceCurve<Interpolator>::maxTime() const {
    calculate();
    return this->times_.back();
}

template <class Interpolator> QuantLib::Time InterpolatedPriceCurve<Interpolator>::minTime() const {
    calculate();
    return this->times_.front();
}

template <class Interpolator> std::vector<QuantLib::Date> InterpolatedPriceCurve<Interpolator>::pillarDates() const {
    calculate();
    return dates_;
}

template <class Interpolator> QuantLib::Real InterpolatedPriceCurve<Interpolator>::priceImpl(QuantLib::Time t) const {
    // Return interpolated/extrapolated price
    calculate();
    return this->interpolation_(t, true);
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::initialise() {

    QL_REQUIRE(this->data_.size() >= Interpolator::requiredPoints, "not enough times for the interpolation method");

    // If we are quotes based, get prices from quotes
    if (!quotes_.empty()) {
        getPricesFromQuotes();
    }

    QL_REQUIRE(this->data_.size() == this->times_.size(), "Number of times must equal number of prices");

    QuantLib::InterpolatedCurve<Interpolator>::setupInterpolation();
    this->interpolation_.update();
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::populateDatesFromTenors() const {
    QuantLib::Date asof = QuantLib::Settings::instance().evaluationDate();
    for (QuantLib::Size i = 0; i < dates_.size(); ++i) {
        dates_[i] = asof + tenors_[i];
        this->times_[i] = timeFromReference(dates_[i]);
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::convertDatesToTimes() {

    QL_REQUIRE(!dates_.empty(), "Dates cannot be empty for InterpolatedPriceCurve");
    this->times_[0] = timeFromReference(dates_[0]);
    for (QuantLib::Size i = 1; i < dates_.size(); ++i) {
        QL_REQUIRE(dates_[i] > dates_[i - 1], "invalid date (" << dates_[i] << ", vs " << dates_[i - 1] << ")");
        this->times_[i] = timeFromReference(dates_[i]);
        QL_REQUIRE(!QuantLib::close(this->times_[i], this->times_[i - 1]), "two dates correspond to the same time "
                                                                           "under this curve's day count convention");
    }
}

template <class Interpolator> void InterpolatedPriceCurve<Interpolator>::getPricesFromQuotes() const {

    for (QuantLib::Size i = 0; i < quotes_.size(); ++i) {
        QL_REQUIRE(!this->quotes_[i].empty(), "price quote at index " << i << " is empty");
        this->data_[i] = quotes_[i]->value();
    }
}

} // namespace QuantExt

#endif


#define BOOST_TEST_MODULE TimeInterval
#include <boost/test/unit_test.hpp>

#include "sycomore/TimeInterval.h"
#include "sycomore/units.h"

void test_quantity_array(
    sycomore::Array<sycomore::Quantity> const & left,
    sycomore::Array<sycomore::Quantity> const & right)
{
    BOOST_TEST(left.size() == right.size());
    for(unsigned int i=0; i<left.size(); ++i)
    {
        auto const & l = left[i];
        auto const & r = right[i];

        BOOST_TEST(l.dimensions == r.dimensions);
        BOOST_TEST(l.magnitude == r.magnitude);
    }
}

BOOST_AUTO_TEST_CASE(DefaultConstructor)
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval;

    BOOST_TEST(interval.get_duration() == 0*s);

    test_quantity_array(
        interval.get_gradient_moment(), {0*rad/m,0*rad/m,0*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {0*T/m,0*T/m,0*T/m});
    test_quantity_array(
        interval.get_gradient_area(), {0*T/m*s,0*T/m*s,0*T/m*s});
    test_quantity_array(
        interval.get_gradient_dephasing(), {0*rad/m,0*rad/m,0*rad/m});
}

BOOST_AUTO_TEST_CASE(DurationConstructor)
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms);

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {0*rad/m,0*rad/m,0*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {0*T/m,0*T/m,0*T/m});
    test_quantity_array(
        interval.get_gradient_area(), {0*T/m*s,0*T/m*s,0*T/m*s});
    test_quantity_array(
        interval.get_gradient_dephasing(), {0*rad/m,0*rad/m,0*rad/m});
}

BOOST_AUTO_TEST_CASE(DephasingScalarConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms, 2.*rad/dm);

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {20*rad/m,20*rad/m,20*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {
            74.76015355016015*uT/m,
            74.76015355016015*uT/m,
            74.76015355016015*uT/m});
    test_quantity_array(
        interval.get_gradient_area(), {
            74.76015355016015*uT/m*ms,
            74.76015355016015*uT/m*ms,
            74.76015355016015*uT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {20*rad/m,20*rad/m,20*rad/m});
}

BOOST_AUTO_TEST_CASE(DephasingVectorConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms, {2*rad/dm,4*rad/m,8*rad/dam});

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {20*rad/m,4*rad/m,0.8*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {
            74.76015355016015*uT/m,
            14.95203071003203*uT/m,
            2.9904061420064063*uT/m});
    test_quantity_array(
        interval.get_gradient_area(), {
            74.76015355016015*uT/m*ms,
            14.95203071003203*uT/m*ms,
            2.9904061420064063*uT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {20*rad/m,4*rad/m,0.8*rad/m});
}

BOOST_AUTO_TEST_CASE(AmplitudeScalarConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms, 2.*mT/dm);

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {20*mT/m, 20*mT/m, 20*mT/m});
    test_quantity_array(
        interval.get_gradient_area(), {20*mT/m*ms, 20*mT/m*ms, 20*mT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m});
}

BOOST_AUTO_TEST_CASE(AmplitudeVectorConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms, {2.*mT/dm, 4.*mT/m, 8.*mT/dam});

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {
            5350.4437993378515*rad/m,
            1070.0887598675702*rad/m,
            214.01775197351404*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {20*mT/m, 4*mT/m, 0.8*mT/m});
    test_quantity_array(
        interval.get_gradient_area(), {20*mT/m*ms, 4*mT/m*ms, 0.8*mT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {
            5350.4437993378515*rad/m,
            1070.0887598675702*rad/m,
            214.01775197351404*rad/m});
}

BOOST_AUTO_TEST_CASE(AreaScalarConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(1._ms, 2.*mT/dm*ms);

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {20*mT/m, 20*mT/m, 20*mT/m});
    test_quantity_array(
        interval.get_gradient_area(), {20*mT/m*ms, 20*mT/m*ms, 20*mT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m,
            5350.4437993378515*rad/m});
}

BOOST_AUTO_TEST_CASE(AreaVectorConstructor, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;
    sycomore::TimeInterval const interval(
        1._ms, {2.*mT/dm*ms, 4.*mT/m*ms, 8.*mT/dam*ms});

    BOOST_TEST(interval.get_duration() == 1.e-3*s);

    test_quantity_array(
        interval.get_gradient_moment(), {
            5350.4437993378515*rad/m,
            1070.0887598675702*rad/m,
            214.01775197351404*rad/m});
    test_quantity_array(
        interval.get_gradient_amplitude(), {20*mT/m, 4*mT/m, 0.8*mT/m});
    test_quantity_array(
        interval.get_gradient_area(), {20*mT/m*ms, 4*mT/m*ms, 0.8*mT/m*ms});
    test_quantity_array(
        interval.get_gradient_dephasing(), {
            5350.4437993378515*rad/m,
            1070.0887598675702*rad/m,
            214.01775197351404*rad/m});
}

using Getter = sycomore::Quantity (sycomore::TimeInterval::*)(sycomore::Quantity const &);
using ScalarSetter = void (sycomore::TimeInterval::*)(sycomore::Quantity const &);
using VectorSetter = void (sycomore::TimeInterval::*)(sycomore::Array<sycomore::Quantity> const &);

sycomore::Array<sycomore::Quantity> const amplitude {
    20*sycomore::units::mT/sycomore::units::m,
    40*sycomore::units::mT/sycomore::units::m,
    80*sycomore::units::mT/sycomore::units::m};
sycomore::Array<sycomore::Quantity> const area {
    20*sycomore::units::mT/sycomore::units::m*sycomore::units::ms,
    40*sycomore::units::mT/sycomore::units::m*sycomore::units::ms,
    80*sycomore::units::mT/sycomore::units::m*sycomore::units::ms};
sycomore::Array<sycomore::Quantity> const dephasing{
    5350.4437993378515*sycomore::units::rad/sycomore::units::m,
    10700.887598675701*sycomore::units::rad/sycomore::units::m,
    21401.775197351402*sycomore::units::rad/sycomore::units::m};
auto const moment = dephasing;

template<typename TGetter>
void test_gradient_accessor(
    TGetter getter, ScalarSetter scalar_setter, VectorSetter vector_setter,
    sycomore::Array<sycomore::Quantity> const & data)
{
    using namespace sycomore::units;
    sycomore::TimeInterval interval(1._ms);

    (interval.*scalar_setter)(data[0]);
    test_quantity_array(
        interval.get_gradient_amplitude(),
        {amplitude[0], amplitude[0], amplitude[0]});
    test_quantity_array(
        interval.get_gradient_area(),
        {area[0], area[0], area[0]});
    test_quantity_array(
        interval.get_gradient_dephasing(),
        {dephasing[0], dephasing[0], dephasing[0]});
    test_quantity_array(
        interval.get_gradient_moment(),
        {dephasing[0], dephasing[0], dephasing[0]});

    (interval.*vector_setter)(data);
    test_quantity_array(interval.get_gradient_amplitude(), amplitude);
    test_quantity_array(interval.get_gradient_area(), area);
    test_quantity_array(interval.get_gradient_dephasing(), dephasing);
    test_quantity_array(interval.get_gradient_moment(), moment);

    interval.set_gradient(data[1]);
    test_quantity_array(
        interval.get_gradient_amplitude(),
        {amplitude[1], amplitude[1], amplitude[1]});
    test_quantity_array(
        interval.get_gradient_area(),
        {area[1], area[1], area[1]});
    test_quantity_array(
        interval.get_gradient_dephasing(),
        {dephasing[1], dephasing[1], dephasing[1]});
    test_quantity_array(
        interval.get_gradient_moment(),
        {dephasing[1], dephasing[1], dephasing[1]});

    interval.set_gradient(data);
    test_quantity_array(interval.get_gradient_amplitude(), amplitude);
    test_quantity_array(interval.get_gradient_area(), area);
    test_quantity_array(interval.get_gradient_dephasing(), dephasing);
    test_quantity_array(interval.get_gradient_moment(), moment);
}

BOOST_AUTO_TEST_CASE(GradientAccessors, *boost::unit_test::tolerance(1e-9))
{
    test_gradient_accessor(
        &sycomore::TimeInterval::get_gradient_amplitude,
        &sycomore::TimeInterval::set_gradient_amplitude,
        &sycomore::TimeInterval::set_gradient_amplitude,
        amplitude);
    test_gradient_accessor(
        &sycomore::TimeInterval::get_gradient_area,
        &sycomore::TimeInterval::set_gradient_area,
        &sycomore::TimeInterval::set_gradient_area,
        area);
    test_gradient_accessor(
        &sycomore::TimeInterval::get_gradient_dephasing,
        &sycomore::TimeInterval::set_gradient_dephasing,
        &sycomore::TimeInterval::set_gradient_dephasing,
        dephasing);
    test_gradient_accessor(
        &sycomore::TimeInterval::get_gradient_moment,
        &sycomore::TimeInterval::set_gradient_moment,
        &sycomore::TimeInterval::set_gradient_moment,
        moment);
}

BOOST_AUTO_TEST_CASE(Comparison)
{
    using namespace sycomore::units;

    sycomore::TimeInterval const interval_1(1._ms, 2*T/m);
    sycomore::TimeInterval const interval_2(1._ms, 2*T/m);
    sycomore::TimeInterval const interval_3(1._ms, {2*T/m, 2*T/m, 2*T/m});

    BOOST_CHECK(interval_1 == interval_2);
    BOOST_CHECK(interval_1 == interval_3);
    BOOST_CHECK(!(interval_1 != interval_2));
    BOOST_CHECK(!(interval_1 != interval_3));

    sycomore::TimeInterval const interval_4(4._ms, 2*T/m);
    BOOST_CHECK(!(interval_1 == interval_4));
    BOOST_CHECK(interval_1 != interval_4);

    sycomore::TimeInterval const interval_5(1._ms, 4*T/m);
    BOOST_CHECK(!(interval_1 == interval_5));
    BOOST_CHECK(interval_1 != interval_5);
}


// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2015 The Pocketcoin Core developers
// Copyright (c) 2018 The Pocketcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <boost/range/adaptor/reversed.hpp>
#include <chain.h>
#include <chainparams.h>
#include <consensus/validation.h>
#include <pos.h>
#include <pow.h>
#include <primitives/block.h>
#include <utiltime.h>
#include <validation.h>
#ifdef ENABLE_WALLET
#include <wallet/wallet.h>
#endif
#include "util.h"
#include "validationinterface.h"

double GetPosDifficulty(const CBlockIndex *blockindex)
{
    // Floating point number that is a multiple of the minimum difficulty,
    // minimum difficulty = 1.0.
    if (blockindex == nullptr)
    {
        if (chainActive.Tip() == nullptr)
        {
            return 1.0;
        } else
        {
            blockindex = GetLastBlockIndex(chainActive.Tip(), false);
        }
    }

    int nShift = (blockindex->nBits >> 24) & 0xff;

    double dDiff = (double) 0x0000ffff / (double) (blockindex->nBits & 0x00ffffff);

    while (nShift < 29)
    {
        dDiff *= 256.0;
        nShift++;
    }
    while (nShift > 29)
    {
        dDiff /= 256.0;
        nShift--;
    }

    return dDiff;
}

double GetPoWMHashPS()
{
    return 0;
}

double GetPoSKernelPS()
{
    int nPoSInterval = 72;
    double dStakeKernelsTriedAvg = 0;
    int nStakesHandled = 0, nStakesTime = 0;

    CBlockIndex *pindex = chainActive.Tip();
    CBlockIndex *pindexPrevStake = NULL;

    while (pindex && nStakesHandled < nPoSInterval)
    {
        if (pindex->IsProofOfStake())
        {
            if (pindexPrevStake)
            {
                dStakeKernelsTriedAvg += GetPosDifficulty(pindexPrevStake) * 4294967296.0;
                nStakesTime += pindexPrevStake->nTime - pindex->nTime;
                nStakesHandled++;
            }
            pindexPrevStake = pindex;
        }

        pindex = pindex->pprev;
    }

    double result = 0;

    if (nStakesTime)
    {
        result = dStakeKernelsTriedAvg / nStakesTime;
    }

    result *= STAKE_TIMESTAMP_MASK + 1;

    return result;
}

int64_t GetProofOfStakeReward(int nHeight, int64_t nFees, const Consensus::Params &consensusParams)
{
    int halvings = nHeight / consensusParams.nSubsidyHalvingInterval;
    // Force block reward to zero when right shift is undefined.
    if (halvings >= 64)
    {
        return nFees;
    }

    CAmount nSubsidy = 5 * COIN;

    // Subsidy is cut in half every 210,000 blocks which will occur approximately every 4 years.
    nSubsidy >>= halvings;
    return nSubsidy + nFees;
}

int64_t GetWeight(int64_t nIntervalBeginning, int64_t nIntervalEnd)
{
    // Kernel hash weight starts from 0 at the min age
    // this change increases active coins participating the hash and helps
    // to secure the network when proof-of-stake difficulty is low
    return nIntervalEnd - nIntervalBeginning - Params().GetConsensus().nStakeMinAge;
}

#ifdef ENABLE_WALLET
bool CheckStake(const std::shared_ptr<CBlock> pblock, const PocketBlockRef& pocketBlock, std::shared_ptr<CWallet> wallet, CChainParams const &chainparams)
{
    arith_uint256 proofHash = arith_uint256(0), hashTarget = arith_uint256(0);
    uint256 hashBlock = pblock->GetHash();

    if (!pblock->IsProofOfStake())
    {
        return error("CheckStake() : %s is not a proof-of-stake block", hashBlock.GetHex());
    }

    // verify hash target and signature of coinstake tx
    CDataStream hashProofOfStakeSource(SER_GETHASH, 0);
    if (!CheckProofOfStake(mapBlockIndex[pblock->hashPrevBlock], pblock->vtx[1], pblock->nBits, proofHash,
        hashProofOfStakeSource, hashTarget, NULL))
    {
        return error("CheckStake() : proof-of-stake checking failed (%s)", pblock->hashPrevBlock.GetHex());
    }

    //// debug print
    LogPrintf("=== Staking : new PoS block found hash: %d - %s\n", chainActive.Height() + 1, hashBlock.GetHex());

    // Found a solution
    {
        LOCK(cs_main);
        auto hashBestChain = chainActive.Tip()->GetBlockHash();
        if (pblock->hashPrevBlock != hashBestChain)
        {
            return error("CheckStake() : generated block is stale");
        }

        GetMainSignals().BlockFound(pblock->GetHash());
    }

    // Process this block the same as if we had received it from another node
    CValidationState state;
    if (!ProcessNewBlock(state, chainparams, pblock, pocketBlock, true, /* fReceived */ false, NULL))
    {
        return error("CoinStaker: ProcessNewBlock, block not accepted %s", state.GetRejectReason());
    }

    return true;
}
#endif

bool CheckProofOfStake(CBlockIndex *pindexPrev, CTransactionRef const &tx, unsigned int nBits,
    arith_uint256 &hashProofOfStake, CDataStream &hashProofOfStakeSource, arith_uint256 &targetProofOfStake,
    std::vector<CScriptCheck> *pvChecks, bool fCheckSignature)
{
    if (!tx->IsCoinStake())
    {
        return error("CheckProofOfStake() : called on non-coinstake %s", tx->GetHash().ToString());
    }

    // Kernel (input 0) must match the stake hash target per coin age (nBits)
    const CTxIn &txin = tx->vin[0];

    CTransactionRef txPrev;
    uint256 hashBlock = uint256();
    if (!GetTransaction(txin.prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true))
    {
        return error("CheckProofOfStake() : INFO: read txPrev failed %s",
            txin.prevout.hash.GetHex()); // previous transaction not in main chain, may occur during initial download
    }

    if (pvChecks)
    {
        pvChecks->reserve(tx->vin.size());
    }

    if (fCheckSignature)
    {
        const CTransaction &txn = *tx;
        PrecomputedTransactionData txdata(txn);
        const COutPoint &prevout = tx->vin[0].prevout;
        const Coin *coins = &pcoinsTip->AccessCoin(prevout);
        assert(coins);

        // Verify signature
        CScriptCheck check(coins->out, *tx, 0, SCRIPT_VERIFY_NONE, false, &txdata);
        if (pvChecks)
        {
            pvChecks->push_back(CScriptCheck());
            check.swap(pvChecks->back());
        } else if (!check())
        {
            return error("CheckProofOfStake() : script-verify-failed %s", ScriptErrorString(check.GetScriptError()));
        }
    }

    if (mapBlockIndex.count(hashBlock) == 0)
    {
        return error("CheckProofOfStake() : read block failed"); // unable to read block of previous transaction
    }

    CBlockIndex *pblockindex = mapBlockIndex[hashBlock];

    if (txin.prevout.hash != txPrev->GetHash())
    {
        return error("CheckProofOfStake(): Coinstake input does not match previous output %s",
            txin.prevout.hash.GetHex());
    }

    if (!CheckStakeKernelHash(pindexPrev, nBits, *pblockindex, txPrev, txin.prevout, tx->nTime, hashProofOfStake,
        hashProofOfStakeSource, targetProofOfStake))
    {
        return error("CheckProofOfStake() : INFO: check kernel failed on coinstake %s, hashProof=%s",
            tx->GetHash().ToString(),
            hashProofOfStake.ToString()); // may occur during initial download or if behind on block chain sync
    }

    return true;
}

#ifdef ENABLE_WALLET
bool CheckKernel(CBlockIndex *pindexPrev, unsigned int nBits, int64_t nTime, const COutPoint &prevout,
    int64_t *pBlockTime, CWallet *wallet, CDataStream &hashProofOfStakeSource)
{
    arith_uint256 hashProofOfStake, targetProofOfStake;

    CTransactionRef txPrev;
    uint256 hashBlock = uint256();
    if (!GetTransaction(prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true))
    {
        LogPrintf("CheckKernel : Could not find previous transaction %s\n", prevout.hash.ToString());
        return false;
    }

    if (mapBlockIndex.count(hashBlock) == 0)
    {
        LogPrintf("CheckKernel : Could not find block of previous transaction %s\n", hashBlock.ToString());
        return false;
    }

    CBlockIndex *pblockindex = mapBlockIndex[hashBlock];

    if (pblockindex->GetBlockTime() + Params().GetConsensus().nStakeMinAge > nTime)
    {
        return false;
    }

    if (pBlockTime)
    {
        *pBlockTime = pblockindex->GetBlockTime();
    }

    if (!wallet->mapWallet.count(prevout.hash))
    {
        return ("CheckProofOfStake(): Couldn't get Tx Index");
    }

    return CheckStakeKernelHash(pindexPrev, nBits, *pblockindex, txPrev,
        prevout, nTime, hashProofOfStake, hashProofOfStakeSource, targetProofOfStake);
}
#endif

bool CheckStakeKernelHash(CBlockIndex *pindexPrev, unsigned int nBits, CBlockIndex &blockFrom,
    CTransactionRef const &txPrev, COutPoint const &prevout, unsigned int nTimeTx, arith_uint256 &hashProofOfStake,
    CDataStream &hashProofOfStakeSource, arith_uint256 &targetProofOfStake, bool fPrintProofOfStake)
{
    unsigned int nTimeBlockFrom = blockFrom.GetBlockTime();

    if (nTimeTx < txPrev->nTime)
    {
        LogPrintf(" === ERROR: CheckStakeKernelHash() : nTime violation");
        return error("CheckStakeKernelHash() : nTime violation");
    }

    if (nTimeBlockFrom + Params().GetConsensus().nStakeMinAge > nTimeTx)
    {
        LogPrintf(" === ERROR: CheckStakeKernelHash() : min age violation");
        return error("CheckStakeKernelHash() : min age violation");
    }

    // Base target
    arith_uint256 bnTarget;
    bnTarget.SetCompact(nBits);

    // Weighted target
    int64_t nValueIn = txPrev->vout[prevout.n].nValue;
    arith_uint256 bnWeight = std::min(
        nValueIn, Params().GetConsensus().nStakeMaximumThreshold);
    bnTarget *= bnWeight;

    targetProofOfStake = bnTarget;

    uint64_t nStakeModifier = pindexPrev->nStakeModifier;
    int nStakeModifierHeight = pindexPrev->nHeight;
    int64_t nStakeModifierTime = pindexPrev->nTime;

    // Calculate hash
    CDataStream ss(SER_GETHASH, 0);
    ss << nStakeModifier << nTimeBlockFrom << txPrev->nTime << prevout.hash << prevout.n << nTimeTx;
    hashProofOfStakeSource = ss;
    hashProofOfStake = UintToArith256(Hash(ss.begin(), ss.end()));

    if (fPrintProofOfStake)
    {
        //LogPrintf("CheckStakeKernelHash() : using modifier 0x%016x at height=%d timestamp=%s for block from timestamp=%s\n",
        //    nStakeModifier, nStakeModifierHeight,
        //    FormatISO8601DateTime(nStakeModifierTime),
        //    FormatISO8601DateTime(nTimeBlockFrom));
        //LogPrintf("CheckStakeKernelHash() : check modifier=0x%016x nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s bnTarget=%s nBits=%08x nValueIn=%d bnWeight=%s\n",
        //    nStakeModifier,
        //    nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
        //    hashProofOfStake.ToString(),bnTarget.ToString(), nBits, nValueIn,bnWeight.ToString());
    }

    // Now check if proof-of-stake hash meets target protocol
    if (hashProofOfStake > bnTarget)
    {
        return false;
    }

    if (!fPrintProofOfStake)
    {
        LogPrintf(
            "CheckStakeKernelHash() : using modifier 0x%016x at height=%d timestamp=%s for block from timestamp=%s\n",
            nStakeModifier, nStakeModifierHeight,
            FormatISO8601DateTime(nStakeModifierTime),
            FormatISO8601DateTime(nTimeBlockFrom));
        LogPrintf(
            "CheckStakeKernelHash() : pass modifier=0x%016x nTimeBlockFrom=%u nTimeTxPrev=%u nPrevout=%u nTimeTx=%u hashProof=%s\n",
            nStakeModifier,
            nTimeBlockFrom, txPrev->nTime, prevout.n, nTimeTx,
            hashProofOfStake.ToString());
    }

    return true;
}

// Check whether the coinstake timestamp meets protocol
bool CheckCoinStakeTimestamp(int nHeight, int64_t nTimeBlock, int64_t nTimeTx)
{
    if (nHeight > 0)
    {
        return (nTimeBlock == nTimeTx) && ((nTimeTx & STAKE_TIMESTAMP_MASK) == 0);
    } else
    {
        return (nTimeBlock == nTimeTx);
    }
}

// Stake Modifier (hash modifier of proof-of-stake):
// The purpose of stake modifier is to prevent a txout (coin) owner from
// computing future proof-of-stake generated by this txout at the time
// of transaction confirmation. To meet kernel protocol, the txout
// must hash with a future stake modifier to generate the proof.
// Stake modifier consists of bits each of which is contributed from a
// selected block of a given block group in the past.
// The selection of a block is based on a hash of the block's proof-hash and
// the previous stake modifier.
// Stake modifier is recomputed at a fixed time interval instead of every
// block. This is to make it difficult for an attacker to gain control of
// additional bits in the stake modifier, even after generating a chain of
// blocks.
bool ComputeNextStakeModifier(const CBlockIndex *pindexPrev, uint64_t &nStakeModifier, bool &fGeneratedStakeModifier)
{
    nStakeModifier = 0;
    fGeneratedStakeModifier = false;
    if (!pindexPrev)
    {
        fGeneratedStakeModifier = true;
        return error("ComputeNextStakeModifier(): Could not find pindexPrev"); // genesis block's modifier is 0
    }
    // First find current stake modifier and its generation block time
    // if it's not old enough, return the same stake modifier
    int64_t nModifierTime = 0;
    if (!GetLastStakeModifier(pindexPrev, nStakeModifier, nModifierTime))
    {
        return error("ComputeNextStakeModifier: unable to get last modifier");
    }
    //LogPrintf("ComputeNextStakeModifier: prev modifier=0x%016x time=%s\n", nStakeModifier, FormatISO8601DateTime(nModifierTime));
    if (nModifierTime / Params().GetConsensus().nModifierInterval >=
        pindexPrev->GetBlockTime() / Params().GetConsensus().nModifierInterval)
    {
        return true;
    }

    // Sort candidate blocks by timestamp
    std::vector<std::pair<int64_t, uint256>> vSortedByTimestamp;
    vSortedByTimestamp.reserve(64 * Params().GetConsensus().nModifierInterval / GetTargetSpacing(pindexPrev->nHeight));
    int64_t nSelectionInterval = GetStakeModifierSelectionInterval();
    int64_t nSelectionIntervalStart = (pindexPrev->GetBlockTime() / Params().GetConsensus().nModifierInterval) *
                                      Params().GetConsensus().nModifierInterval - nSelectionInterval;
    //LogPrintf("nSelectionInterval = %d nSelectionIntervalStart = %d\n",nSelectionInterval,nSelectionIntervalStart);

    const CBlockIndex *pindex = pindexPrev;
    while (pindex && pindex->GetBlockTime() >= nSelectionIntervalStart)
    {
        vSortedByTimestamp.push_back(std::make_pair(pindex->GetBlockTime(), pindex->GetBlockHash()));
        pindex = pindex->pprev;
    }
    int nHeightFirstCandidate = pindex ? (pindex->nHeight + 1) : 0;
    std::reverse(vSortedByTimestamp.begin(), vSortedByTimestamp.end());
    std::sort(vSortedByTimestamp.begin(), vSortedByTimestamp.end());

    // Select 64 blocks from candidate blocks to generate stake modifier
    uint64_t nStakeModifierNew = 0;
    int64_t nSelectionIntervalStop = nSelectionIntervalStart;
    std::map<uint256, const CBlockIndex *> mapSelectedBlocks;
    for (int nRound = 0; nRound < std::min(64, (int) vSortedByTimestamp.size()); nRound++)
    {
        // add an interval section to the current selection round
        nSelectionIntervalStop += GetStakeModifierSelectionIntervalSection(nRound);
        // select a block from the candidates of current round
        if (!SelectBlockFromCandidates(vSortedByTimestamp, mapSelectedBlocks, nSelectionIntervalStop, nStakeModifier,
            &pindex))
        {
            return error("ComputeNextStakeModifier: unable to select block at round %d", nRound);
        }
        // write the entropy bit of the selected block
        nStakeModifierNew |= (((uint64_t) pindex->GetStakeEntropyBit()) << nRound);
        // add the selected block from candidates to selected list
        mapSelectedBlocks.insert(std::make_pair(pindex->GetBlockHash(), pindex));
        //        LogPrint("stakemodifier", "ComputeNextStakeModifier: selected round %d stop=%s height=%d bit=%d\n", nRound, DateTimeStrFormat("%Y-%m-%d %H:%M:%S", nSelectionIntervalStop), pindex->nHeight, pindex->GetStakeEntropyBit());
    }

    // Print selection map for visualization of the selected blocks
    if (LogAcceptCategory(BCLog::STAKEMODIF))
    {
        std::string strSelectionMap = "";
        // '-' indicates proof-of-work blocks not selected
        strSelectionMap.insert(0, pindexPrev->nHeight - nHeightFirstCandidate + 1, '-');
        pindex = pindexPrev;
        while (pindex && pindex->nHeight >= nHeightFirstCandidate)
        {
            // '=' indicates proof-of-stake blocks not selected
            if (pindex->IsProofOfStake())
            {
                strSelectionMap.replace(pindex->nHeight - nHeightFirstCandidate, 1, "=");
            }
            pindex = pindex->pprev;
        }
        for (auto &item : mapSelectedBlocks)
        {
            // 'S' indicates selected proof-of-stake blocks
            // 'W' indicates selected proof-of-work blocks
            strSelectionMap.replace(item.second->nHeight - nHeightFirstCandidate, 1,
                item.second->IsProofOfStake() ? "S" : "W");
        }
    }

    nStakeModifier = nStakeModifierNew;
    fGeneratedStakeModifier = true;
    return true;
}

// Get the last stake modifier and its generation time from a given block
static bool GetLastStakeModifier(const CBlockIndex *pindex, uint64_t &nStakeModifier, int64_t &nModifierTime)
{
    if (!pindex)
        return error("GetLastStakeModifier: null pindex");
    while (pindex && pindex->pprev && !pindex->GeneratedStakeModifier())
        pindex = pindex->pprev;
    if (!pindex->GeneratedStakeModifier())
    {
        nStakeModifier = 1;
        nModifierTime = pindex->GetBlockTime();
        return true;
    }
    nStakeModifier = pindex->nStakeModifier;
    nModifierTime = pindex->GetBlockTime();
    return true;
}

// Get selection interval section (in seconds)
static int64_t GetStakeModifierSelectionIntervalSection(int nSection)
{
    assert(nSection >= 0 && nSection < 64);
    return (Params().GetConsensus().nModifierInterval * 63 / (63 + ((63 - nSection) * (MODIFIER_INTERVAL_RATIO - 1))));
}

// Get stake modifier selection interval (in seconds)
static int64_t GetStakeModifierSelectionInterval()
{
    int64_t nSelectionInterval = 0;
    for (int nSection = 0; nSection < 64; nSection++)
        nSelectionInterval += GetStakeModifierSelectionIntervalSection(nSection);
    return nSelectionInterval;
}

// select a block from the candidate blocks in vSortedByTimestamp, excluding
// already selected blocks in vSelectedBlocks, and with timestamp up to
// nSelectionIntervalStop.
static bool SelectBlockFromCandidates(std::vector<std::pair<int64_t, uint256>> &vSortedByTimestamp, std::map<uint256,
    const CBlockIndex *> &mapSelectedBlocks, int64_t nSelectionIntervalStop, uint64_t nStakeModifierPrev,
    const CBlockIndex **pindexSelected)
{
    bool fSelected = false;
    uint256 hashBest = uint256();
    *pindexSelected = (const CBlockIndex *) 0;
    for (auto &item : vSortedByTimestamp)
    {
        if (!mapBlockIndex.count(item.second))
            return error("SelectBlockFromCandidates: failed to find block index for candidate block %s",
                item.second.ToString());
        const CBlockIndex *pindex = mapBlockIndex[item.second];
        if (fSelected && pindex->GetBlockTime() > nSelectionIntervalStop)
        {
            break;
        }

        if (mapSelectedBlocks.count(pindex->GetBlockHash()) > 0)
            continue;
        // compute the selection hash by hashing its proof-hash and the
        // previous proof-of-stake modifier
        CDataStream ss(SER_GETHASH, 0);
        ss << ArithToUint256(pindex->hashProof) << nStakeModifierPrev;
        uint256 hashSelection = Hash(ss.begin(), ss.end());

        // the selection hash is divided by 2**32 so that proof-of-stake block
        // is always favored over proof-of-work block. this is to preserve
        // the energy efficiency property
        if (pindex->IsProofOfStake())
        {
            hashSelection = ArithToUint256(UintToArith256(hashSelection) >> 32);
        }

        if (fSelected && hashSelection < hashBest)
        {
            hashBest = hashSelection;
            *pindexSelected = (const CBlockIndex *) pindex;
        } else if (!fSelected)
        {
            fSelected = true;
            hashBest = hashSelection;
            *pindexSelected = (const CBlockIndex *) pindex;
        }
    }
    return fSelected;
}

bool TransactionGetCoinAge(CTransactionRef transaction, uint64_t &nCoinAge)
{
    CAmount bnCentSecond = 0; // coin age in the unit of cent-seconds
    nCoinAge = 0;

    if (transaction->IsCoinBase())
    {
        return true;
    }

    for (auto txin : transaction->vin)
    {
        // First try finding the previous transaction in database
        CTransactionRef txPrev;
        uint256 hashBlock = uint256();

        if (!GetTransaction(txin.prevout.hash, txPrev, Params().GetConsensus(), hashBlock, true))
            continue; // previous transaction not in main chain

        if (transaction->nTime < txPrev->nTime)
            return false; // Transaction timestamp violation

        if (mapBlockIndex.count(hashBlock) == 0)
            return false; //Block not found

        CBlockIndex *pblockindex = mapBlockIndex[hashBlock];

        if (pblockindex->nTime + Params().GetConsensus().nStakeMinAge > transaction->nTime)
            continue; // only count coins meeting min age requirement

        int64_t nValueIn = txPrev->vout[txin.prevout.n].nValue;
        bnCentSecond += CAmount(nValueIn) * (transaction->nTime - txPrev->nTime) / CENT;
    }

    CAmount bnCoinDay = ((bnCentSecond * CENT) / COIN) / (24 * 60 * 60);
    nCoinAge = bnCoinDay;

    return true;
}

bool GetRatingRewards(CAmount nCredit, std::vector<CTxOut> &results, CAmount &totalAmount,
    const CBlockIndex *pindexPrev, CDataStream &hashProofOfStakeSource, std::vector<opcodetype> &winner_types,
    const CBlock *block)
{
    // Read previous block data from disk
    CBlock blockPrev;
    ReadBlockFromDisk(blockPrev, pindexPrev, Params().GetConsensus());

    // Get all winners from block
    // LotteryFactory get actual version of consensus service by current height
    auto lotteryConsensus = lotteryConsensusFactory.Instance(pindexPrev->nHeight);

    auto winners = lotteryConsensus->Winners(blockPrev, hashProofOfStakeSource);

    // Generate new outs in transaction for all winners
    auto result = true;
    result &= GenerateOuts(nCredit, results, winners.PostWinners, lotteryConsensus, OP_WINNER_POST, totalAmount, winner_types);
    result &= GenerateOuts(nCredit, results, winners.CommentWinners, lotteryConsensus, OP_WINNER_COMMENT, totalAmount, winner_types);
    result &= GenerateOuts(nCredit, results, winners.PostReferrerWinners, lotteryConsensus, OP_WINNER_POST_REFERRAL, totalAmount, winner_types);
    result &= GenerateOuts(nCredit, results, winners.CommentReferrerWinners, lotteryConsensus, OP_WINNER_COMMENT_REFERRAL, totalAmount, winner_types);

    return result;
}

bool GenerateOuts(CAmount nCredit,
    std::vector<CTxOut>& results,
    std::vector<std::string>& winners,
    std::shared_ptr<PocketConsensus::LotteryConsensus>& lotteryInst,
    opcodetype type,
    CAmount& totalAmount,
    std::vector<opcodetype>& winner_types)
{
    if (winners.empty())
        return true;

    CAmount ratingReward = lotteryInst->RatingReward(nCredit, type);
    totalAmount += ratingReward;

    int current = 0;
    const int rewardsCount = (int)winners.size();
    CAmount rewardsPool = ratingReward;
    CAmount reward = ratingReward / rewardsCount;

    for (const auto &addr : boost::adaptors::reverse(winners))
    {
        CAmount re;
        if (++current == rewardsCount)
        {
            re = rewardsPool;
        }
        else
        {
            rewardsPool = rewardsPool - reward;
            re = reward;
        }

        CTxDestination dest = DecodeDestination(addr);
        CScript scriptPubKey = GetScriptForDestination(dest);
        results.emplace_back(re, scriptPubKey);

        lotteryInst->ExtendWinnerTypes(type, winner_types);
    }

    return true;
}


#ifndef BOOST_QVM_GEN_VEC_MAT_OPERATIONS2_HPP_INCLUDED
#define BOOST_QVM_GEN_VEC_MAT_OPERATIONS2_HPP_INCLUDED

// Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/config.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>

namespace boost { namespace qvm {

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==2 && mat_traits<A>::cols==2 &&
    vec_traits<B>::dim==2,
    deduce_vec2<A,B,2> >::type
operator*( A const & a, B const & b )
    {
    typedef typename mat_traits<A>::scalar_type Ta;
    typedef typename vec_traits<B>::scalar_type Tb;
    Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
    Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
    Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
    Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
    Tb const b0 = vec_traits<B>::template read_element<0>(b);
    Tb const b1 = vec_traits<B>::template read_element<1>(b);
    typedef typename deduce_vec2<A,B,2>::type R;
    BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim==2);
    R r;
    write_vec_element<0>(r,a00*b0+a01*b1);
    write_vec_element<1>(r,a10*b0+a11*b1);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_mv_defined;

    template <>
    struct
    mul_mv_defined<2,2>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<B>::rows==2 && mat_traits<B>::cols==2 &&
    vec_traits<A>::dim==2,
    deduce_vec2<A,B,2> >::type
operator*( A const & a, B const & b )
    {
    typedef typename vec_traits<A>::scalar_type Ta;
    typedef typename mat_traits<B>::scalar_type Tb;
    Ta const a0 = vec_traits<A>::template read_element<0>(a);
    Ta const a1 = vec_traits<A>::template read_element<1>(a);
    Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
    Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
    Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
    Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
    typedef typename deduce_vec2<A,B,2>::type R;
    BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim==2);
    R r;
    write_vec_element<0>(r,a0*b00+a1*b10);
    write_vec_element<1>(r,a0*b01+a1*b11);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_vm_defined;

    template <>
    struct
    mul_vm_defined<2,2>
        {
        static bool const value=true;
        };
    }

} }

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIGNNZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIGNNZ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the sign of x. I.e. -1 or 1,
    depending on the fact  @c x is negative or positive.

    @par Header <boost/simd/function/signnz.hpp>
    @par Notes:

    - This function never returns @ref Zero (zero is considered positive for integers).

    - For floating point numbers the bit of sign is taken into account and so
    we always have `signnz(-z) == -signnz(z)` except is the entry is a NaN
    (in this later case the result is undefined).

    @see Mzero, sign, is_negative, is_positive

    @par Example:

      @snippet signnz.cpp signnz

    @par Possible output:

      @snippet signnz.txt signnz

  **/
  Value signnz(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/signnz.hpp>
#include <boost/simd/function/simd/signnz.hpp>

#endif


#include "Point.h"

#include <iostream>
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <iterator>
#include <string>
#include <string.h>
#include <algorithm>
#include <math.h>
using namespace std;

Point::Point()
{
}

Point::Point(long long id, float x, float y)
{
    this->id = id;
    this->x = x;
    this->y = y;
}

bool Point::operator==(const Point &point)
{
    if (this == &point)
    {
        return true;
    }
    else if (this->x == point.x && this->y == point.y)
    {
        return true;
    }
    return false;
}

float Point::cal_dist(Point point)
{
    if (temp_dist == 0)
        temp_dist = sqrt(pow((point.x - x), 2) + pow((point.y - y), 2));
    return temp_dist;
}

void Point::print()
{
    cout << "(x=" << x << ",y=" << y << ")" << " index=" << index << " curve_val=" << curve_val << endl;
}

vector<Point> Point::get_points(vector<Point> dataset, int num)
{
    srand(time(0));
    int length = dataset.size();
    vector<Point> points;
    for (int i = 0; i < num; i++)
    {
        int index = rand() % length;
        points.push_back(dataset[index]);
    }
    return points;
}

vector<Point> Point::get_inserted_points(long long num)
{
    srand(time(0));
    vector<Point> points;
    for (int i = 0; i < num; i++)
    {
        float x = (float)(rand() % num) / num;
        float y = (float)(rand() % num) / num;
        Point point(0, x, y);
        points.push_back(point);
    }
    return points;
}

string Point::get_self()
{
    return to_string(x) + "," + to_string(y) + "\n";
}


#ifndef INTERSIM_SOLVER_MATRIX_H
#define INTERSIM_SOLVER_MATRIX_H

#include <memory>
#include <Eigen/Sparse>
#include <Data/State.hpp>
#include <Solver/Solver.hpp>

namespace InterSim
{
	typedef Eigen::SparseLU<sparse_matrix> sparse_solver;

	class Solver_Matrix : public Solver
	{
	public:
		Solver_Matrix(std::shared_ptr<State> state_);

		void solve() override;

	private:
		void set_matrix();

		sparse_solver solver;
		std::shared_ptr<sparse_matrix> A;
		std::shared_ptr<field> b;
		std::shared_ptr<field> x;
	};
}

#endif


#include <NTL/GF2.h>

#include <NTL/new.h>

NTL_START_IMPL

void div(GF2& x, GF2 a, GF2 b)
{
   if (b == 0) Error("GF2: division by zero");
   x = a;
}

void div(GF2& x, long a, GF2 b)
{
   if (b == 0) Error("GF2: division by zero");
   x = a;
}

void div(GF2& x, GF2 a, long b)
{
   if ((b & 1) == 0) Error("GF2: division by zero");
   x = a;
}

void inv(GF2& x, GF2 a)
{
   if (a == 0) Error("GF2: division by zero");
   x = a;
}

void power(GF2& x, GF2 a, long e)
{
   if (e == 0) {
      x = 1;
      return;
   }

   if (e < 0 && a == 0)
      Error("GF2: division by zero");

   x = a;
}

ostream& operator<<(ostream& s, GF2 a)
{
   if (a == 0)
      s << "0";
   else
      s << "1";

   return s;
}

istream& operator>>(istream& s, GF2& x)
{
   static ZZ a;

   s >> a;
   conv(x, a);
   return s;
}

NTL_END_IMPL


/*
 * Copyright (c) 2012 Jonathan Perry
 * This code is released under the MIT license (see LICENSE file).
 */
#include "codes/strider/LayerManipulator.h"

#include <stdexcept>
#include <stdint.h>
#include <assert.h>
#include <math.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>

using namespace boost::numeric::ublas;

LayerManipulator::LayerManipulator(
		unsigned int layerLength,
		std::complex<double> *matrixG,
		int rowsG,
		int colsG)
:	m_G(rowsG, colsG),
 	m_symbols(rowsG, layerLength),
 	m_fading(scalar_matrix<FadingMagnitude>(rowsG, layerLength, 1.0)),
 	m_numFullPasses(0),
 	m_nonFullPassLength(0)
{
	for(int row = 0; row < rowsG; row++) {
		for(int col = 0; col < colsG; col++) {
			m_G(row, col) = *(matrixG++);
		}
	}
}

void LayerManipulator::setSymbols(
		unsigned int numFullPasses,
		unsigned int nonFullPassLength,
		const boost_col_cmat & symbols)
{
	const unsigned int passLength = m_symbols.size2();
	m_numFullPasses = numFullPasses;
	m_nonFullPassLength = nonFullPassLength;

	// Copy short columns
	subrange(m_symbols, 0, m_numFullPasses,
						m_nonFullPassLength, passLength) =
		subrange(symbols, 0, m_numFullPasses,
						  m_nonFullPassLength, passLength);

	if(m_nonFullPassLength > 0) {
		// Copy long columns
		subrange(m_symbols, 0, m_numFullPasses+1, 0, m_nonFullPassLength) =
			subrange(symbols, 0, m_numFullPasses+1, 0, m_nonFullPassLength);
	}
}

void LayerManipulator::setSymbols(
		unsigned int numFullPasses,
		unsigned int nonFullPassLength,
		const boost_col_cmat & symbols,
		const boost_col_mat & fading)
{
	// Set the symbols
	setSymbols(numFullPasses, nonFullPassLength, symbols);

	// Set the fading coefficients
	// Copy short columns
	const unsigned int passLength = m_symbols.size2();
	subrange(m_fading, 0, m_numFullPasses,
						m_nonFullPassLength, passLength) =
		subrange(fading, 0, m_numFullPasses,
						  m_nonFullPassLength, passLength);

	if(m_nonFullPassLength > 0) {
		// Copy long columns
		subrange(m_fading, 0, m_numFullPasses+1, 0, m_nonFullPassLength) =
			subrange(fading, 0, m_numFullPasses+1, 0, m_nonFullPassLength);
	}
}

void LayerManipulator::maximalRatioCombining(
		unsigned int layerInd,
		float externalSnr,
		boost_cvec & combined,
		boost_vec & combinedScale)
{
	const unsigned int passLength = m_symbols.size2();

	maximalRatioCombiningPartial(layerInd,
	                             externalSnr,
	                             combined,
	                             combinedScale,
	                             m_nonFullPassLength,
	                             passLength,
	                             m_numFullPasses);

	if(m_nonFullPassLength > 0) {
		// We have symbols in a non-full pass, need to combine them as well
		maximalRatioCombiningPartial(layerInd,
		                             externalSnr,
		                             combined,
		                             combinedScale,
		                             0,
		                             m_nonFullPassLength,
		                             m_numFullPasses + 1);
	}
}

void LayerManipulator::subtractLayer(
		unsigned int layerInd,
		const std::vector<ComplexSymbol> & symbols)
{
	if(symbols.size() != m_symbols.size2()) {
		throw(std::runtime_error("Number of symbols to subtract should be equal to a layer's length"));
	}

	// full passes
	for(unsigned int i = m_nonFullPassLength; i < m_symbols.size2(); i++) {
		ComplexNumber sym = symbols[i];
		for(unsigned int pass = 0; pass < m_numFullPasses; pass++) {
			m_symbols(pass,i) -= sym * m_G(pass, layerInd) * m_fading(pass,i);
		}
	}

	// non full passes
	for(unsigned int i = 0; i < m_nonFullPassLength; i++) {
		ComplexNumber sym = symbols[i];
		for(unsigned int pass = 0; pass < m_numFullPasses + 1; pass++) {
			m_symbols(pass,i) -= sym * m_G(pass, layerInd) * m_fading(pass,i);
		}
	}
}

void LayerManipulator::maximalRatioCombiningPartial(unsigned int layerInd,
													float externalSnr,
													boost_cvec & combined,
													boost_vec & combinedScale,
													unsigned int beginIndex,
													unsigned int endIndex,
													unsigned int numPasses)
{
	boost_double_vec fading(
			matrix_vector_slice<boost_col_mat> (m_fading,
												slice(0,1,numPasses),
												slice(beginIndex,0,numPasses)));
	uint32_t fadingIndex = beginIndex;

	for(uint32_t i = beginIndex + 1; i < endIndex; i++) {
		matrix_vector_slice<boost_col_mat> thisFading(
												m_fading,
												slice(0,1,numPasses),
												slice(i,0,numPasses));
		for(uint32_t j = 0; j < numPasses; j++) {
			if(thisFading[j] != fading[j]) {
				// Run maximal ratio combining on previous sequence of indices,
				//  fadingIndex, fadingIndex+1, .., i-1
				maximalRatioCombiningSameFading(layerInd,
												externalSnr,
												fading,
												combined,
												combinedScale,
												fadingIndex,
												i,
												numPasses);
				// Update state so the fading information for index i will be cached
				fading = thisFading;
				fadingIndex = i;

				// no need to check more indices
				break;
			}
		}
	}

	// We finished the for loop, there is still another combining to do
	maximalRatioCombiningSameFading(layerInd,
								    externalSnr,
								    fading,
								    combined,
								    combinedScale,
								    fadingIndex,
								    endIndex,
								    numPasses);
}

void LayerManipulator::maximalRatioCombiningSameFading(
		unsigned int layerInd,
		float externalSnr,
		const boost_double_vec & fading,
		boost_cvec & combined,
		boost_vec & combinedScale,
		unsigned int beginIndex,
		unsigned int endIndex,
		unsigned int numPasses)
{
	// debug print
//	cout << "MRC indices [" << beginIndex << ", " << endIndex << ")"
//			<< " num passes=" << numPasses << endl;

	// Stores the interference from other layers
	boost_vec interference;

	// Weights to combine the passes coherently
	boost_cvec weights;

	// Get the internal interference power for each of the layers
	getInterference(layerInd, numPasses, interference);

	// Calculate normalized weights
	float signalPower = getWeights(layerInd,
									externalSnr,
									interference,
									fading,
									weights);

	if (signalPower == 0) {
		subrange(combined, beginIndex, endIndex) =
				scalar_vector<float>(endIndex - beginIndex, 0);
		subrange(combinedScale, beginIndex, endIndex) =
				scalar_vector<float>(endIndex - beginIndex, 0);
	} else {
		// Calculate SNRs of combined signal after coherent combining of
		// interference
		float interferencePower = getCoherentInterference(layerInd, weights, fading);

		// Calculate normalized noise power
		float noisePower = externalSnr * real(inner_prod(weights, conj(weights)));

		// Scaling factor makes the noise+interference be of power externalSnr
		float noiseNormalizationFactor =
				sqrt(externalSnr / (noisePower + interferencePower));
		assert(isfinite(noiseNormalizationFactor));

		// combinedScale is the total scaling from combinedScalingFactor and
		// the weights
		float scale = sqrt(signalPower) * noiseNormalizationFactor;
		assert(isfinite(scale));
		subrange(combinedScale, beginIndex, endIndex) =
				scalar_vector<float>(endIndex - beginIndex, scale);

		// Combine the different passes into one coherent reception
		subrange(combined, beginIndex, endIndex) =
				   double(noiseNormalizationFactor) *
				   prod(weights,
						subrange(m_symbols, 0, numPasses,
											beginIndex, endIndex));
		// debug print
//		cout << "combined_scale=" << scale
//			 << " signal_power=" << signalPower
//			 << " interference_power=" << interferencePower
//			 << " noise_norm=" << noiseNormalizationFactor
//				<< endl;

	}
}

void LayerManipulator::getInterference(	unsigned int layerInd,
										unsigned int numPasses,
										boost_vec & interference)
{
	matrix_range<boost_col_cmat> coeffs(
			subrange(m_G, 0, numPasses, layerInd+1, m_G.size2()));

	if(layerInd != m_G.size2() - 1) {
		interference =
			real(
				prod(element_prod(coeffs, conj(coeffs)),
					 scalar_vector<ComplexBaseType>(m_G.size2() - (layerInd + 1), 1.0f)));
	} else {
		interference = scalar_vector<float>(numPasses, 0);
	}
}

float LayerManipulator::getWeights(
		unsigned int layerInd,
		float noise,
		const boost_vec & interference,
		const boost_double_vec & fading,
		boost_cvec & weights)
{
	const unsigned int num_passes = interference.size();

	matrix_vector_slice<boost_col_cmat> layerCoeff(m_G,
												   slice(0,1,num_passes),
												   slice(layerInd,0,num_passes));
	// Calculate un-normalized weights
	weights = element_div(element_prod(conj(layerCoeff), fading),
						  ( element_prod(interference, element_prod(fading,fading))
						   + scalar_vector<ComplexBaseType>(num_passes,noise)));

	float scale = real(inner_prod(weights, element_prod(layerCoeff, fading)));
	return scale*scale;
}

float LayerManipulator::getCoherentInterference(
		unsigned int layerInd,
		const boost_cvec & weights,
		const boost_double_vec & fading)
{
	const unsigned int num_passes = weights.size();

	// Get interference power if not on the last layer
	float interferencePower = 0;
	if(layerInd != m_G.size2() - 1) {
		boost_cvec layerInterference =
			prod(element_prod(weights,fading),
				 subrange(m_G, 0, num_passes, layerInd+1, m_G.size2()));

		interferencePower = real(inner_prod(layerInterference,
											conj(layerInterference)));
	}

	return interferencePower;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2009-2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

#define BOOST_GEOMETRY_REPORT_OVERLAY_ERROR
#define BOOST_GEOMETRY_NO_BOOST_TEST

#include <test_overlay_p_q.hpp>

#include <boost/program_options.hpp>
#include <boost/timer.hpp>

template <typename Polygon>
inline void make_pie(Polygon& polygon,
    int count, int offset, int offset_y, double factor1, int total_segment_count = 36)
{
    typedef typename bg::point_type<Polygon>::type p;
    typedef typename bg::select_most_precise
        <
            typename bg::coordinate_type<Polygon>::type,
            long double
        >::type coordinate_type;

    // Create pie
    coordinate_type cx = 2500.0;
    coordinate_type cy = 2500.0 + offset_y;

    bg::exterior_ring(polygon).push_back(bg::make<p>(int(cx), int(cy)));

    coordinate_type const dx = 5000.0;
    coordinate_type const dy = 5000.0;

    coordinate_type const half = 0.5;
    coordinate_type const two = 2.0;

    coordinate_type const a = coordinate_type(factor1) * half * dx;
    coordinate_type const b = coordinate_type(factor1) * half * dy;

    coordinate_type const pi = boost::math::constants::pi<long double>();
    coordinate_type const delta = pi * two / total_segment_count;
    coordinate_type angle = coordinate_type(offset) * delta;
    for (int i = 0; i < count; i++, angle += delta)
    {
        coordinate_type const s = sin(angle);
        coordinate_type const c = cos(angle);
        coordinate_type const x = cx + a * s;
        coordinate_type const y = cy + b * c;
        bg::exterior_ring(polygon).push_back(bg::make<p>(int(x), int(y)));
    }
    bg::exterior_ring(polygon).push_back(bg::make<p>(int(cx), int(cy)));
    bg::correct(polygon);
}

/*
sql server

TWO INNERS
1a) select geometry::STGeomFromText('POLYGON((0 0,0 10,10 10,10 0,0 0),(4 4,5 5,4 6,4 4),(6 6,5 5,6 4,6 6)) ', 0).STIsValid();  -> valid
same:
1b) POLYGON((0 0,0 10,10 10,10 0,0 0),(4 4,5 5,6 4,6 6,5 5,4 6,4 4))   -> valid

I-E tangency IET
2a) select geometry::STGeomFromText('POLYGON((0 0,0 10,10 10,10 0,0 0),(5 0,6 1,4 1,5 0))', 0).STIsValid(); -> valid
same by Self tangency ST
2b) select geometry::STGeomFromText('POLYGON((0 0,0 10,10 10,10 0,5 0,6 1,4 1,5 0, 0 0))', 0).STIsValid(); -> valid

Two inners all tangent
3a) POLYGON((0 0,0 10,10 10,10 0,0 0),(5 0,6 1,4 1,5 0),(5 1,6 2,4 2,5 1)) -> valid

postgis:
1a) select ST_IsValid(ST_GeomFromText('POLYGON((0 0,0 10,10 10,10 0,0 0),(4 4,5 5,4 6,4 4),(6 6,5 5,6 4,6 6))', 0)); -> valid
1b) POLYGON((0 0,0 10,10 10,10 0,0 0),(4 4,5 5,6 4,6 6,5 5,4 6,4 4))   -> NOT valid

2a) POLYGON((0 0,0 10,10 10,10 0,0 0),(5 0,6 1,4 1,5 0)) -> valid
2b) POLYGON((0 0,0 10,10 10,10 0,5 0,6 1,4 1,5 0, 0 0)) -> NOT valid (?)

3a) POLYGON((0 0,0 10,10 10,10 0,0 0),(5 0,6 1,4 1,5 0),(5 1,6 2,4 2,5 1)) -> valid

*/

template <typename Polygon>
inline void holify(Polygon& polygon)
{
    typedef typename bg::point_type<Polygon>::type point_type;

    Polygon p;
    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 0));
    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 5000));
    bg::exterior_ring(p).push_back(bg::make<point_type>(5000, 5000));
    bg::exterior_ring(p).push_back(bg::make<point_type>(5000, 0));
    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 0));
    bg::interior_rings(p).push_back(bg::exterior_ring(polygon));
    bg::correct(p);

    polygon = p;
}

template <typename MultiPolygon>
inline void holify_multi(MultiPolygon& multi_polygon)
{
    typedef typename bg::point_type<MultiPolygon>::type point_type;

    typename boost::range_value<MultiPolygon>::type p;

    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 0));
    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 5000));
    bg::exterior_ring(p).push_back(bg::make<point_type>(5000, 5000));
    bg::exterior_ring(p).push_back(bg::make<point_type>(5000, 0));
    bg::exterior_ring(p).push_back(bg::make<point_type>(0, 0));

    for (int i = 0; i < multi_polygon.size(); i++)
    {
        bg::interior_rings(p).push_back(bg::exterior_ring(multi_polygon[i]));
    }

    bg::correct(p);

    multi_polygon.clear();
    multi_polygon.push_back(p);
}

template <typename T, bool Clockwise, bool Closed>
void test_pie(int total_segment_count, T factor_p, T factor_q,
            bool multi, bool single_selftangent, p_q_settings const& settings)
{
    boost::timer t;
    typedef bg::model::d2::point_xy<T> point_type;
    typedef bg::model::polygon<point_type, Clockwise, Closed> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    int good_count = 0;
    int bad_count = 0;

    for (int a = 2; a < total_segment_count; a++)
    {
        polygon p;
        make_pie(p, a, 0, 0, factor_p, total_segment_count);

        //holify(p);

        for (int b = 2; b < total_segment_count; b++)
        {
            for (int offset = 1; offset < total_segment_count; offset++)
            {
                //for (int y = 0; y <= 2500; y += 500)
                int y = 0;
                {
                    polygon q;
                    make_pie(q, b, offset, y, factor_q, total_segment_count);

                    if (! multi)
                    {
                        //holify(q);

                        std::ostringstream out;
                        out << "pie_" << a << "_" << b << "_" << offset << "_" << y;
                        if (test_overlay_p_q<polygon, T>(out.str(), p, q, settings))
                        {
                            good_count++;
                        }
                        else
                        {
                            bad_count++;
                        }
                    }
                    else
                    {
                        int left = total_segment_count - b - 2;
                        //std::cout << a << " " << b << " " << left << std::endl;
                        for (int c = 2; c < left; c++)
                        {
                            polygon q2;
                            make_pie(q2, c, offset + b + 1, y, factor_q, total_segment_count);

                            std::ostringstream out;
                            out << "pie_" << a << "_" << b << "_" << offset << "_" << y
                                << "_" << c
                                ;

                            bool good = false;

                            // Represent as either multi-polygon, or as single-self-touching-polygon (INVALID)
                            if (single_selftangent)
                            {
                                polygon q1 = q;
                                for (unsigned int i = 1; i < q2.outer().size(); i++)
                                {
                                    q1.outer().push_back(q2.outer()[i]);
                                }
                                //holify(q1);
                                good = test_overlay_p_q<polygon, T>(out.str(), p, q1, settings);
                            }
                            else
                            {
                                multi_polygon mq;
                                mq.push_back(q);
                                mq.push_back(q2);
                                //holify_multi(mq);
                                good = test_overlay_p_q<polygon, T>(out.str(), p, mq, settings);
                            }

                            if (good)
                            {
                                good_count++;
                            }
                            else
                            {
                                bad_count++;
                            }
                        }
                    }
                }
            }
        }
    }
    std::cout
        << "Time: " << t.elapsed()  << std::endl
        << "Good: " << good_count << std::endl
        << "Bad: " << bad_count << std::endl;
}

template <typename T, bool Clockwise, bool Closed>
void test_all(bool multi, bool single_selftangent, p_q_settings const& settings)
{
    test_pie<T, Clockwise, Closed>(24, 0.55, 0.45, multi, single_selftangent, settings);
}

int main(int argc, char** argv)
{
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== intersection_pies ===\nAllowed options");

        p_q_settings settings;
        bool multi = false;
        bool ccw = false;
        bool open = false;
        bool single_selftangent = false; // keep false, true does not work!

        description.add_options()
            ("help", "Help message")
            ("multi", po::value<bool>(&multi)->default_value(false), "Multiple tangencies at one point")
            ("diff", po::value<bool>(&settings.also_difference)->default_value(false), "Include testing on difference")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help"))
        {
            std::cout << description << std::endl;
            return 1;
        }

        // template par's are: CoordinateType, Clockwise, Closed
        if (ccw && open)
        {
            test_all<double, false, false>(multi, single_selftangent, settings);
        }
        else if (ccw)
        {
            test_all<double, false, true>(multi, single_selftangent, settings);
        }
        else if (open)
        {
            test_all<double, true, false>(multi, single_selftangent, settings);
        }
        else
        {
            test_all<double, true, true>(multi, single_selftangent, settings);
        }
        //test_all<long double>();
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }
    return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/matrix/transpose_multiply.h>
#include <scitbx/math/utils.h>
#include <scitbx/array_family/tiny_types.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/format.hpp>
#include <scitbx/array_family/boost_python/flex_helpers.h>
#include <cctbx/uctbx.h>
#include <cctbx/miller.h>
#include <dials/error.h>

namespace dials { namespace af { namespace boost_python {

  using cctbx::uctbx::unit_cell;

  scitbx::af::shared<double> d(
    const scitbx::af::const_ref<unit_cell> &self,
    const scitbx::af::const_ref<cctbx::miller::index<> > &hkl,
    const scitbx::af::const_ref<std::size_t> index) {
    DIALS_ASSERT(index.size() == hkl.size());
    scitbx::af::shared<double> result(hkl.size());
    for (std::size_t i = 0; i < hkl.size(); ++i) {
      std::size_t j = index[i];
      DIALS_ASSERT(j < self.size());
      result[i] = self[j].d(hkl[i]);
    }
    return result;
  }

  void export_flex_unit_cell() {
    using namespace boost::python;
    using boost::python::arg;
    typedef scitbx::af::boost_python::flex_wrapper<unit_cell> f_w;
    f_w::plain("unit_cell").def("d", &d, (arg("hkl"), arg("id")));
    ;
  }

}}}  // namespace dials::af::boost_python


// Author: Gianni Lunardi, University of Trento 2022

#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/algorithm/frames-derivatives.hpp>
#include <armadillo>
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/residuals/obstacle-avoidance.hpp"

namespace crocoddyl {

template <typename Scalar>
ResidualModelObstacleAvoidanceTpl<Scalar>::ResidualModelObstacleAvoidanceTpl(boost::shared_ptr<StateMultibody> state,
                                                                             const std::size_t nu,
                                                                             boost::shared_ptr<GeometryModel> geom_model,
                                                                             const pinocchio::PairIndex pair_id,
                                                                             const pinocchio::FrameIndex frame_id,
                                                                             const pinocchio::ReferenceFrame type,
                                                                             const double beta)
    : Base(state, 2, nu, true, true, false),
      pin_model_(*state->get_pinocchio()),
      geom_model_(geom_model),
      pair_id_(pair_id),
      frame_id_(frame_id),
      type_(type),
      beta_(beta)  {}

template <typename Scalar>
ResidualModelObstacleAvoidanceTpl<Scalar>::~ResidualModelObstacleAvoidanceTpl()  {}

template <typename Scalar>
void ResidualModelObstacleAvoidanceTpl<Scalar>::calc(const boost::shared_ptr<ResidualDataAbstract> &data,
                                                     const Eigen::Ref<const VectorXs> &x,
                                                     const Eigen::Ref<const VectorXs> &) {
    Data* d = static_cast<Data*>(data.get());

    pinocchio::forwardKinematics(pin_model_, *d->pinocchio, x.head(nq_), x.tail(nv_));
    // Compute the distance for the collision pair
    pinocchio::updateGeometryPlacements(pin_model_, *d->pinocchio, *geom_model_.get(), d->geometry, x.head(nq_));
    pinocchio::computeDistance(*geom_model_.get(), d->geometry, pair_id_);
    // Compute the distance between the two nearest points
    d->p_diff = d->geometry.distanceResults[pair_id_].nearest_points[0] -
                d->geometry.distanceResults[pair_id_].nearest_points[1];
    d->dist = d->p_diff.norm();
    // Compute the velocity of the foot
    d->v = (pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id_, type_)).toVector();
    // Compute the residual

    if(d->geometry.distanceResults[pair_id_].min_distance > 0) {
        d->r[0] = std::pow(d->v[0], 2) / (std::sqrt(d->dist) + beta_); // usa la moltiplicazione, non pow
        d->r[1] = std::pow(d->v[1], 2) / (std::sqrt(d->dist) + beta_);
    }
    else {
        d->r[0] = std::pow(d->v[0], 2) / beta_;
        d->r[1] = std::pow(d->v[1], 2) / beta_;
    }
}

template <typename Scalar>
void ResidualModelObstacleAvoidanceTpl<Scalar>::calcDiff(const boost::shared_ptr<ResidualDataAbstract> &data,
                                                         const Eigen::Ref<const VectorXs> &,
                                                         const Eigen::Ref<const VectorXs> &) {
    Data* d = static_cast<Data*>(data.get());
    const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");

    // Compute the frame Jacobian, consider the first three rows (derivatives wrt the position of the frame)
    pinocchio::getFrameJacobian(pin_model_, *d->pinocchio, frame_id_, type_, d->J);
    Matrix3xs p_diff_der = d->J.template topRows<3>();

    // Compute the derivatives of the foot's velocity
    pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id_, type_,
                                           d->dv_dx.leftCols(nv_), d->dv_dx.rightCols(nv_));

    // IMPORTANT: translate the derivatives of the linear velocity from the WORLD to LOCAL_WORLD_ALIGNED reference frame
    Matrix6xs dv0_dx(Matrix6xs::Zero(6, state_->get_ndx()));
    pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id_, pinocchio::WORLD,
                                           dv0_dx.leftCols(nv_), dv0_dx.rightCols(nv_));

    Vector3s p = d->pinocchio->oMf[frame_id_].translation();
    Vector3s omega = d->v.tail(3);
    Matrix3s p_skew = pinocchio::skew(p);
    Matrix3s omega_skew = pinocchio::skew(omega);
    d->dv_dx.topLeftCorner(3, nv_) = dv0_dx.topLeftCorner(3, nv_) - p_skew * dv0_dx.bottomLeftCorner(3, nv_) +
                                     omega_skew * p_diff_der;
    // Compute the residual Jacobian, considering the two cases
    if(d->geometry.distanceResults[pair_id_].min_distance > 0) {
        // 1) min dist > 0 --> compute the complete derivatives of the residual
        VectorXs norm_der = d->p_diff / d->dist;
        VectorXs dist_der_dq = norm_der.transpose() * p_diff_der;
        VectorXs dist_der_dv(VectorXs::Zero(nv_, 1));
        VectorXs dist_der(dist_der_dq.size() + dist_der_dv.size());
        dist_der << dist_der_dq, dist_der_dv;    // definisci in data, poi aggiungi l'head con le derivate

        // Chain rule
        for(size_t i = 0; i < nr_; i++) {
            for(size_t j = 0; j < ndx_; j++) {
                d->Rx(i,j) = - (std::pow(d->v[i], 2) * dist_der[j]) / (2 * std::pow((beta_ + std::sqrt(d->dist)), 2) * std::sqrt(d->dist)) +
                                (2 * d->v[i] * d->dv_dx(i,j)) / (beta_ + std::sqrt(d->dist));  // evita pow
            }
        }
    }
    else {
        for(size_t i = 0; i < nr_; i++) {
            for(size_t j = 0; j < ndx_; j++) {
                d->Rx(i,j) = (2 * d->v[i] * d->dv_dx(i,j)) / beta_;
            }
        }
    }
}

template <typename Scalar>
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > ResidualModelObstacleAvoidanceTpl<Scalar>::createData(
        DataCollectorAbstract *const data) {
    return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
}

template <typename Scalar>
const pinocchio::GeometryModel &ResidualModelObstacleAvoidanceTpl<Scalar>::get_geometry() const {
    return *geom_model_.get();
}

template <typename Scalar>
pinocchio::PairIndex ResidualModelObstacleAvoidanceTpl<Scalar>::get_pair_id() const {
    return pair_id_;
}

template <typename Scalar>
pinocchio::FrameIndex ResidualModelObstacleAvoidanceTpl<Scalar>::get_frame_id() const {
    return frame_id_;
}

template <typename Scalar>
pinocchio::ReferenceFrame ResidualModelObstacleAvoidanceTpl<Scalar>::get_type() const {
    return type_;
}

template <typename Scalar>
void ResidualModelObstacleAvoidanceTpl<Scalar>::set_type(const pinocchio::ReferenceFrame type) {
    type_ = type;
}

}   // namespace crocoddyl


// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder 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 HOLDER 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 <complex>
#include <iostream>

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>

#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"
#include "badslam/cuda_depth_processing.cuh"

using namespace vis;

// Helper function to render a set of planes into a depth and normal image.
void RenderPlanes(
    const SE3f& global_tr_frame,
    int plane_count,
    Hyperplane<float, 3>* planes,
    float raw_to_float_depth,
    const PinholeCamera4f& camera,
    Image<u16>* depth_image,
    Image<Vec3u8>* color_image) {
  const Vec3f& global_t_frame = global_tr_frame.translation();
  const Mat3f global_r_frame = global_tr_frame.rotationMatrix();

  float invalid_float_depth = raw_to_float_depth * numeric_limits<u16>::max();
  depth_image->SetTo(numeric_limits<u16>::max());
  color_image->SetTo(Vec3u8(0, 0, 0));

  for (int plane_index = 0; plane_index < plane_count; ++ plane_index) {
    for (int y = 0; y < depth_image->height(); ++ y) {
      for (int x = 0; x < depth_image->width(); ++ x) {
        Vec3f frame_direction = camera.UnprojectFromPixelCenterConv(Vec2f(x, y));
        ASSERT_EQ(frame_direction.z(), 1.0f);
        ParametrizedLine<float, 3> ray(global_t_frame, global_r_frame * frame_direction);

        float z = ray.intersectionParameter(planes[plane_index]);
        float previous_measurement = raw_to_float_depth * (*depth_image)(x, y);
        if (z > 0 && (previous_measurement == invalid_float_depth || z < previous_measurement)) {
          (*depth_image)(x, y) = std::min<u32>(numeric_limits<u16>::max(), z / raw_to_float_depth + 0.5f);

          Vec3f global_position = ray.pointAt(z);
          constexpr float kFactor = 200;
          u8 cx = (255 / 2.f) * (1.f + sin(0.15f * kFactor * global_position.x() + 0.5f * sin(0.25f * kFactor * global_position.y()))) + rand() % 1;
          u8 cy = (255 / 2.f) * (1.f + sin(0.15f * kFactor * global_position.y() + 0.5f * sin(0.25f * kFactor * global_position.z()))) + rand() % 1;
          u8 cz = (255 / 2.f) * (1.f + sin(0.15f * kFactor * global_position.z() + 0.5f * sin(0.25f * kFactor * global_position.x()))) + rand() % 1;
          (*color_image)(x, y) = Vec3u8(cx, cy, cz);
        }
      }
    }
  }

  // Uncomment to debug:
//   static ImageDisplay depth_debug_display;
//   depth_debug_display.Update(*depth_image, "debug: depth image", static_cast<u16>(0.f), static_cast<u16>(3.0f / raw_to_float_depth));
//   static ImageDisplay color_debug_display;
//   color_debug_display.Update(*color_image, "debug: color image");
//   std::getchar();
}

// Intrinsics optimization test. Creates a scene consisting of
// a set of planes, then renders some keyframes from this geometry and distorts
// their color intrinsics. Applies bundle adjustment and tests whether this
// distortion can be compensated.
//
// It should be straightforward to use this test code as a template to apply
// direct bundle adjustment on a set of images. Simply load the depth and color
// images from files instead of generating them, and supply initial estimates
// for the keyframe poses.
static void TestIntrinsicsOptimizationWithPhotometricResidual(bool use_pcg) {
  srand(0);

  // Initialize the camera (here we use the same camera intrinsics for the depth
  // and color camera).
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.45f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Initialize a CUDA stream.
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Optional: Create a render window for debugging.
  constexpr bool show_visualization = false;

  shared_ptr<BadSlamRenderWindow> render_window;  // render window callbacks
  shared_ptr<RenderWindow> render_window_ui;  // render window UI object
  OpenGLContext opengl_context;  // context for the main thread
  OpenGLContext opengl_context_2;  // context for the BA thread
  OpenGLContext no_opengl_context;  // stores the original (non-existing) context

  if (show_visualization) {
    float splat_half_extent_in_pixels = 3.0f;
    int render_window_default_width = 1280;
    int render_window_default_height = 720;

    // Warning: do not use make_shared() here to allocate render_window, since
    // it might ignore the class' aligned allocator, leading to potential
    // crashes in Eigen operations later on render_window's members.
    render_window = shared_ptr<BadSlamRenderWindow>(
        new BadSlamRenderWindow(splat_half_extent_in_pixels));
    render_window_ui = RenderWindow::CreateWindow(
        "BAD SLAM Test", render_window_default_width, render_window_default_height,
        RenderWindow::API::kOpenGL, render_window);
    render_window->InitializeForCUDAInterop(
        /*max_surfel_count*/ 1000 * 1000,
        &opengl_context,
        &opengl_context_2,
        camera);
    SwitchOpenGLContext(opengl_context, &no_opengl_context);
  }

  // Distort color intrinsics
  const float distorted_camera_parameters[4] = {0.5f * height + 0.5f, 0.45f * height - 0.6f, 0.5f * width - 0.5f + 1.23f, 0.5f * height - 0.5f - 2.17f};
  PinholeCamera4f distorted_color_camera(width, height, distorted_camera_parameters);

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 1000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 2.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 2,
      /*min_observation_count_while_bootstrapping_2*/ 2,
      /*min_observation_count*/ 2,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ false,  // NOTE: disabling geometric residuals!
      /*use_descriptor_residuals*/ true,
      render_window,
      /*global_T_anchor_frame*/ SE3f());

  if (show_visualization) {
    render_window->SetDirectBA(&direct_ba);
  }

  // Set the first keyframe's pose
  Matrix<float, 6, 1> global_tr_frame_log;
  global_tr_frame_log << 0.01f, 0.02f, 0.03f, 0.004f, 0.005f, 0.006f;
  SE3f global_tr_frame_0 = SE3f::exp(global_tr_frame_log);

  // Generate test geometry consisting of some random planes
  constexpr int kPlaneCount = 20;
  Hyperplane<float, 3> planes[kPlaneCount];

  for (int plane_index = 0; plane_index < kPlaneCount; ++ plane_index) {
    Vec3f plane_normal = Vec3f::Random();
    plane_normal.z() = -1.f;
    plane_normal.normalize();
    planes[plane_index] = Hyperplane<float, 3>(plane_normal, 2.5f);
  }

  // Allocate buffers for rendering the planes into
  Image<u16> depth_image(width, height);
  Image<Vec3u8> color_image(width, height);

  // Create keyframes observing the surface from different distances
  constexpr int kNumKeyframes = 12;
  RGBDVideo<Vec3u8, u16> rgbd_video;

  SE3f last_frame_tr_global = SE3f();
  for (int i = 0; i < kNumKeyframes; ++ i) {
    Matrix<float, 6, 1> frame_0_T_frame_log;
    frame_0_T_frame_log <<
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.0f * ((rand() % 200) / 200.f - 0.5f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f),
        3.5f * (((rand() % 200) - 100) / 500.f);
    SE3f frame_0_T_frame = SE3f::exp(frame_0_T_frame_log);
    SE3f global_tr_frame = global_tr_frame_0 * frame_0_T_frame;

    RenderPlanes(global_tr_frame, kPlaneCount, planes,
                 raw_to_float_depth, camera, &depth_image, &color_image);

    shared_ptr<Keyframe> keyframe(new Keyframe(
        stream,
        /*frame_index*/ i,
        direct_ba.depth_params(),
        direct_ba.depth_camera(),
        depth_image,
        color_image,
        global_tr_frame));
    direct_ba.AddKeyframe(keyframe);
    last_frame_tr_global = keyframe->frame_T_global();
  }

  for (auto& keyframe : direct_ba.keyframes()) {
    direct_ba.CreateSurfelsForKeyframe(stream, true, keyframe);
  }
  direct_ba.SetColorCamera(distorted_color_camera);

  // Optimize intrinsics
  for (int i = 0; i < 10; ++ i) {
    direct_ba.BundleAdjustment(
        stream,
        /*optimize_depth_intrinsics*/ false,
        /*optimize_color_intrinsics*/ true,
        /*do_surfel_updates*/ true,
        /*optimize_poses*/ false,
        /*optimize_geometry*/ false,
        /*min_iterations*/ 1,
        /*max_iterations*/ 10,
        use_pcg,
        /*active_keyframe_window_start*/ 0,
        /*active_keyframe_window_end*/ direct_ba.keyframes().size() - 1,
        /*increase_ba_iteration_count*/ i != 0);

    if (show_visualization) {
      std::getchar();
    }

    // Uncomment this to print the current state at each iteration:
    PinholeCamera4f estimated_camera = direct_ba.color_camera();
    LOG(INFO) << "camera_difference: " << (estimated_camera.parameters()[0] - camera.parameters()[0])
                                      << ", " << (estimated_camera.parameters()[1] - camera.parameters()[1])
                                      << ", " << (estimated_camera.parameters()[2] - camera.parameters()[2])
                                      << ", " << (estimated_camera.parameters()[3] - camera.parameters()[3]);
  }

  PinholeCamera4f estimated_camera = direct_ba.color_camera();
  EXPECT_NEAR(camera.parameters()[0], estimated_camera.parameters()[0], 0.03);
  EXPECT_NEAR(camera.parameters()[1], estimated_camera.parameters()[1], 0.03);
  EXPECT_NEAR(camera.parameters()[2], estimated_camera.parameters()[2], 0.15);
  EXPECT_NEAR(camera.parameters()[3], estimated_camera.parameters()[3], 0.15);

  if (show_visualization) {
    SwitchOpenGLContext(no_opengl_context);
    opengl_context.Deinitialize();
    opengl_context_2.Deinitialize();
  }

  cudaStreamDestroy(stream);
}

TEST(Optimization, AlternatingIntrinsicsOptimizationWithPhotometricResidual) {
  TestIntrinsicsOptimizationWithPhotometricResidual(/*use_pcg*/ false);
}

TEST(Optimization, PCGIntrinsicsOptimizationWithPhotometricResidual) {
  TestIntrinsicsOptimizationWithPhotometricResidual(/*use_pcg*/ true);
}


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

#include <boost/hana/core/typeclass.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>

#include <type_traits>
namespace hana = boost::hana;

struct Incrementable {
    BOOST_HANA_TYPECLASS(Incrementable);
    struct next_mcd;
    struct next_n_mcd;
};

BOOST_HANA_CONSTEXPR_LAMBDA auto next = [](auto x) {
    return Incrementable::instance<
        hana::datatype_t<decltype(x)>
    >::next_impl(x);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto next_n = [](auto x, unsigned int n) {
    return Incrementable::instance<
        hana::datatype_t<decltype(x)>
    >::next_n_impl(x, n);
};

struct Incrementable::next_mcd {
    template <typename X>
    static constexpr auto next_n_impl(X x, unsigned int n) {
        if (n == 0) return x;
        else        return next_n_impl(next(x), n - 1);
    }
};

struct Incrementable::next_n_mcd {
    template <typename X>
    static constexpr auto next_impl(X x)
    { return next_n(x, 1); }
};

template <>
struct Incrementable::instance<int> : Incrementable::next_mcd {
    static constexpr auto next_impl(int x)
    { return x + 1; }
};

template <>
struct Incrementable::instance<long> : Incrementable::next_n_mcd {
    static constexpr auto next_n_impl(long x, unsigned int n)
    { return x + n; }
};

template <typename T>
struct Incrementable::instance<T, hana::when<std::is_floating_point<T>{}>>
    : Incrementable::next_mcd
{
    template <typename X>
    static constexpr auto next_impl(X x)
    { return x + 1; }
};

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(next(1) == 2);
    BOOST_HANA_CONSTEXPR_ASSERT(next_n(1, 3) == 4);

    BOOST_HANA_CONSTEXPR_ASSERT(next(1l) == 2l);
    BOOST_HANA_CONSTEXPR_ASSERT(next_n(1l, 3) == 4l);

    BOOST_HANA_CONSTEXPR_ASSERT(next(1.0) == 2.0);
    BOOST_HANA_CONSTEXPR_ASSERT(next_n(1.0, 3) == 4.0);
}


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

#include <boost/hana/ext/std/integer_sequence.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/comparable.hpp>

#include <utility>
using namespace boost::hana;

using T = int;
using U = long long;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto sequences = make<Tuple>(
        std::integer_sequence<T>{},
        std::integer_sequence<U>{},

        std::integer_sequence<T, 0>{},
        std::integer_sequence<U, 0>{},

        std::integer_sequence<T, 0, 1>{},
        std::integer_sequence<U, 0, 1>{},

        std::integer_sequence<T, 1, 0, 2>{},
        std::integer_sequence<U, 1, 0, 2>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    {
        // equal
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                std::integer_sequence<T>{},
                std::integer_sequence<U>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(not_(equal(
                std::integer_sequence<T, 0>{},
                std::integer_sequence<U>{}
            )));
            BOOST_HANA_CONSTANT_CHECK(not_(equal(
                std::integer_sequence<T>{},
                std::integer_sequence<U, 0>{}
            )));

            BOOST_HANA_CONSTANT_CHECK(equal(
                std::integer_sequence<T, 0>{},
                std::integer_sequence<U, 0>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(not_(equal(
                std::integer_sequence<T, 0>{},
                std::integer_sequence<U, 0, 1>{}
            )));
            BOOST_HANA_CONSTANT_CHECK(not_(equal(
                std::integer_sequence<T, 0, 2>{},
                std::integer_sequence<U, 0, 1>{}
            )));

            BOOST_HANA_CONSTANT_CHECK(equal(
                std::integer_sequence<T, 0, 1, 2, 3>{},
                std::integer_sequence<U, 0, 1, 2, 3>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(not_(equal(
                std::integer_sequence<T, 0, 1, 2, 3, 5>{},
                std::integer_sequence<U, 0, 1, 2, 3>{}
            )));
        }

        // laws
        test::TestComparable<ext::std::IntegerSequence>{sequences};
    }
}