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

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

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

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

#include <qle/termstructures/spreadedblackvolatilitysurfacemoneyness.hpp>

#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/flatextrapolation2d.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/utilities/dataformatters.hpp>

#include <boost/make_shared.hpp>

using namespace std;

namespace QuantExt {

SpreadedBlackVolatilitySurfaceMoneyness::SpreadedBlackVolatilitySurfaceMoneyness(
    const Handle<BlackVolTermStructure>& referenceVol, const Handle<Quote>& movingSpot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote>>>& volSpreads,
    const Handle<Quote>& stickySpot, const Handle<YieldTermStructure>& stickyDividendTs,
    const Handle<YieldTermStructure>& stickyRiskFreeTs, const Handle<YieldTermStructure>& movingDividendTs,
    const Handle<YieldTermStructure>& movingRiskFreeTs, bool stickyStrike)
    : BlackVolatilityTermStructure(referenceVol->businessDayConvention(), referenceVol->dayCounter()),
      referenceVol_(referenceVol), movingSpot_(movingSpot), times_(times), moneyness_(moneyness),
      volSpreads_(volSpreads), stickySpot_(stickySpot), stickyDividendTs_(stickyDividendTs),
      stickyRiskFreeTs_(stickyRiskFreeTs), movingDividendTs_(movingDividendTs), movingRiskFreeTs_(movingRiskFreeTs),
      stickyStrike_(stickyStrike) {

    // register with observables

    registerWith(referenceVol_);
    registerWith(movingSpot_);
    registerWith(stickySpot_);

    for (auto const& v : volSpreads_)
        for (auto const& s : v)
            registerWith(s);

    registerWith(stickyDividendTs_);
    registerWith(stickyRiskFreeTs_);
    registerWith(movingDividendTs_);
    registerWith(movingRiskFreeTs_);

    // check our preconditions on the inputs

    QL_REQUIRE(!times_.empty(), "no times given");
    QL_REQUIRE(!moneyness_.empty(), "no moneyness values given");
    QL_REQUIRE(moneyness_.size() == volSpreads_.size(), "mismatch between moneyness vector and vol matrix rows");

    for (auto const& v : volSpreads_) {
        QL_REQUIRE(times_.size() == v.size(), "mismatch between times vector and vol matrix colums");
    }

    for (Size j = 1; j < times_.size(); ++j) {
        QL_REQUIRE(times_[j] > times_[j - 1], "Times must be sorted and unique but found that the "
                                                  << io::ordinal(j) << " time, " << times_[j]
                                                  << ", is not greater than the " << io::ordinal(j - 1) << " time, "
                                                  << times_[j - 1] << ".");
    }

    // add an artificial time if there is only one to make the interpolation work

    if (times_.size() == 1) {
        times_.push_back(times_.back() + 1.0);
        for (auto& v : volSpreads_)
            v.push_back(v.back());
    }

    // add an artificial moneyness if there is only one to make the interpolation work

    if (moneyness_.size() == 1) {
        moneyness_.push_back(moneyness_.back() + 1.0);
        volSpreads_.push_back(volSpreads_.back());
    }

    // create data matrix used for interpolation and the interpolation object

    data_ = Matrix(moneyness_.size(), times_.size(), 0.0);
    volSpreadSurface_ = FlatExtrapolator2D(boost::make_shared<BilinearInterpolation>(
        times_.begin(), times_.end(), moneyness_.begin(), moneyness_.end(), data_));
    volSpreadSurface_.enableExtrapolation();
}

Date SpreadedBlackVolatilitySurfaceMoneyness::maxDate() const { return referenceVol_->maxDate(); }
const Date& SpreadedBlackVolatilitySurfaceMoneyness::referenceDate() const { return referenceVol_->referenceDate(); }
Calendar SpreadedBlackVolatilitySurfaceMoneyness::calendar() const { return referenceVol_->calendar(); }
Natural SpreadedBlackVolatilitySurfaceMoneyness::settlementDays() const { return referenceVol_->settlementDays(); }
Real SpreadedBlackVolatilitySurfaceMoneyness::minStrike() const { return referenceVol_->minStrike(); }
Real SpreadedBlackVolatilitySurfaceMoneyness::maxStrike() const { return referenceVol_->maxStrike(); }

void SpreadedBlackVolatilitySurfaceMoneyness::update() {
    LazyObject::update();
    BlackVolatilityTermStructure::update();
}

const std::vector<QuantLib::Real>& SpreadedBlackVolatilitySurfaceMoneyness::moneyness() const { return moneyness_; }

void SpreadedBlackVolatilitySurfaceMoneyness::performCalculations() const {
    for (Size j = 0; j < data_.columns(); ++j) {
        for (Size i = 0; i < data_.rows(); ++i) {
            data_(i, j) = volSpreads_[i][j]->value();
        }
    }
    volSpreadSurface_.update();
}

Real SpreadedBlackVolatilitySurfaceMoneyness::blackVolImpl(Time t, Real strike) const {
    calculate();
    QL_REQUIRE(!referenceVol_.empty(), "SpreadedBlackVolatilitySurfaceMoneyness: reference vol is empty");
    Real m = moneynessFromStrike(t, strike, false);
    QL_REQUIRE(std::isfinite(m),
               "SpreadedBlackVolatilitySurfaceMoneyness: got invalid moneyness (dynamic reference) at t = "
                   << t << ", strike = " << strike << ": " << m);
    Real effStrike;
    if (stickyStrike_)
        effStrike = strike;
    else {
        effStrike = strikeFromMoneyness(t, m, true);
        QL_REQUIRE(std::isfinite(effStrike),
                   "SpreadedBlackVolatilitySurfaceMoneyness: got invalid strike from moneyness at t = "
                       << t << ", input strike = " << strike << ", moneyness = " << m);
    }
    Real m2 = moneynessFromStrike(t, strike, false);
    QL_REQUIRE(std::isfinite(m2),
               "SpreadedBlackVolatilitySurfaceMoneyness: got invalid moneyness (sticky reference) at t = "
                   << t << ", strike = " << strike << ": " << m2);
    return referenceVol_->blackVol(t, effStrike) + volSpreadSurface_(t, m2);
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpot::moneynessFromStrike(Time t, Real strike,
                                                                      const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 1.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
        return strike / (stickyReference ? stickySpot_->value() : movingSpot_->value());
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpot::strikeFromMoneyness(Time t, Real moneyness,
                                                                      const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
    return moneyness * (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessSpot::moneynessFromStrike(Time t, Real strike,
                                                                         const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 0.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: movingSpot is empty");
        return std::log(strike / (stickyReference ? stickySpot_->value() : movingSpot_->value()));
    }
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessSpot::strikeFromMoneyness(Time t, Real moneyness,
                                                                         const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceLogMoneynessSpot: movingSpot is empty");
    return std::exp(moneyness) * (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceMoneynessForward::moneynessFromStrike(Time t, Real strike,
                                                                         const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 1.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return strike / forward;
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessForward::strikeFromMoneyness(Time t, Real moneyness,
                                                                         const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return moneyness * forward;
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessForward::moneynessFromStrike(Time t, Real strike,
                                                                            const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 0.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceLogMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return std::log(strike / forward);
    }
}

Real SpreadedBlackVolatilitySurfaceLogMoneynessForward::strikeFromMoneyness(Time t, Real moneyness,
                                                                            const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceLogMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return std::exp(moneyness) * forward;
}

Real SpreadedBlackVolatilitySurfaceStdDevs::moneynessFromStrike(Time t, Real strike, const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0) || close_enough(t, 0.0))
        return 0.0;
    else {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: stickyRiskFreeTs is empty");
        Real stickyForward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        Real forward;
        if (stickyReference) {
            forward = stickyForward;
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        // We use the sticky forward to read the vol for the definition of the standardised moneyness to
        // avoid that this changes under forward curve changes. In the end this is a matter of definition and
        // we might want to revise this later.
        Real vol = referenceVol_->blackVol(t, stickyForward);
        return std::log(strike / forward) / (vol * std::sqrt(t));
    }
}

Real SpreadedBlackVolatilitySurfaceStdDevs::strikeFromMoneyness(Time t, Real moneyness,
                                                                const bool stickyReference) const {
    Real stickyForward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    Real forward;
    if (stickyReference) {
        forward = stickyForward;
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(), "SpreadedBlackVolatilitySurfaceStdDevs: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    // We use the sticky forward to read the vol for the definition of the standardised moneyness to
    // avoid that this changes under forward curve changes. In the end this is a matter of definition and
    // we might want to revise this later.
    Real vol = referenceVol_->blackVol(t, stickyForward);
    return std::exp(moneyness * vol * std::sqrt(t)) * forward;
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpotAbsolute::moneynessFromStrike(Time t, Real strike,
                                                                              const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0)) {
        return 0.0;
    } else {
        QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
        QL_REQUIRE(stickyReference || !movingSpot_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
        return strike - (stickyReference ? stickySpot_->value() : movingSpot_->value());
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessSpotAbsolute::strikeFromMoneyness(Time t, Real moneyness,
                                                                              const bool stickyReference) const {
    QL_REQUIRE(!stickyReference || !stickySpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: stickySpot is empty");
    QL_REQUIRE(stickyReference || !movingSpot_.empty(),
               "SpreadedBlackVolatilitySurfaceMoneynessSpot: movingSpot is empty");
    return moneyness + (stickyReference ? stickySpot_->value() : movingSpot_->value());
}

Real SpreadedBlackVolatilitySurfaceMoneynessForwardAbsolute::moneynessFromStrike(Time t, Real strike,
                                                                                const bool stickyReference) const {
    if (strike == Null<Real>() || close_enough(strike, 0.0))
        return 0.0;
    else {
        Real forward;
        if (stickyReference) {
            QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
            QL_REQUIRE(!stickyDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
            QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
            forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
        } else {
            QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
            QL_REQUIRE(!movingDividendTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
            QL_REQUIRE(!movingRiskFreeTs_.empty(),
                       "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
            forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
        }
        return strike - forward;
    }
}

Real SpreadedBlackVolatilitySurfaceMoneynessForwardAbsolute::strikeFromMoneyness(Time t, Real moneyness,
                                                                                const bool stickyReference) const {
    Real forward;
    if (stickyReference) {
        QL_REQUIRE(!stickySpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: stickySpot is empty");
        QL_REQUIRE(!stickyDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyDividendTs is empty");
        QL_REQUIRE(!stickyRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: stickyRiskFreeTs is empty");
        forward = stickySpot_->value() * stickyDividendTs_->discount(t) / stickyRiskFreeTs_->discount(t);
    } else {
        QL_REQUIRE(!movingSpot_.empty(), "SpreadedBlackVolatilitySurfaceMoneynessForward: movingSpot is empty");
        QL_REQUIRE(!movingDividendTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: movingDividendTs is empty");
        QL_REQUIRE(!movingRiskFreeTs_.empty(),
                   "SpreadedBlackVolatilitySurfaceMoneynessForward: mocingRiskFreeTs is empty");
        forward = movingSpot_->value() * movingDividendTs_->discount(t) / movingRiskFreeTs_->discount(t);
    }
    return moneyness + forward;
}

} // namespace QuantExt


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

// Boost.SpatialIndex - rtree implementation
//
// Copyright 2008 Federico J. Fernandez.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP

#include <cstddef>
#include <iostream> // TODO: Remove if print() is removed
#include <stdexcept>
#include <utility>
#include <vector>

#include <boost/concept_check.hpp>
#include <boost/shared_ptr.hpp>

#include <boost/geometry/algorithms/area.hpp>

#include <boost/geometry/extensions/index/rtree/rtree_node.hpp>
#include <boost/geometry/extensions/index/rtree/rtree_leaf.hpp>

namespace boost { namespace geometry { namespace index
{

template <typename Box, typename Value >
class rtree
{
public:

    typedef boost::shared_ptr<rtree_node<Box, Value> > node_pointer;
    typedef boost::shared_ptr<rtree_leaf<Box, Value> > leaf_pointer;

    /**
     * \brief Creates a rtree with 'maximum' elements per node and 'minimum'.
     */
    rtree(unsigned int const& maximum, unsigned int const& minimum)
        : m_count(0)
        , m_min_elems_per_node(minimum)
        , m_max_elems_per_node(maximum)
        , m_root(new rtree_node<Box, Value>(node_pointer(), 1))
    {
    }

    /**
     * \brief Creates a rtree with maximum elements per node
     *        and minimum (box is ignored).
     */
    rtree(Box const& box, unsigned int const& maximum, unsigned int const& minimum)
        : m_count(0)
        , m_min_elems_per_node(minimum)
        , m_max_elems_per_node(maximum)
        , m_root(new rtree_node<Box, Value>(node_pointer(), 1))
    {
        boost::ignore_unused_variable_warning(box);
    }

    /**
     * \brief destructor (virtual because we have virtual functions)
     */
    virtual ~rtree() {}

    /**
     * \brief Remove elements inside the 'box'
     */
    inline void remove(Box const& box)
    {
        try
        {
            node_pointer leaf(choose_exact_leaf(box));
            typename rtree_leaf<Box, Value>::leaf_map q_leaves;

            leaf->remove(box);

            if (leaf->elements() < m_min_elems_per_node && elements() > m_min_elems_per_node)
            {
                q_leaves = leaf->get_leaves();

                // we remove the leaf_node in the parent node because now it's empty
                leaf->get_parent()->remove(leaf->get_parent()->get_box(leaf));
            }

            typename rtree_node<Box, Value>::node_map q_nodes;
            condense_tree(leaf, q_nodes);

            std::vector<std::pair<Box, Value> > s;
            for (typename rtree_node<Box, Value>::node_map::const_iterator it = q_nodes.begin();
                 it != q_nodes.end(); ++it)
            {
                typename rtree_leaf<Box, Value>::leaf_map leaves = it->second->get_leaves();

                // reinserting leaves from nodes
                for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator itl = leaves.begin();
                     itl != leaves.end(); ++itl)
                {
                    s.push_back(*itl);
                }
            }

            for (typename std::vector<std::pair<Box, Value> >::const_iterator it = s.begin(); it != s.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            // if the root has only one child and the child is not a leaf,
            // make it the root
            if (m_root->elements() == 1)
            {
                if (!m_root->first_element()->is_leaf())
                {
                    m_root = m_root->first_element();
                }
            }
            // reinserting leaves
            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = q_leaves.begin();
                 it != q_leaves.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            m_count--;
        }
        catch(std::logic_error & e)
        {
            // TODO: mloskot - replace with Boost.Geometry exception

            // not found
            std::cerr << e.what() << std::endl;
            return;
        }
    }

    /**
     * \brief Remove element inside the box with value
     */
    void remove(Box const& box, Value const& value)
    {
        try
        {
            node_pointer leaf;

            // find possible leaves
            typedef typename std::vector<node_pointer > node_type;
            node_type nodes;
            m_root->find_leaves(box, nodes);

            // refine the result
            for (typename node_type::const_iterator it = nodes.begin(); it != nodes.end(); ++it)
            {
                leaf = *it;
                try
                {
                    leaf->remove(value);
                    break;
                } catch (...)
                {
                    leaf = node_pointer();
                }
            }

            if (!leaf)
                return;

            typename rtree_leaf < Box, Value >::leaf_map q_leaves;

            if (leaf->elements() < m_min_elems_per_node && elements() > m_min_elems_per_node)
            {
                q_leaves = leaf->get_leaves();

                // we remove the leaf_node in the parent node because now it's empty
                leaf->get_parent()->remove(leaf->get_parent()->get_box(leaf));
            }

            typename rtree_node<Box, Value>::node_map q_nodes;
            condense_tree(leaf, q_nodes);

            std::vector<std::pair<Box, Value> > s;
            for (typename rtree_node<Box, Value>::node_map::const_iterator it = q_nodes.begin();
                 it != q_nodes.end(); ++it)
            {
                typename rtree_leaf<Box, Value>::leaf_map leaves = it->second->get_leaves();

                // reinserting leaves from nodes
                for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator itl = leaves.begin();
                     itl != leaves.end(); ++itl)
                {
                    s.push_back(*itl);
                }
            }

            for (typename std::vector<std::pair<Box, Value> >::const_iterator it = s.begin(); it != s.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            // if the root has only one child and the child is not a leaf,
            // make it the root
            if (m_root->elements() == 1)
            {
                if (!m_root->first_element()->is_leaf())
                {
                    m_root = m_root->first_element();
                }
            }

            // reinserting leaves
            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = q_leaves.begin();
                 it != q_leaves.end(); ++it)
            {
                m_count--;
                insert(it->first, it->second);
            }

            m_count--;

        }
        catch(std::logic_error & e)
        {
            // TODO: mloskot - ggl exception

            // not found
            std::cerr << e.what() << std::endl;
            return;
        }
    }

    /**
     * \brief Returns the number of elements.
     */
    inline unsigned int elements() const
    {
        return m_count;
    }

    /**
     * \brief Inserts an element with 'box' as key with value.
     */
    inline void insert(Box const& box, Value const& value)
    {
        m_count++;

        node_pointer leaf(choose_corresponding_leaf(box));

        // check if the selected leaf is full to do the split if necessary
        if (leaf->elements() >= m_max_elems_per_node)
        {
            leaf->insert(box, value);

            // split!
            node_pointer n1(new rtree_leaf<Box, Value>(leaf->get_parent()));
            node_pointer n2(new rtree_leaf<Box, Value>(leaf->get_parent()));

            split_node(leaf, n1, n2);
            adjust_tree(leaf, n1, n2);
        }
        else
        {
            leaf->insert(box, value);
            adjust_tree(leaf);
        }
    }

    /**
     * \brief Returns all the values inside 'box'
     */
    inline std::deque<Value> find(Box const& box) const
    {
        std::deque<Value> result;
        m_root->find(box, result, false);
        return result;
    }

    /**
     * \brief Print Rtree (mainly for debug)
     */
    inline void print()
    {
        std::cerr << "===================================" << std::endl;
        std::cerr << " Min/Max: " << m_min_elems_per_node << " / " << m_max_elems_per_node << std::endl;
        std::cerr << "Leaves: " << m_root->get_leaves().size() << std::endl;
        m_root->print();
        std::cerr << "===================================" << std::endl;
    }

private:

    /// number of elements
    unsigned int m_count;

    /// minimum number of elements per node
    unsigned int m_min_elems_per_node;

    /// maximum number of elements per node
    unsigned int m_max_elems_per_node;

    /// tree root
    node_pointer m_root;

    /**
     * \brief Reorganize the tree after a removal. It tries to
     *        join nodes with less elements than m.
     */
    void condense_tree(node_pointer const& leaf,
        typename rtree_node<Box, Value>::node_map& q_nodes)
    {
        if (leaf.get() == m_root.get())
        {
            // if it's the root we are done
            return;
        }

        node_pointer parent = leaf->get_parent();
        parent->adjust_box(leaf);

        if (parent->elements() < m_min_elems_per_node)
        {
            if (parent.get() == m_root.get())
            {
                // if the parent is underfull and it's the root we just exit
                return;
            }

            // get the nodes that we should reinsert
            typename rtree_node<Box, Value>::node_map this_nodes = parent->get_nodes();
            for(typename rtree_node<Box, Value>::node_map::const_iterator it = this_nodes.begin();
                it != this_nodes.end(); ++it)
            {
                q_nodes.push_back(*it);
            }

            // we remove the node in the parent node because now it should be
            // re inserted
            parent->get_parent()->remove(parent->get_parent()->get_box(parent));
        }

        condense_tree(parent, q_nodes);
    }

    /**
     * \brief After an insertion splits nodes with more than 'maximum' elements.
     */
    inline void adjust_tree(node_pointer& node)
    {
        if (node.get() == m_root.get())
        {
            // we finished the adjust
            return;
        }

        // as there are no splits just adjust the box of the parent and go on
        node_pointer parent = node->get_parent();
        parent->adjust_box(node);
        adjust_tree(parent);
    }

    /**
     * \brief After an insertion splits nodes with more than maximum elements
     *        (recursive step with subtrees 'n1' and 'n2' to be joined).
     */
    void adjust_tree(node_pointer& leaf, node_pointer& n1, node_pointer& n2)
    {
        // check if we are in the root and do the split
        if (leaf.get() == m_root.get())
        {
            node_pointer new_root(new rtree_node<Box,Value>(node_pointer (), leaf->get_level() + 1));
            new_root->add_node(n1->compute_box(), n1);
            new_root->add_node(n2->compute_box(), n2);

            n1->set_parent(new_root);
            n2->set_parent(new_root);

            n1->update_parent(n1);
            n2->update_parent(n2);

            m_root = new_root;
            return;
        }

        node_pointer parent = leaf->get_parent();

        parent->replace_node(leaf, n1);
        parent->add_node(n2->compute_box(), n2);

        // if parent is full, split and readjust
        if (parent->elements() > m_max_elems_per_node)
        {
            node_pointer p1(new rtree_node<Box, Value>(parent->get_parent(), parent->get_level()));
            node_pointer p2(new rtree_node<Box, Value>(parent->get_parent(), parent->get_level()));

            split_node(parent, p1, p2);
            adjust_tree(parent, p1, p2);
        }
        else
        {
            adjust_tree(parent);
        }
    }

    /**
     * \brief Splits 'n' in 'n1' and 'n2'
     */
    void split_node(node_pointer const& n, node_pointer& n1, node_pointer& n2) const
    {
        unsigned int seed1 = 0;
        unsigned int seed2 = 0;
        std::vector<Box> boxes = n->get_boxes();

        n1->set_parent(n->get_parent());
        n2->set_parent(n->get_parent());

        linear_pick_seeds(n, seed1, seed2);

        if (n->is_leaf())
        {
            n1->add_value(boxes[seed1], n->get_value(seed1));
            n2->add_value(boxes[seed2], n->get_value(seed2));
        }
        else
        {
            n1->add_node(boxes[seed1], n->get_node(seed1));
            n2->add_node(boxes[seed2], n->get_node(seed2));
        }

        unsigned int index = 0;

        if (n->is_leaf())
        {
            // TODO: mloskot - add BOOST_GEOMETRY_ASSERT(node.size() >= 2); or similar

            typename rtree_leaf<Box, Value>::leaf_map nodes = n->get_leaves();
            unsigned int remaining = nodes.size() - 2;

            for (typename rtree_leaf<Box, Value>::leaf_map::const_iterator it = nodes.begin();
                 it != nodes.end(); ++it, index++)
            {
                if (index != seed1 && index != seed2)
                {
                    if (n1->elements() + remaining == m_min_elems_per_node)
                    {
                        n1->add_value(it->first, it->second);
                        continue;
                    }
                    if (n2->elements() + remaining == m_min_elems_per_node)
                    {
                        n2->add_value(it->first, it->second);
                        continue;
                    }

                    remaining--;

                    /// current boxes of each group
                    Box b1, b2;

                    /// enlarged boxes of each group
                    Box eb1, eb2;
                    b1 = n1->compute_box();
                    b2 = n2->compute_box();

                    /// areas
                    typedef typename coordinate_type<Box>::type coordinate_type;
                    coordinate_type b1_area, b2_area;
                    coordinate_type eb1_area, eb2_area;
                    b1_area = geometry::area(b1);
                    b2_area = geometry::area(b2);
                    eb1_area = compute_union_area(b1, it->first);
                    eb2_area = compute_union_area(b2, it->first);

                    if (eb1_area - b1_area > eb2_area - b2_area)
                    {
                        n2->add_value(it->first, it->second);
                    }
                    if (eb1_area - b1_area < eb2_area - b2_area)
                    {
                        n1->add_value(it->first, it->second);
                    }
                    if (eb1_area - b1_area == eb2_area - b2_area)
                    {
                        if (b1_area < b2_area)
                        {
                            n1->add_value(it->first, it->second);
                        }
                        if (b1_area > b2_area)
                        {
                            n2->add_value(it->first, it->second);
                        }
                        if (b1_area == b2_area)
                        {
                            if (n1->elements() > n2->elements())
                            {
                                n2->add_value(it->first, it->second);
                            }
                            else
                            {
                                n1->add_value(it->first, it->second);
                            }
                        }
                    }
                }
            }
        }
        else
        {
            // TODO: mloskot - add BOOST_GEOMETRY_ASSERT(node.size() >= 2); or similar

            typename rtree_node<Box, Value>::node_map nodes = n->get_nodes();
            unsigned int remaining = nodes.size() - 2;
            for(typename rtree_node<Box, Value>::node_map::const_iterator it = nodes.begin();
                it != nodes.end(); ++it, index++)
            {

                if (index != seed1 && index != seed2)
                {

                    if (n1->elements() + remaining == m_min_elems_per_node)
                    {
                        n1->add_node(it->first, it->second);
                        continue;
                    }
                    if (n2->elements() + remaining == m_min_elems_per_node)
                    {
                        n2->add_node(it->first, it->second);
                        continue;
                    }

                    remaining--;

                    /// current boxes of each group
                    Box b1, b2;

                    /// enlarged boxes of each group
                    Box eb1, eb2;
                    b1 = n1->compute_box();
                    b2 = n2->compute_box();

                    /// areas
                    typedef typename coordinate_type<Box>::type coordinate_type;
                    coordinate_type b1_area, b2_area;
                    coordinate_type eb1_area, eb2_area;
                    b1_area = geometry::area(b1);
                    b2_area = geometry::area(b2);

                    eb1_area = compute_union_area(b1, it->first);
                    eb2_area = compute_union_area(b2, it->first);

                    if (eb1_area - b1_area > eb2_area - b2_area)
                    {
                        n2->add_node(it->first, it->second);
                    }
                    if (eb1_area - b1_area < eb2_area - b2_area)
                    {
                        n1->add_node(it->first, it->second);
                    }
                    if (eb1_area - b1_area == eb2_area - b2_area)
                    {
                        if (b1_area < b2_area)
                        {
                            n1->add_node(it->first, it->second);
                        }
                        if (b1_area > b2_area)
                        {
                            n2->add_node(it->first, it->second);
                        }
                        if (b1_area == b2_area)
                        {
                            if (n1->elements() > n2->elements())
                            {
                                n2->add_node(it->first, it->second);
                            }
                            else
                            {
                                n1->add_node(it->first, it->second);
                            }
                        }
                    }

                }
            }
        }
    }

    /**
     * \brief Choose initial values for the split algorithm (linear version)
     */
    void linear_pick_seeds(node_pointer const& n, unsigned int &seed1, unsigned int &seed2) const
    {
        // get boxes from the node
        std::vector<Box>boxes = n->get_boxes();
        if (boxes.size() == 0)
        {
            // TODO: mloskot - throw ggl exception
            throw std::logic_error("Empty Node trying to Pick Seeds");
        }

        // only two dim for now
        // unsigned int dimensions =
        //   geometry::point_traits<Point>::coordinate_count;

        // find the first two elements
        typedef typename coordinate_type<Box>::type coordinate_type;
        coordinate_type separation_x, separation_y;
        unsigned int first_x, second_x;
        unsigned int first_y, second_y;
        find_normalized_separations<0u>(boxes, separation_x, first_x, second_x);
        find_normalized_separations<1u>(boxes, separation_y, first_y, second_y);

        if (separation_x > separation_y)
        {
            seed1 = first_x;
            seed2 = second_x;
        }
        else
        {
            seed1 = first_y;
            seed2 = second_y;
        }
    }

    /**
     * \brief Find distances between possible initial values for the
     *        pick_seeds algorithm.
     */
    template <std::size_t D, typename T>
    void find_normalized_separations(std::vector<Box> const& boxes, T& separation,
        unsigned int& first, unsigned int& second) const
    {
        if (boxes.size() < 2)
        {
            throw std::logic_error("At least two boxes needed to split");
        }

        // find the lowest high
        typename std::vector<Box>::const_iterator it = boxes.begin();
        typedef typename coordinate_type<Box>::type coordinate_type;
        coordinate_type lowest_high = geometry::get<max_corner, D>(*it);
        unsigned int lowest_high_index = 0;
        unsigned int index = 1;
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<max_corner, D>(*it) < lowest_high)
            {
                lowest_high = geometry::get<max_corner, D>(*it);
                lowest_high_index = index;
            }
            index++;
        }

        // find the highest low
        coordinate_type highest_low = 0;
        unsigned int highest_low_index = 0;
        if (lowest_high_index == 0)
        {
            highest_low = geometry::get<min_corner, D>(boxes[1]);
            highest_low_index = 1;
        }
        else
        {
            highest_low = geometry::get<min_corner, D>(boxes[0]);
            highest_low_index = 0;
        }

        index = 0;
        for (typename std::vector<Box>::const_iterator it = boxes.begin();
             it != boxes.end(); ++it, index++)
        {
            if (geometry::get<min_corner, D>(*it) >= highest_low && index != lowest_high_index)
            {
                highest_low = geometry::get<min_corner, D>(*it);
                highest_low_index = index;
            }
        }

        // find the lowest low
        it = boxes.begin();
        coordinate_type lowest_low = geometry::get<min_corner, D>(*it);
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<min_corner, D>(*it) < lowest_low)
            {
                lowest_low = geometry::get<min_corner, D>(*it);
            }
        }

        // find the highest high
        it = boxes.begin();
        coordinate_type highest_high = geometry::get<max_corner, D>(*it);
        ++it;
        for(; it != boxes.end(); ++it)
        {
            if (geometry::get<max_corner, D>(*it) > highest_high)
            {
                highest_high = geometry::get<max_corner, D>(*it);
            }
        }

        coordinate_type const width = highest_high - lowest_low;

        separation = (highest_low - lowest_high) / width;
        first = highest_low_index;
        second = lowest_high_index;
    }

    /**
     * \brief Choose one of the possible leaves to make an insertion
     */
    inline node_pointer choose_corresponding_leaf(Box const& e)
    {
        node_pointer node = m_root;

        // if the tree is empty add an initial leaf
        if (m_root->elements() == 0)
        {
            leaf_pointer new_leaf(new rtree_leaf<Box, Value>(m_root));
            m_root->add_leaf_node(Box (), new_leaf);

            return new_leaf;
        }

        while (!node->is_leaf())
        {
            /// traverse node's map to see which node we should select
            node = node->choose_node(e);
        }
        return node;
    }

    /**
     * \brief Choose the exact leaf where an insertion should be done
     */
    node_pointer choose_exact_leaf(Box const&e) const
    {
        // find possible leaves
        typedef typename std::vector<node_pointer> node_type;
        node_type nodes;
        m_root->find_leaves(e, nodes);

        // refine the result
        for (typename node_type::const_iterator it = nodes.begin(); it != nodes.end(); ++it)
        {
            typedef std::vector<std::pair<Box, Value> > leaves_type;
            leaves_type leaves = (*it)->get_leaves();

            for (typename leaves_type::const_iterator itl = leaves.begin();
                 itl != leaves.end(); ++itl)
            {

                if (itl->first.max_corner() == e.max_corner()
                    && itl->first.min_corner() == e.min_corner())
                {
                    return *it;
                }
            }
        }

        // TODO: mloskot - ggl exception
        throw std::logic_error("Leaf not found");
    }
};

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_EXTENSIONS_INDEX_RTREE_RTREE_HPP


//=============================================================================================================
/**
 * @file     warp.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>
 * @version  dev
 * @date     November, 2015
 *
 * @section  LICENSE
 *
 * Copyright (C) 2015, Lorenz Esch. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief   Warp class definition.
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "warp.h"

#include <iostream>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/LU>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QDebug>
#include <QFile>
#include <QList>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

MatrixXf Warp::calculate(const MatrixXf &sLm, const MatrixXf &dLm, const MatrixXf &sVert)
{
    MatrixXf warpWeight, polWeight;
    calcWeighting(sLm, dLm, warpWeight, polWeight);
    MatrixXf wVert = warpVertices(sVert, sLm, warpWeight, polWeight);
    return wVert;
}

//=============================================================================================================

void Warp::calculate(const MatrixXf & sLm, const MatrixXf &dLm, QList<MatrixXf> & vertList)
{
    MatrixXf warpWeight, polWeight;
    calcWeighting(sLm, dLm, warpWeight, polWeight);

    for (int i=0; i<vertList.size(); i++)
    {
        vertList.replace(i,warpVertices(vertList.at(i), sLm, warpWeight, polWeight));
    }
    return;
}

//=============================================================================================================

bool Warp::calcWeighting(const MatrixXf &sLm, const MatrixXf &dLm, MatrixXf& warpWeight, MatrixXf& polWeight)
{
    MatrixXf K = MatrixXf::Zero(sLm.rows(),sLm.rows());     //K(i,j)=||sLm(i)-sLm(j)||
    for (int i=0; i<sLm.rows(); i++)
        K.col(i)=((sLm.rowwise()-sLm.row(i)).rowwise().norm());

//    std::cout << "Here is the matrix K:" << std::endl << K << std::endl;

    MatrixXf P (sLm.rows(),4);                              //P=[ones,sLm]
    P << MatrixXf::Ones(sLm.rows(),1),sLm;
//    std::cout << "Here is the matrix P:" << std::endl << P << std::endl;

    MatrixXf L ((sLm.rows()+4),(sLm.rows()+4));             //L=Full Matrix of the linear eq.
    L <<    K,P,
            P.transpose(),MatrixXf::Zero(4,4);
//    std::cout << "Here is the matrix L:" << std::endl << L << std::endl;

    MatrixXf Y ((dLm.rows()+4),3);                          //Y=[dLm,Zero]
    Y <<    dLm,
            MatrixXf::Zero(4,3);
//    std::cout << "Here is the matrix Y:" << std::endl << Y << std::endl;

    //
    // calculate the weighting matrix (Y=L*W)
    //
    MatrixXf W ((dLm.rows()+4),3);                          //W=[warpWeight,polWeight]
    Eigen::FullPivLU <MatrixXf> Lu(L);                      //LU decomposition is one method to solve lin. eq.
    W=Lu.solve(Y);
//    std::cout << "Here is the matrix W:" << std::endl << W << std::endl;

    warpWeight = W.topRows(sLm.rows());
    polWeight = W.bottomRows(4);

    return true;
}

//=============================================================================================================

MatrixXf Warp::warpVertices(const MatrixXf &sVert, const MatrixXf & sLm, const MatrixXf& warpWeight, const MatrixXf& polWeight)
{
    MatrixXf wVert = sVert * polWeight.bottomRows(3);         //Pol. Warp
    wVert.rowwise() += polWeight.row(0);                      //Translation

    //
    // TPS Warp
    //
    MatrixXf K = MatrixXf::Zero(sVert.rows(),sLm.rows());     //K(i,j)=||sLm(i)-sLm(j)||
    for (int i=0; i<sVert.rows(); i++)
        K.row(i)=((sLm.rowwise()-sVert.row(i)).rowwise().norm().transpose());
//    std::cout << "Here is the matrix K:" << std::endl << K << std::endl;

    wVert += K*warpWeight;
//    std::cout << "Here is the matrix wVert:" << std::endl << wVert << std::endl;
    return wVert;
}

//=============================================================================================================

MatrixXf Warp::readsLm(const QString &electrodeFileName)
{
    MatrixXf electrodes;
    QFile file(electrodeFileName);

    if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        qDebug()<<"Error opening file";
//        return false;
    }

    //Start reading from file
    double numberElectrodes;
    QTextStream in(&file);
    int i=0;

    while(!in.atEnd())
    {
        QString line = in.readLine();
        QStringList fields = line.split(QRegExp("\\s+"));

        //Delete last element if it is a blank character
        if(fields.at(fields.size()-1) == "")
            fields.removeLast();

        //Read number of electrodes
        if(i == 0){
            numberElectrodes = fields.at(fields.size()-1).toDouble();
            electrodes = MatrixXf::Zero(numberElectrodes, 3);
        }
        //Read actual electrode positions
        else{
            Vector3f x;
            x << fields.at(fields.size()-3).toFloat(),fields.at(fields.size()-2).toFloat(),fields.at(fields.size()-1).toFloat();
            electrodes.row(i-1)=x.transpose();
        }
        i++;
    }
    return electrodes;
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Molecule.hpp"

#include <algorithm>
#include <iostream>
#include <string>
#include <vector>
#include <cmath>
#include <iomanip>

#include "Config.hpp"

#include <boost/format.hpp>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "Atom.hpp"
#include "cavity/Element.hpp"
#include "MathUtils.hpp"
#include "Symmetry.hpp"

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph)
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph)
{
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(0, 0, 0, 0);
}

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph,
                   int nr_gen, int gen[3])
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph)
{
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(nr_gen, gen[0], gen[1], gen[2]);
}

Molecule::Molecule(int nat, const Eigen::VectorXd & chg, const Eigen::VectorXd & m,
                   const Eigen::Matrix3Xd & geo, const std::vector<Atom> & at,
                   const std::vector<Sphere> & sph,
                   const Symmetry & pg)
    : nAtoms_(nat), charges_(chg), masses_(m), geometry_(geo), atoms_(at), spheres_(sph),
      pointGroup_(pg)
{
    rotor_ = findRotorType();
}

Molecule::Molecule(const std::vector<Sphere> & sph)
    : nAtoms_(sph.size()), spheres_(sph)
{
    charges_ = Eigen::VectorXd::Ones(nAtoms_);
    masses_.resize(nAtoms_);
    geometry_.resize(Eigen::NoChange, nAtoms_);
    for (size_t i = 0; i < nAtoms_; ++i) {
        masses_(i) = spheres_[i].radius;
        geometry_.col(i) = spheres_[i].center;
        double charge = charges_(i);
        double mass = masses_(i);
        atoms_.push_back( Atom("Dummy", "Du", charge, mass, mass, geometry_.col(i)) );
    }
    rotor_ = findRotorType();
    pointGroup_ = buildGroup(0, 0, 0, 0);
}

Molecule::Molecule(const Molecule &other)
{
    *this = other;
}

Eigen::Vector3d Molecule::centerOfMass()
{
    Eigen::Vector3d com;
    com << 0.0, 0.0, 0.0;
    for (size_t i = 0; i < nAtoms_; ++i) {
        com += masses_(i) * atoms_[i].position;
    }
    com *= 1.0/masses_.sum();
    return com;
}

Eigen::Matrix3d Molecule::inertiaTensor()
{
    Eigen::Matrix3d inertia = Eigen::Matrix3d::Zero();

    for (size_t i = 0; i < nAtoms_; ++i) {
        // Diagonal
        inertia(0,0) += masses_(i) * (geometry_(1,i) * geometry_(1,i) + geometry_(2,
                                      i) * geometry_(2,i));
        inertia(1,1) += masses_(i) * (geometry_(0,i) * geometry_(0,i) + geometry_(2,
                                      i) * geometry_(2,i));
        inertia(2,2) += masses_(i) * (geometry_(0,i) * geometry_(0,i) + geometry_(1,
                                      i) * geometry_(1,i));

        // Off-diagonal
        inertia(0,1) -= masses_(i) * (geometry_(0,i) * geometry_(1,i));
        inertia(0,2) -= masses_(i) * (geometry_(0,i) * geometry_(2,i));
        inertia(1,2) -= masses_(i) * (geometry_(1,i) * geometry_(2,i));
    }
    // Now symmetrize
    hermitivitize(inertia);

    // Check elements for a numerical zero and make it a hard zero
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            if (fabs(inertia(i,j)) < 1.0e-14) {
                inertia(i,j) = 0.0;
            }
        }
    }

    return inertia;
}

rotorType Molecule::findRotorType()
{
    rotorType type;
    if (nAtoms_ == 1) {
        type = rtAtom;
    } else {
        // Get inertia tensor
        Eigen::Matrix3d inertia = inertiaTensor();
        // Diagonalize inertia tensor V^t * I * V
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
        if (eigenSolver.info() != Eigen::Success) abort();
        // Determine the degeneracy of the eigenvalues.
        int deg = 0;
        double tmp, abs, rel;
        for (int i = 0; i < 2; ++i) {
            for (int j = i + 1; j < 3 && deg < 2; ++j) { // Check i and j != i
                abs = fabs(eigenSolver.eigenvalues()[i] - eigenSolver.eigenvalues()[j]);
                tmp = eigenSolver.eigenvalues()[j]; // Because the eigenvalues are already in ascending order.
                if (abs > 1.0e-14) {
                    rel = abs/tmp;
                } else {
                    rel = 0.0;
                }
                if (rel < 1.0e-8) {
                    ++deg;
                }
            }
        }
        // Get the rotor type based on the degeneracy.
        if (eigenSolver.eigenvalues()[0] == 0.0) {
            type = rtLinear;
        } else if (deg == 2) {
            type = rtSpherical;
        } else if (deg == 1) { // We do not distinguish between prolate and oblate.
            type = rtSymmetric;
        } else {
            type = rtAsymmetric;
        }
    }

    return type;
}

void Molecule::translate(const Eigen::Vector3d &translationVector)
{
    // Translate the geometry_ matrix and update the geometric data in atoms_.
    for (size_t i = 0; i < nAtoms_; ++i) {
        geometry_.col(i) -= translationVector;
        Eigen::Vector3d tmp = geometry_.col(i);
        atoms_[i].position = tmp;
    }
}

void Molecule::moveToCOM()
{
    Eigen::Vector3d com = centerOfMass();
    this->translate(com);
}

void Molecule::rotate(const Eigen::Matrix3d &rotationMatrix)
{
    // Rotate the geometry_ matrix and update the geometric data in atoms_.
    geometry_ *=
        rotationMatrix; // The power of Eigen: geometry_ = geometry_ * rotationMatrix;
    for (size_t i = 0; i < nAtoms_; ++i) {
        Eigen::Vector3d tmp = geometry_.col(i);
        atoms_[i].position = tmp;
    }
}

void Molecule::moveToPAF()
{
    Eigen::Matrix3d inertia = inertiaTensor();
    // Diagonalize inertia tensor V^t * I * V
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigenSolver(inertia);
    if (eigenSolver.info() != Eigen::Success) abort();
    // Rotate to Principal Axes Frame
    this->rotate(eigenSolver.eigenvectors());
    std::cout << eigenSolver.eigenvalues() << std::endl;
}

Molecule& Molecule::operator=(const Molecule& other)
{
    // Self assignment is bad
    if (this == &other)
        return *this;

    nAtoms_ = other.nAtoms_;
    charges_ = other.charges_;
    masses_ = other.masses_;
    geometry_ = other.geometry_;
    atoms_ = other.atoms_;
    spheres_ = other.spheres_;
    rotor_ = other.rotor_;
    pointGroup_ = other.pointGroup_;

    return *this;
}

std::ostream & operator<<(std::ostream &os, const Molecule &m)
{
    if (m.nAtoms_ != 0) {
      os << "                 Geometry (in Angstrom)" << std::endl;
      os << "   Center            X             Y             Z     \n";
      os << "------------   ------------  ------------  ------------\n";
      for (size_t i = 0; i < m.nAtoms_; ++i) {
        os << boost::format("%|=12s|") % m.atoms_[i].symbol;
        os << boost::format("   %10.6f  ") % (m.geometry_(0, i) * bohrToAngstrom());
        os << boost::format("  %10.6f  ")  % (m.geometry_(1, i) * bohrToAngstrom());
        os << boost::format("  %10.6f  ")  % (m.geometry_(2, i) * bohrToAngstrom());
        os << std::endl;
      }
      os << "Rotor type: " << rotorTypeList[m.rotor_];
    } else {
      os << "  No atoms in this molecule!" << std::endl;
    }

    return os;

}

Eigen::VectorXd computeMEP(const Molecule & mol, const std::vector<Element> & el)
{
    Eigen::VectorXd mep = Eigen::VectorXd::Zero(el.size());
    for (size_t i = 0; i < mol.nAtoms(); ++i) {
        for (size_t j = 0; j < el.size(); ++j) {
            double dist = (mol.geometry().col(i) - el[j].center()).norm();
            mep(j) += mol.charges(i) / dist;
        }
    }
    return mep;
}

Eigen::VectorXd computeMEP(const std::vector<Element> & el, double charge, const Eigen::Vector3d & origin)
{
    Eigen::VectorXd mep = Eigen::VectorXd::Zero(el.size());
    for (size_t i = 0; i < el.size(); ++i) {
        double dist = (origin - el[i].center()).norm();
        mep(i) += charge / dist;
    }
    return mep;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The electron analog elastic scattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace MonteCarlo{

//! The scattering distribution base class
class AnalogElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                    public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the  elastic distribution
  typedef Teuchos::Array<Utility::Pair< double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElasticDistribution;

  //! Constructor
  AnalogElasticElectronScatteringDistribution(
        const ElasticDistribution& elastic_scattering_distribution,
        const double lower_cutoff_angle = 1.0e-6,
        const bool angle_is_used_as_independent_variable = true );

  //! Destructor
  virtual ~AnalogElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  double evaluatePDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the distribution
  double evaluate( const unsigned incoming_energy_bin,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluate( const double incoming_energy,
                   const double scattering_angle ) const;

  //! Evaluate the PDF
  double evaluatePDF( const unsigned incoming_energy_bin,
                      const double scattering_angle ) const;

  //! Evaluate the CDF
  double evaluateCDF( const double incoming_energy,
                      const double scattering_angle ) const;

  //! Evaluate the cross section ratio for the cutoff angle
  double evaluateCutoffCrossSectionRatio( const double incoming_energy ) const;

  //! Return the energy at a given energy bin
  double getEnergy( const unsigned energy_bin ) const;

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle_cosine ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle_cosine,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

//protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

private:

  // The scattering angle above which the analog distribution is used
  double d_lower_cutoff_angle;

  // Independent parameter flag: false = angle cosine, true = angle (in units of pi)
  bool d_angle_is_used_as_independent_variable;

  // elastic scattering distribution without forward screening data
  ElasticDistribution d_elastic_scattering_distribution;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_ANALOG_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_AnalogElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/power.hpp>
#include <boost/units/systems/si/solid_angle.hpp>
#include <boost/units/systems/si/temperature.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

/// \file
/// CODATA recommended values of fundamental physico-chemical constants
/// CODATA 2006 values as of 2007/03/30

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

// PHYSICO-CHEMICAL
/// Avogadro constant
BOOST_UNITS_PHYSICAL_CONSTANT(N_A,quantity<inverse_amount>,6.02214179e23/mole,3.0e16/mole);
/// atomic mass constant
BOOST_UNITS_PHYSICAL_CONSTANT(m_u,quantity<mass>,1.660538782e-27*kilograms,8.3e-35*kilograms);
/// Faraday constant
BOOST_UNITS_PHYSICAL_CONSTANT(F,quantity<electric_charge_over_amount>,96485.3399*coulombs/mole,2.4e-3*coulombs/mole);
/// molar gas constant
BOOST_UNITS_PHYSICAL_CONSTANT(R,quantity<energy_over_temperature_amount>,8.314472*joules/kelvin/mole,1.5e-5*joules/kelvin/mole);
/// Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(k_B,quantity<energy_over_temperature>,1.3806504e-23*joules/kelvin,2.4e-29*joules/kelvin);
/// Stefan-Boltzmann constant
BOOST_UNITS_PHYSICAL_CONSTANT(sigma_SB,quantity<power_over_area_temperature_4>,5.670400e-8*watts/square_meter/pow<4>(kelvin),4.0e-13*watts/square_meter/pow<4>(kelvin));
/// first radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_1,quantity<power_area>,3.74177118e-16*watt*square_meters,1.9e-23*watt*square_meters);
/// first radiation constant for spectral radiance
BOOST_UNITS_PHYSICAL_CONSTANT(c_1L,quantity<power_area_over_solid_angle>,1.191042759e-16*watt*square_meters/steradian,5.9e-24*watt*square_meters/steradian);
/// second radiation constant
BOOST_UNITS_PHYSICAL_CONSTANT(c_2,quantity<length_temperature>,1.4387752e-2*meter*kelvin,2.5e-8*meter*kelvin);
/// Wien displacement law constant : lambda_max T
BOOST_UNITS_PHYSICAL_CONSTANT(b,quantity<length_temperature>,2.8977685e-3*meter*kelvin,5.1e-9*meter*kelvin);
/// Wien displacement law constant : nu_max/T
BOOST_UNITS_PHYSICAL_CONSTANT(b_prime,quantity<frequency_over_temperature>,5.878933e10*hertz/kelvin,1.0e15*hertz/kelvin);

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_PHYSICO_CHEMICAL_CONSTANTS_HPP


#include <algorithm>
#include <numeric>
#include <functional>
#include <limits>
#include <iostream>
#include <cmath>
#include <cassert>

#include <boost/lambda/lambda.hpp>

#include "BeliefPropagation.h"
#include "LogSumExp.h"

using namespace boost::lambda;

namespace Grante {

BeliefPropagation::BeliefPropagation(const FactorGraph* fg,
	MessageSchedule sched)
	: InferenceMethod(fg), verbose(false), max_iter(100), conv_tol(1.0e-5),
		sched(sched), min_sum(false),
		log_z(std::numeric_limits<double>::quiet_NaN())
{
	// Setup message indices
	const std::vector<Factor*>& factors = fg->Factors();
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const std::vector<unsigned int>& fac_vars = factors[fi]->Variables();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi) {
			// 1. Message from variable to factor
			unsigned int msg_id =
				static_cast<unsigned int>(msg_for_factor_srcvar.size());
			msglist_for_factor[fi].push_back(msg_id);
			msg_for_factor_srcvar.push_back(fac_vars[fvi]);

			// 2. Message from factor to variable
			msg_id = static_cast<unsigned int>(msg_for_var_srcfactor.size());
			msglist_for_var[fac_vars[fvi]].push_back(msg_id);
			msg_for_var_srcfactor.push_back(fi);
		}
	}

	// If using a sequential schedule, obtain a message order and id's
	if (sched == Sequential) {
		FactorGraphStructurizer::ComputeEulerianMessageTrail(fg, order);
		order_msgid.resize(order.size());

		for (size_t oi = 0; oi < order.size(); ++oi) {
			unsigned int vi = order[oi].VariableNode();
			unsigned int fi = order[oi].FactorNode();
			if (order[oi].steptype == FactorGraphStructurizer::LeafIsFactorNode) {
				// message: fi -> vi
				msg_list_t::const_iterator mli = msglist_for_var.find(vi);
				assert(mli != msglist_for_var.end());
				const std::vector<unsigned int>& msg_list = mli->second;
				for (unsigned int mi = 0; mi < msg_list.size(); ++mi) {
					if (msg_for_var_srcfactor[msg_list[mi]] != fi)
						continue;

					order_msgid[oi] = msg_list[mi];
					break;
				}
			} else {
				// message: vi -> fi
				msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
				assert(mli != msglist_for_factor.end());
				const std::vector<unsigned int>& msg_list = mli->second;
				for (unsigned int mi = 0; mi < msg_list.size(); ++mi) {
					if (msg_for_factor_srcvar[msg_list[mi]] != vi)
						continue;

					order_msgid[oi] = msg_list[mi];
					break;
				}
			}
		}
	}
}

BeliefPropagation::~BeliefPropagation() {
}

InferenceMethod* BeliefPropagation::Produce(const FactorGraph* fg) const {
	return (new BeliefPropagation(fg));
}

void BeliefPropagation::SetParameters(bool verbose,
	unsigned int max_iter, double conv_tol) {
	this->verbose = verbose;
	this->max_iter = max_iter;
	assert(conv_tol >= 0.0);
	this->conv_tol = conv_tol;
}

void BeliefPropagation::PerformInference() {
	InferenceInitialize();

	// Min-sum variables
	double best_energy = std::numeric_limits<double>::infinity();

	// Perform message passing
	double conv_measure = std::numeric_limits<double>::infinity();
	for (unsigned int iter = 1; (max_iter == 0 || iter <= max_iter) &&
		conv_measure >= conv_tol; ++iter)
	{
		if (verbose) {
			std::cout << "iter " << iter << ", conv " << conv_measure;
			if (min_sum)
				std::cout << ", E* " << best_energy;
			std::cout << std::endl;
		}

		if (sched == ParallelSync) {
			PerformInferenceStepParallel();
		} else if (sched == Sequential) {
			PerformInferenceSequential();
		} else {
			assert(0);
		}

		// Convergence measure: maximum update to marginals
		// TODO
		if (min_sum) {
			conv_measure = ComputeVariableBeliefs();
			std::vector<unsigned int> cur_state(fg->Cardinalities().size());
			double cur_energy = ReconstructMinimumEnergyState(cur_state);
			if (cur_energy < best_energy) {
				best_energy = cur_energy;
				best_state = cur_state;
			}
		} else {
			conv_measure = ConstructMarginals();
		}
	}
	ConstructMarginals();
	ComputeVariableBeliefs();
	if (min_sum) {
		if (verbose) {
			std::cout << "Converged, tol " << conv_measure << ", E* "
				<< best_energy << std::endl;
		}
	} else {
		log_z = -ComputeBetheFreeEnergy();
		if (verbose) {
			std::cout << "Converged, tol " << conv_measure
				<< ", log_z(Bethe) " << log_z << std::endl;
		}
	}

	InferenceTeardown();
}

void BeliefPropagation::InferenceInitialize() {
	// Initialize messages
	const std::vector<unsigned int>& card = fg->Cardinalities();
	//  i) factor-to-variable
	msg_for_var.resize(msg_for_var_srcfactor.size());
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;

		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());

			// Resize and fill (value does not matter for parallel schedule,
			// but does for sequential)
			msg_for_var[ml[mli]].resize(card[vi]);
			std::fill(msg_for_var[ml[mli]].begin(),
				msg_for_var[ml[mli]].end(), 0.0);
		}
	}
	// ii) variable-to-factor
	msg_for_factor.resize(msg_for_factor_srcvar.size());
	for (unsigned int mfi = 0; mfi < msg_for_factor_srcvar.size(); ++mfi) {
		unsigned int vi = msg_for_factor_srcvar[mfi];
		assert(vi < card.size());
		// Resize and fill with log(1) = 0.
		msg_for_factor[mfi].resize(card[vi]);
		std::fill(msg_for_factor[mfi].begin(), msg_for_factor[mfi].end(), 0.0);
	}

	// Initialize marginals (beliefs)
	const std::vector<Factor*>& factors = fg->Factors();
	marginals.resize(factors.size());
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		marginals[fi].resize(factors[fi]->Type()->ProdCardinalities());
		std::fill(marginals[fi].begin(), marginals[fi].end(), 0.0);
	}
}

void BeliefPropagation::InferenceTeardown() {
	// Clear messages
	msg_for_var.clear();
	msg_for_factor.clear();
}

void BeliefPropagation::ClearInferenceResult() {
	marginals.clear();
	var_beliefs.clear();
}

const std::vector<double>& BeliefPropagation::Marginal(
	unsigned int factor_id) const {
	assert(factor_id < marginals.size());
	return (marginals[factor_id]);
}

const std::vector<std::vector<double> >& BeliefPropagation::Marginals() const
{
	return (marginals);
}

double BeliefPropagation::LogPartitionFunction() const {
	return (log_z);
}

// NOT IMPLEMENTED
void BeliefPropagation::Sample(std::vector<std::vector<unsigned int> >& states,
	unsigned int sample_count) {
	assert(0);
}

double BeliefPropagation::MinimizeEnergy(std::vector<unsigned int>& state) {
	min_sum = true;
	PerformInference();
	state.resize(fg->Cardinalities().size());
	std::copy(best_state.begin(), best_state.end(), state.begin());
	min_sum = false;
	return (fg->EvaluateEnergy(state));
}

void BeliefPropagation::PerformInferenceStepParallel() {
	// 1. factor-to-variable
	PassFactorToVariable();
	// 2. variable-to-factor
	PassVariableToFactor();
}

void BeliefPropagation::PerformInferenceSequential() {
	// Perform one sequential pass over all messages
	for (unsigned int oi = 0; oi < order.size(); ++oi) {
		unsigned int vi = order[oi].VariableNode();
		unsigned int fi = order[oi].FactorNode();
		unsigned int msg_id = order_msgid[oi];

		if (order[oi].steptype == FactorGraphStructurizer::LeafIsFactorNode) {
			// message: fi -> vi
			const Factor* factor = fg->Factors()[fi];
			PassFactorToVariable(factor, vi, msg_for_var[msg_id],
				msglist_for_factor[fi]);
		} else {
			// message: vi -> fi
			PassVariableToFactor(fi, msg_for_factor[msg_id], vi);
		}
	}
}

void BeliefPropagation::PassFactorToVariable() {
	const std::vector<Factor*>& factors = fg->Factors();
	const std::vector<unsigned int>& card = fg->Cardinalities();

	// For each variable
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;

		// For each factor connected to that variable:
		// Send message from factor to variable
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());

			unsigned int factor_index = msg_for_var_srcfactor[ml[mli]];
			const Factor* factor = factors[factor_index];
			PassFactorToVariable(factor, vi, msg_for_var[ml[mli]],
				msglist_for_factor[factor_index]);
		}
	}
}

// Single message variant
void BeliefPropagation::PassFactorToVariable(const Factor* factor,
	unsigned int vi, std::vector<double>& msg,
	const std::vector<unsigned int>& msglist_for_factor_cur) {
	// Obtain type and adjacent variables of the factor
	const FactorType* ftype = factor->Type();

	const std::vector<unsigned int>& fvars = factor->Variables();
	unsigned int fvi_to = static_cast<unsigned int>(
		std::find(fvars.begin(), fvars.end(), vi) - fvars.begin());

	// Target message to be computed
	// r_{m->n}(x_n) = log sum_{x_m \ n} exp(
	//    -E(x_m) + sum_{n' \in N(m) \ n} q_{n'->m}(x_{n'}) )
	std::fill(msg.begin(), msg.end(), 0.0);

	// Compute the message within the factor type
	ftype->ComputeBPMessage(factor, vi, fvi_to,
		msglist_for_factor_cur, msg_for_factor,
		msg_for_factor_srcvar, msg, min_sum);
}

void BeliefPropagation::PassVariableToFactor() {
	const std::vector<Factor*>& factors = fg->Factors();
	// For all factors
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		// Obtain messages directed to factor fi
		msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
		assert(mli != msglist_for_factor.end());
		const std::vector<unsigned int>& ml = mli->second;

		// For all adjacent variables
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_factor.size());

			std::vector<double>& msg = msg_for_factor[ml[mli]];
			unsigned int from_var = msg_for_factor_srcvar[ml[mli]];
			PassVariableToFactor(fi, msg, from_var);
		}
	}
}

void BeliefPropagation::PassVariableToFactor(unsigned int fi,
	std::vector<double>& msg, unsigned int from_var) {
	// Target message to be computed
	//    q_{n->m}(x_n) = sum_{m' \in M(n) \ m} r_{m'->n}(x_n),
	// (26.11) McKay, in log-domain.
	std::fill(msg.begin(), msg.end(), 0.0);
	msg_list_t::const_iterator mvi = msglist_for_var.find(from_var);
	assert(mvi != msglist_for_var.end());
	const std::vector<unsigned int>& mliv = mvi->second;
	for (std::vector<unsigned int>::const_iterator fvi = mliv.begin();
		fvi != mliv.end(); ++fvi) {
		unsigned int for_var_msg_index = *fvi;
		// Skip messages from the target factor
		if (msg_for_var_srcfactor[for_var_msg_index] == fi)
			continue;

		// Add log-sum messages, (26.11)
		std::transform(msg_for_var[for_var_msg_index].begin(),
			msg_for_var[for_var_msg_index].end(),
			msg.begin(), msg.begin(), std::plus<double>());
	}

	// Normalization for numerical stability,
	//   i) sum-product: log-sum-exp = 0,
	//  ii) min-sum: sum = 0.
	double norm_delta = min_sum ?
		(std::accumulate(msg.begin(), msg.end(), 0.0) /
			static_cast<double>(msg.size()))
		: LogSumExp::Compute(msg);
	std::transform(msg.begin(), msg.end(), msg.begin(), _1 - norm_delta);
}

double BeliefPropagation::ConstructMarginals() {
	const std::vector<Factor*>& factors = fg->Factors();
	// Compute mean and variance of log_z estimate
	double marg_max_diff = -std::numeric_limits<double>::infinity();

	// Compute marginals of all factors
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* factor = factors[fi];
		const FactorType* ftype = factor->Type();

		// Obtain messages directed to factor fi
		msg_list_t::const_iterator mli = msglist_for_factor.find(fi);
		assert(mli != msglist_for_factor.end());
		const std::vector<unsigned int>& msglist_for_factor_cur = mli->second;

		double cur_marg_max_diff = ftype->ComputeBPMarginal(factor,
			msglist_for_factor_cur, msg_for_factor, marginals[fi], min_sum);
		if (cur_marg_max_diff > marg_max_diff)
			marg_max_diff = cur_marg_max_diff;

	}

	return (marg_max_diff);
}

double BeliefPropagation::ComputeVariableBeliefs() {
	const std::vector<unsigned int>& card = fg->Cardinalities();
	var_beliefs.resize(card.size());

	// For each variable
	double max_change = -std::numeric_limits<double>::infinity();
	for (unsigned int vi = 0; vi < card.size(); ++vi) {
		// Initialize beliefs
		std::vector<double> var_belief_vi_old(var_beliefs[vi]);
		var_beliefs[vi].resize(card[vi]);
		std::fill(var_beliefs[vi].begin(), var_beliefs[vi].end(), 0.0);

		// For each message directed to variable vi
		msg_list_t::const_iterator mli = msglist_for_var.find(vi);
		assert(mli != msglist_for_var.end());
		const std::vector<unsigned int>& ml = mli->second;
		for (unsigned int mli = 0; mli < ml.size(); ++mli) {
			assert(ml[mli] < msg_for_var.size());
			std::vector<double>& msg = msg_for_var[ml[mli]];

			// sum_{m \in M(vi)} log r_{m->n}(x_n)
			assert(msg.size() == card[vi]);
			std::transform(msg.begin(), msg.end(), var_beliefs[vi].begin(),
				var_beliefs[vi].begin(), std::plus<double>());
		}

		// Compute normalized variable marginal (belief)
		double Z_vi = min_sum ?
			(std::accumulate(var_beliefs[vi].begin(),
				var_beliefs[vi].end(), 0.0) /
				static_cast<double>(var_beliefs[vi].size()))
			: LogSumExp::Compute(var_beliefs[vi]);
		for (unsigned int vs = 0; vs < var_beliefs[vi].size(); ++vs) {
			if (min_sum) {
				var_beliefs[vi][vs] -= Z_vi;
			} else {
				var_beliefs[vi][vs] = std::exp(var_beliefs[vi][vs] - Z_vi);
			}
			if (var_belief_vi_old.empty()) {
				max_change = std::numeric_limits<double>::infinity();
			} else {
				max_change = std::max(max_change,
					std::fabs(var_beliefs[vi][vs] - var_belief_vi_old[vs]));
			}
		}
	}
	return (max_change);
}

double BeliefPropagation::ReconstructMinimumEnergyState(
	std::vector<unsigned int>& state) const {
	const std::vector<unsigned int>& card = fg->Cardinalities();
	assert(state.size() == card.size());
	assert(var_beliefs.size() == card.size());

	for (size_t vi = 0; vi < card.size(); ++vi) {
		assert(var_beliefs[vi].size() == card[vi]);
		state[vi] = static_cast<unsigned int>(std::max_element(
			var_beliefs[vi].begin(), var_beliefs[vi].end())
			- var_beliefs[vi].begin());
	}
	return (fg->EvaluateEnergy(state));
}

// (3.45) in [Wainwright and Jordan], "A(theta) = negative free energy"
// Theorem 5 in [Yedidia, Freeman, and Weiss], "Interior stationary points of
// the constrained Bethe free energy must be BP fixed points".
// (37) in [Yedidia, Freeman, and Weiss] gives the Bethe free energy.  This is
// an approximation to the negative log partition function logZ.
double BeliefPropagation::ComputeBetheFreeEnergy() const {
	double U_Bethe = 0.0;	// Bethe average energy
	double H_Bethe = 0.0;	// Bethe entropy
	const std::vector<Factor*>& factors = fg->Factors();
	const std::vector<unsigned int>& card = fg->Cardinalities();
	std::vector<unsigned int> var_degree(card.size(), 0);
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* factor = factors[fi];
		const std::vector<double>& energies = factor->Energies();
		size_t energies_size = energies.size();
		for (size_t ei = 0; ei < energies_size; ++ei) {
			U_Bethe += -marginals[fi][ei] * (-energies[ei]);
			H_Bethe += -marginals[fi][ei] * std::log(marginals[fi][ei]);
		}

		// Increase degrees of all variables involved in this factor
		const std::vector<unsigned int>& fac_vars = factor->Variables();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
			var_degree[fac_vars[fvi]] += 1;
	}

	size_t var_count = card.size();
	for (size_t vi = 0; vi < var_count; ++vi) {
		assert(var_degree[vi] >= 1);
		assert(var_beliefs[vi].size() == card[vi]);
		double corr = 0.0;
		for (unsigned int state = 0; state < card[vi]; ++state)
			corr += var_beliefs[vi][state] * std::log(var_beliefs[vi][state]);
		H_Bethe += static_cast<double>(var_degree[vi] - 1) * corr;
	}

	// Return the Bethe free energy, log Z = -Bethe = -(U-H)
	return (U_Bethe - H_Bethe);
}

}