/*

Copyright (c) 2005-2015, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 the University of Oxford 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.

*/

#ifndef WNTCELLCYCLEODESYSTEM_HPP_
#define WNTCELLCYCLEODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/shared_ptr.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystem.hpp"
#include "AbstractCellMutationState.hpp"

/**
 * Represents the Mirams et al. system of ODEs, based on Swat et al. (2004)
 * [doi:10.1093/bioinformatics/bth110]
 * and a simple Wnt model (unpublished)
 *
 * The variables are
 *
 * 0. r = pRb
 * 1. e = E2F1
 * 2. i = CycD (inactive)
 * 3. j = CycD (active)
 * 4. p = pRb-p
 * 5. c = destruction complex (Active)
 * 6. b1 = Beta-Catenin (from 1st allele)
 * 7. b2 = Beta-Catenin (from 1st allele)
 * 8. WntLevel
 */
class WntCellCycleOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Parameters for the Swat et al. (2004) model
     */

    /** Dimensional parameter k_2. */
    double mk2d;
    /** Dimensional parameter k_3. */
    double mk3d;
    /** Dimensional parameter k_34. */
    double mk34d;
    /** Dimensional parameter k_2. */
    double mk43d;
    /** Dimensional parameter k_23. */
    double mk23d;
    /** Dimensional parameter a. */
    double mad;
    /** Dimensional parameter J_11. */
    double mJ11d;
    /** Dimensional parameter J_12. */
    double mJ12d;
    /** Dimensional parameter J_13. */
    double mJ13d;
    /** Dimensional parameter J_13. */
    double mJ61d;
    /** Dimensional parameter J_62. */
    double mJ62d;
    /** Dimensional parameter J_63. */
    double mJ63d;
    /** Dimensional parameter K_m1. */
    double mKm1d;
    /** Dimensional parameter k_p. */
    double mkpd;
    /** Dimensionless parameter phi_r. */
    double mphi_r;
    /** Dimensionless parameter phi_i. */
    double mphi_i;
    /** Dimensionless parameter phi_j. */
    double mphi_j;
    /** Dimensionless parameter phi_p. */
    double mphi_p;
    /** Dimensional parameter a_2. */
    double ma2d;
    /** Dimensional parameter a_3. */
    double ma3d;
    /** Dimensional parameter a_4. */
    double ma4d;
    /** Dimensional parameter a_5. */
    double ma5d;
    /** Dimensional parameter k_16. */
    double mk16d;
    /** Dimensional parameter k_61. */
    double mk61d;
    /** Dimensionless parameter phi_E2F1. */
    double mPhiE2F1;

    /** The mutation state of the cell - Wnt pathway behaviour (and hence cell cycle time) changes depending on this */
    boost::shared_ptr<AbstractCellMutationState> mpMutationState;

    /** The Wnt level (this affects the ODE system). */
    double mWntLevel;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param wntLevel is a non-dimensional Wnt value between 0 and 1. This sets up the Wnt pathway in its steady state.
     * @param pMutationState optional mutation state (affects the ODE system)
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    WntCellCycleOdeSystem(double wntLevel=0.0,
                          boost::shared_ptr<AbstractCellMutationState> pMutationState=boost::shared_ptr<AbstractCellMutationState>(),
                          std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~WntCellCycleOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Set the mutation state of the cell.
     *
     * This should be called by the relevant cell-cycle model before any solving
     * of the ODE system (as it is used to evaluate the Y derivatives).
     *
     * @param pMutationState the mutation state.
     */
    void SetMutationState(boost::shared_ptr<AbstractCellMutationState> pMutationState);

    /**
     * Called by the archive function on the Wnt cell-cycle model.
     *
     * @return #mpMutationState the mutation state of the cell.
     */
    const boost::shared_ptr<AbstractCellMutationState> GetMutationState() const;

    /**
     * Compute the RHS of the WntCellCycle system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * This also contains a calculation of dY[1], copied from EvaluateYDerivatives.
     * Ensure they do not get out of sync!
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return whether we have reached the stopping event
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * When using CVODE this function is called instead of CalculateStoppingEvent.
     * It allows the point at which rY[1] reaches 1 to be found to greater precision.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return function value - giving CVODE an estimate of how close we are to the root.
     */
    double CalculateRootFunction(double time, const std::vector<double>& rY);

    /**
     * @return #mWntLevel.
     */
    double GetWntLevel() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(WntCellCycleOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a WntCellCycleOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const WntCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const double wnt_level = t->GetWntLevel();
    ar & wnt_level;

    const boost::shared_ptr<AbstractCellMutationState> p_mutation_state = t->GetMutationState();
    ar & p_mutation_state;

    const std::vector<double> state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a WntCellCycleOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, WntCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    double wnt_level;
    ar & wnt_level;

    boost::shared_ptr<AbstractCellMutationState> p_mutation_state;
    ar & p_mutation_state;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)WntCellCycleOdeSystem(wnt_level, p_mutation_state, state_variables);
}
}
} // namespace ...

#endif /*WNTCELLCYCLEODESYSTEM_HPP_*/


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program 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 General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"

#include "graph_filtering.hh"

#include "graph_selectors.hh"
#include "graph_properties.hh"

#include <cmath>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_get_radial
{
    template <class Graph, class PosProp, class LevelMap, class OrderMap,
              class WeightMap>
    void operator()(Graph& g, PosProp tpos, LevelMap level, OrderMap order,
                    WeightMap weight, size_t root, bool weighted, double r,
                    bool order_propagate) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        typedef typename vprop_map_t<typename property_traits<WeightMap>::value_type>::type vcount_t;
        typename vcount_t::unchecked_t count(get(vertex_index, g), num_vertices(g));

        if (!weighted)
        {
            for (auto v : vertices_range(g))
                count[v] = weight[v];
        }
        else
        {
            deque<vertex_t> q;
            for (auto v : vertices_range(g))
            {
                if (out_degree(v, g) == 0)
                {
                    q.push_back(v);
                    count[v] = weight[v];
                }
            }

            typedef vprop_map_t<uint8_t>::type vmark_t;
            vmark_t::unchecked_t mark(get(vertex_index, g), num_vertices(g));

            while (!q.empty())
            {
                vertex_t v = q.front();
                q.pop_front();
                for (auto e : in_edges_range(v, g))
                {
                    vertex_t w = source(e, g);
                    count[w] += count[v];
                    if (!mark[w])
                    {
                        q.push_back(w);
                        mark[w] = true;
                    }
                }
            }
        }

        vprop_map_t<double>::type::unchecked_t vorder(get(vertex_index, g));

        if (order_propagate)
        {
            vorder.resize(num_vertices(g));
            std::vector<size_t> vs(vertices(g).first, vertices(g).second);
            std::sort(vs.begin(), vs.end(),
                      [&] (vertex_t u, vertex_t v) { return order[u] < order[v]; });

            for (size_t i = 0; i < vs.size(); ++i)
                vorder[vs[i]] = i;

            std::sort(vs.begin(), vs.end(),
                      [&] (vertex_t u, vertex_t v) { return level[u] > level[v]; });

            for (auto v : vs)
            {
                if (out_degree(v,g) == 0)
                    continue;
                vorder[v] = 0;
                for (auto e : out_edges_range(v, g))
                    vorder[v] += vorder[target(e,g)];
                vorder[v] /= out_degree(v,g);
            }
        }

        vector<vector<vertex_t>> layers(1);
        layers[0].push_back(root);

        bool last = false;
        while (!last)
        {
            layers.resize(layers.size() + 1);
            vector<vertex_t>& new_layer = layers[layers.size() - 1];
            vector<vertex_t>& last_layer = layers[layers.size() - 2];

            last = true;
            for (size_t i = 0; i < last_layer.size(); ++i)
            {
                vertex_t v = last_layer[i];
                for (auto e : out_edges_range(v, g))
                {
                    vertex_t w = target(e, g);
                    new_layer.push_back(w);

                    if (int(layers.size()) - 1 == int(level[w]))
                        last = false;
                }

                if (order_propagate)
                {
                    std::sort(new_layer.end() - out_degree(v, g),
                              new_layer.end(),
                              [&] (vertex_t u, vertex_t v)
                              { return vorder[u] < vorder[v]; });
                }
                else
                {
                    std::sort(new_layer.end() - out_degree(v, g),
                              new_layer.end(),
                              [&] (vertex_t u, vertex_t v)
                              { return order[u] < order[v]; });
                }

                if (out_degree(v, g) == 0)
                    new_layer.push_back(v);
            }

            if (last)
                layers.pop_back();
        }

        typedef vprop_map_t<double>::type vangle_t;
        vangle_t::unchecked_t angle(get(vertex_index, g), num_vertices(g));

        double d_sum = 0;
        vector<vertex_t>& outer_layer = layers.back();
        for (size_t i = 0; i < outer_layer.size(); ++i)
            d_sum += count[outer_layer[i]];
        angle[outer_layer[0]] = (2 * M_PI * count[outer_layer[0]]) / d_sum;
        for (size_t i = 1; i < outer_layer.size(); ++i)
            angle[outer_layer[i]] = angle[outer_layer[i-1]] + (2 * M_PI * count[outer_layer[i]]) / d_sum;
        for (size_t i = 0; i < outer_layer.size(); ++i)
            angle[outer_layer[i]] -= (2 * M_PI * count[outer_layer[i]]) / (2 * d_sum);

        for (size_t i = 0; i < layers.size(); ++i)
        {
            vector<vertex_t>& vs = layers[layers.size() - 1 - i];
            for (size_t j = 0; j < vs.size(); ++j)
            {
                vertex_t v = vs[j];
                d_sum = 0;
                for (auto e : out_edges_range(v, g))
                {
                    vertex_t w = target(e, g);
                    d_sum += count[w];
                }
                for (auto e : out_edges_range(v, g))
                {
                    vertex_t w = target(e, g);
                    angle[v] += angle[w] * count[w] / d_sum;
                }
                double d = level[v] * r;
                tpos[v].resize(2);
                tpos[v][0] = d * cos(angle[v]);
                tpos[v][1] = d * sin(angle[v]);
            }
        }
    }
};

void get_radial(GraphInterface& gi, boost::any otpos, boost::any olevels,
                boost::any oorder, boost::any oweight, size_t root,
                bool weighted, double r, bool order_propagate)
{
    typedef vprop_map_t<int32_t>::type vmap_t;

    vmap_t levels = boost::any_cast<vmap_t>(olevels);

    typedef vprop_map_t<double>::type wmap_t;

    wmap_t weight = boost::any_cast<wmap_t>(oweight);

    run_action<graph_tool::detail::always_directed>()
        (gi, std::bind(do_get_radial(), std::placeholders::_1, std::placeholders::_2,
                       levels, std::placeholders::_3, weight, root, weighted, r,
                       order_propagate),
         vertex_scalar_vector_properties(),
         vertex_properties())(otpos, oorder);
}

#include <boost/python.hpp>

void export_radial()
{
    python::def("get_radial", &get_radial);
}


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <boost/make_shared.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/utilities/dataformatters.hpp>
#include <qle/termstructures/blackvariancesurfacemoneyness.hpp>

using namespace std;

namespace QuantExt {

BlackVarianceSurfaceMoneyness::BlackVarianceSurfaceMoneyness(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceTermStructure(0, cal, Following, dayCounter), stickyStrike_(stickyStrike), spot_(spot),
      times_(times), moneyness_(moneyness), flatExtrapMoneyness_(flatExtrapMoneyness), quotes_(blackVolMatrix) {
    init();
}

BlackVarianceSurfaceMoneyness::BlackVarianceSurfaceMoneyness(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceTermStructure(referenceDate, cal, Following, dayCounter), stickyStrike_(stickyStrike), spot_(spot),
      times_(times), moneyness_(moneyness), flatExtrapMoneyness_(flatExtrapMoneyness), quotes_(blackVolMatrix) {
    init();
}

void BlackVarianceSurfaceMoneyness::update() {
    TermStructure::update();
    LazyObject::update();
}

void BlackVarianceSurfaceMoneyness::performCalculations() const {
    for (Size j = 1; j < variances_.columns(); j++) {
        for (Size i = 0; i < variances_.rows(); i++) {
            Real vol = quotes_[i][j - 1]->value();
            variances_[i][j] = times_[j] * vol * vol;
        }
    }
    varianceSurface_.update();
}

void BlackVarianceSurfaceMoneyness::init() {

    QL_REQUIRE(times_.size() == quotes_.front().size(), "mismatch between times vector and vol matrix colums");
    QL_REQUIRE(moneyness_.size() == quotes_.size(), "mismatch between moneyness vector and vol matrix rows");

    QL_REQUIRE(times_[0] > 0, "The first time must be greater than 0.0.");

    if (stickyStrike_) {
        // we don't want to know if the spot has changed - we take a copy here
        spot_ = Handle<Quote>(boost::make_shared<SimpleQuote>(spot_->value()));
    } else {
        registerWith(spot_);
    }

    // Insert time 0.0 in times_ and initialise variances_ with 0.0.
    times_.insert(times_.begin(), 0.0);
    variances_ = Matrix(moneyness_.size(), times_.size(), 0.0);

    // Check times_ and register with quotes
    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] << ".");

        for (Size i = 0; i < moneyness_.size(); i++) {
            registerWith(quotes_[i][j - 1]);
        }
    }

    varianceSurface_ =
        Bilinear().interpolate(times_.begin(), times_.end(), moneyness_.begin(), moneyness_.end(), variances_);

    notifyObservers();
}

Real BlackVarianceSurfaceMoneyness::blackVarianceImpl(Time t, Real strike) const {

    calculate();

    if (t == 0.0)
        return 0.0;

    return blackVarianceMoneyness(t, moneyness(t, strike));
}

Real BlackVarianceSurfaceMoneyness::blackVarianceMoneyness(Time t, Real m) const {
    if (t <= times_.back())
        return varianceSurface_(t, m, true);
    else
        return varianceSurface_(times_.back(), m, true) * t / times_.back();
}

BlackVarianceSurfaceMoneynessSpot::BlackVarianceSurfaceMoneynessSpot(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike,
                                    flatExtrapMoneyness) {}

BlackVarianceSurfaceMoneynessSpot::BlackVarianceSurfaceMoneynessSpot(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(referenceDate, cal, spot, times, moneyness, blackVolMatrix, dayCounter,
                                    stickyStrike, flatExtrapMoneyness) {}

Real BlackVarianceSurfaceMoneynessSpot::moneyness(Time, Real strike) const {
    if (strike == Null<Real>() || strike == 0) {
        return 1.0;
    } else {
        Real moneyness = strike / spot_->value();
        if (flatExtrapMoneyness_) {
            if (moneyness < moneyness_.front()) {
                moneyness = moneyness_.front();
            } else if (moneyness > moneyness_.back()) {
                moneyness = moneyness_.back();
            }
        }
        return moneyness;
    }
}

BlackVarianceSurfaceMoneynessForward::BlackVarianceSurfaceMoneynessForward(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter,
    const Handle<YieldTermStructure>& forTS, const Handle<YieldTermStructure>& domTS, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike,
                                    flatExtrapMoneyness),
      forTS_(forTS), domTS_(domTS) {
    init();
}

BlackVarianceSurfaceMoneynessForward::BlackVarianceSurfaceMoneynessForward(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, const Handle<YieldTermStructure>& forTS, const Handle<YieldTermStructure>& domTS,
    bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(referenceDate, cal, spot, times, moneyness, blackVolMatrix, dayCounter,
                                    stickyStrike, flatExtrapMoneyness),
      forTS_(forTS), domTS_(domTS) {
    init();
}

void BlackVarianceSurfaceMoneynessForward::init() {

    if (!stickyStrike_) {
        QL_REQUIRE(!forTS_.empty(), "foreign discount curve required for atmf surface");
        QL_REQUIRE(!domTS_.empty(), "domestic discount curve required for atmf surface");
        registerWith(forTS_);
        registerWith(domTS_);
    } else {
        for (Size i = 0; i < times_.size(); i++) {
            Time t = times_[i];
            Real fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
            forwards_.push_back(fwd);
        }
        forwardCurve_ = Linear().interpolate(times_.begin(), times_.end(), forwards_.begin());
    }
}

Real BlackVarianceSurfaceMoneynessForward::moneyness(Time t, Real strike) const {
    Real fwd;
    Real reqMoneyness; // for flat extrapolation
    if (strike == Null<Real>() || strike == 0)
        return 1.0;
    else {
        if (stickyStrike_)
            fwd = forwardCurve_(t, true);
        else
            fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
        reqMoneyness = strike / fwd;
        if (flatExtrapMoneyness_) {
            if ((strike / fwd) < moneyness_.front()) {
                reqMoneyness = moneyness_.front();
            } else if ((strike / fwd) > moneyness_.back()) {
                reqMoneyness = moneyness_.back();
            }
        }
        return reqMoneyness;
    }
}

} // namespace QuantExt


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2022 Matt Borland. 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_MP_DETAIL_FUNCTIONS_TRUNC_HPP
#define BOOST_MP_DETAIL_FUNCTIONS_TRUNC_HPP

#include <cmath>
#include <limits>
#include <stdexcept>
#include <boost/multiprecision/detail/standalone_config.hpp>
#include <boost/multiprecision/detail/no_exceptions_support.hpp>

#ifdef BOOST_MP_MATH_AVAILABLE
#include <boost/math/special_functions/trunc.hpp>
#endif

namespace boost { namespace multiprecision { namespace detail {

namespace impl {

template <typename T>
inline T trunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    using std::floor;
    using std::ceil;

    return (arg > 0) ? floor(arg) : ceil(arg);
}

} // namespace impl

#ifdef BOOST_MP_MATH_AVAILABLE

template <typename T>
inline long long lltrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    return boost::math::lltrunc(arg);
}

template <typename T>
inline int itrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    return boost::math::itrunc(arg);
}

#else

template <typename T>
inline long long lltrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    if (arg > LLONG_MAX)
    {
        BOOST_MP_THROW_EXCEPTION(std::domain_error("arg cannot be converted into a long long"));
    }

    return static_cast<long long>(boost::multiprecision::detail::impl::trunc(arg));
}

template <typename T>
inline int itrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    if (arg > INT_MAX)
    {
        BOOST_MP_THROW_EXCEPTION(std::domain_error("arg cannot be converted into an int"));
    }

    return static_cast<int>(boost::multiprecision::detail::impl::trunc(arg));
}

#endif

}}} // Namespaces

#endif // BOOST_MP_DETAIL_FUNCTIONS_TRUNC_HPP


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include <boost/python/def.hpp>

#include "pxr/pxr.h"
#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/pyContainerConversions.h"

#include "pxr/base/gf/math.h"

#include "pxr/base/gf/vec2i.h"
#include "pxr/base/gf/vec3i.h"
#include "pxr/base/gf/vec2f.h"
#include "pxr/base/gf/vec3f.h"
#include "pxr/base/gf/vec4f.h"
#include "pxr/base/gf/vec2d.h"
#include "pxr/base/gf/vec3d.h"
#include "pxr/base/gf/vec4d.h"

using namespace boost::python;
using std::vector;

PXR_NAMESPACE_USING_DIRECTIVE

void wrapMath()
{

    def("IsClose", (bool (*)(double, double, double))GfIsClose);
    def("RadiansToDegrees", GfRadiansToDegrees);
    def("DegreesToRadians", GfDegreesToRadians);

    def("Sqr", GfSqr<double>);
    def("Sqr", GfSqr<int>);

    def("Sqr", GfSqr<GfVec2i>);
    def("Sqr", GfSqr<GfVec3i>);
    def("Sqr", GfSqr<GfVec2f>);
    def("Sqr", GfSqr<GfVec3f>);
    def("Sqr", GfSqr<GfVec4f>);
    def("Sqr", GfSqr<GfVec2d>);
    def("Sqr", GfSqr<GfVec3d>);
    def("Sqr", GfSqr<GfVec4d>);

    def("Sgn", GfSgn<double>);
    def("Sgn", GfSgn<int>);

    def("Sqrt", (double (*)(double))GfSqrt);
    def("Sqrtf", (float (*)(float))GfSqrt,
        "Sqrtf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Sqrt() to return the square root of f as a float instead of a double.");

    def("Exp", (double (*)(double))GfExp);
    def("Expf", (float (*)(float))GfExp,
        "Expf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Exp() to return the exponent of f as a float instead of a double.");

    def("Log", (double (*)(double))GfLog);
    def("Logf", (float (*)(float))GfLog,
        "Logf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Log() to return the logarithm of f as a float instead of a double.");

    def("Floor", (double (*)(double))GfFloor);
    def("Floorf", (float (*)(float))GfFloor,
        "Floorf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Floor() to return the floor of f as a float instead of a double.");

    def("Ceil", (double (*)(double))GfCeil);
    def("Ceilf", (float (*)(float))GfCeil,
        "Ceilf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Ceil() to return the ceiling of f as a float instead of a double.");

    def("Abs", (double (*)(double))GfAbs);
    def("Absf", (float (*)(float))GfAbs,
        "Absf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Abs() to return the absolute value of f as a float instead of a double.");

    def("Round", (double (*)(double))GfRound);
    def("Roundf", (float (*)(float))GfRound,
        "Roundf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Round() to return the rounded value of f as a float instead of a double.");

    def("Pow", (double (*)(double, double))GfPow);
    def("Powf", (float (*)(float, float))GfPow,
        "Powf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Pow() to return the power of f as a float instead of a double.");

    def("Clamp", (double (*)(double, double, double))GfClamp);
    def("Clampf", (float (*)(float, float, float))GfClamp,
        "Clampf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Clamp() to return the clamped value of f as a float instead of a double.");

    def("Mod", (double (*)(double, double))GfMod);
    def("Modf", (float (*)(float, float))GfMod,
        "Modf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Mod() to return the modulus of f as a float instead of a double.");

    def("Lerp", GfLerp<double>);
    def("Lerpf", GfLerp<float>,
         "Lerpf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Lerp() to return the linear interpolation of f as a float instead of a double.");

    def("Lerp", GfLerp<GfVec2i>);
    def("Lerp", GfLerp<GfVec3i>);
    def("Lerp", GfLerp<GfVec2f>);
    def("Lerp", GfLerp<GfVec3f>);
    def("Lerp", GfLerp<GfVec4f>);
    def("Lerp", GfLerp<GfVec2d>);
    def("Lerp", GfLerp<GfVec3d>);
    def("Lerp", GfLerp<GfVec4d>);

    def("Min", (double (*)(double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double, double, double))
        GfMin<double>);
    def("Min", (int (*)(int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int, int, int)) GfMin<int>);

    def("Max", (double (*)(double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double, double, double))
        GfMax<double>);
    def("Max", (int (*)(int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int, int, int)) GfMax<int>);

    def("Dot", (double (*)(double, double)) GfDot);

    TfPyContainerConversions::from_python_sequence< std::vector<int>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<unsigned int>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<bool>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<double>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector< std::vector<int> >, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector< std::vector<double> >, TfPyContainerConversions::variable_capacity_policy>();

}


/***************************************************************************
 *   Copyright (C) by GFZ Potsdam                                          *
 *                                                                         *
 *   You can redistribute and/or modify this program under the             *
 *   terms of the SeisComP Public License.                                 *
 *                                                                         *
 *   This program 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         *
 *   SeisComP Public License for more details.                             *
 ***************************************************************************/

#define SEISCOMP_COMPONENT MLh

#include <seiscomp3/logging/log.h>
#include <seiscomp3/processing/amplitudes/MLh.h>

#include <boost/bind.hpp>
#include <cstdio>

namespace Seiscomp {
namespace Processing {

namespace {

AmplitudeProcessor::AmplitudeValue average(
	const AmplitudeProcessor::AmplitudeValue &v0,
	const AmplitudeProcessor::AmplitudeValue &v1)
{
	AmplitudeProcessor::AmplitudeValue v;
	// Average both values
	v.value = (v0.value + v1.value) * 0.5;

	// Compute lower and upper uncertainty
	double v0l = v0.value;
	double v0u = v0.value;
	double v1l = v1.value;
	double v1u = v1.value;

	if ( v0.lowerUncertainty ) v0l -= *v0.lowerUncertainty;
	if ( v0.upperUncertainty ) v0u += *v0.upperUncertainty;
	if ( v1.lowerUncertainty ) v1l -= *v1.lowerUncertainty;
	if ( v1.upperUncertainty ) v1u += *v1.upperUncertainty;

	double l = 0, u = 0;

	l = std::max(l, v.value - v0l);
	l = std::max(l, v.value - v0u);
	l = std::max(l, v.value - v1l);
	l = std::max(l, v.value - v1u);

	u = std::max(l, v0l - v.value);
	u = std::max(l, v0u - v.value);
	u = std::max(l, v1l - v.value);
	u = std::max(l, v1u - v.value);

	v.lowerUncertainty = l;
	v.upperUncertainty = u;

	return v;
}

AmplitudeProcessor::AmplitudeTime average(
	const AmplitudeProcessor::AmplitudeTime &t0,
	const AmplitudeProcessor::AmplitudeTime &t1)
{
	AmplitudeProcessor::AmplitudeTime t;
	t.reference = Core::Time((double(t0.reference) + double(t1.reference)) * 0.5);

	// Compute lower and upper uncertainty
	Core::Time t0b = t0.reference + Core::TimeSpan(t0.begin);
	Core::Time t0e = t0.reference + Core::TimeSpan(t0.end);
	Core::Time t1b = t1.reference + Core::TimeSpan(t1.begin);
	Core::Time t1e = t1.reference + Core::TimeSpan(t1.end);

	Core::Time minTime = t.reference;
	Core::Time maxTime = t.reference;

	minTime = std::min(minTime, t0b);
	minTime = std::min(minTime, t0e);
	minTime = std::min(minTime, t1b);
	minTime = std::min(minTime, t1e);

	maxTime = std::max(maxTime, t0b);
	maxTime = std::max(maxTime, t0e);
	maxTime = std::max(maxTime, t1b);
	maxTime = std::max(maxTime, t1e);

	t.begin = (double)(minTime - t.reference);
	t.end = (double)(maxTime - t.reference);

	return t;
}

AmplitudeProcessor::AmplitudeValue gmean(
	const AmplitudeProcessor::AmplitudeValue &v0,
	const AmplitudeProcessor::AmplitudeValue &v1)
{
	AmplitudeProcessor::AmplitudeValue v;
	// Average both values
	v.value = sqrt(v0.value * v1.value);

	// Compute lower and upper uncertainty
	double v0l = v0.value;
	double v0u = v0.value;
	double v1l = v1.value;
	double v1u = v1.value;

	if ( v0.lowerUncertainty ) v0l -= *v0.lowerUncertainty;
	if ( v0.upperUncertainty ) v0u += *v0.upperUncertainty;
	if ( v1.lowerUncertainty ) v1l -= *v1.lowerUncertainty;
	if ( v1.upperUncertainty ) v1u += *v1.upperUncertainty;

	double l = 0, u = 0;

	l = std::max(l, v.value - v0l);
	l = std::max(l, v.value - v0u);
	l = std::max(l, v.value - v1l);
	l = std::max(l, v.value - v1u);

	u = std::max(l, v0l - v.value);
	u = std::max(l, v0u - v.value);
	u = std::max(l, v1l - v.value);
	u = std::max(l, v1u - v.value);

	v.lowerUncertainty = l;
	v.upperUncertainty = u;

	return v;
}

}

IMPLEMENT_SC_CLASS_DERIVED(AmplitudeProcessor_ML2h, AbstractAmplitudeProcessor_ML, "AmplitudeProcessor_ML");
REGISTER_AMPLITUDEPROCESSOR(AmplitudeProcessor_ML2h, "ML");

AmplitudeProcessor_MLh::AmplitudeProcessor_MLh()
: AbstractAmplitudeProcessor_ML("ML") {}

AmplitudeProcessor_ML2h::AmplitudeProcessor_ML2h()
: Processing::AmplitudeProcessor("ML") {
	setSignalEnd(150.);
	setMinSNR(0);
	// Maximum distance is 8 degrees
	setMaxDist(8);
	// Maximum depth is 80 km
	setMaxDepth(80);

	setUsedComponent(Horizontal);

	_combiner = TakeAverage;

	_ampN.setUsedComponent(FirstHorizontal);
	_ampE.setUsedComponent(SecondHorizontal);

	_ampE.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));
	_ampN.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));

	// Propagate configuration to single processors
	_ampN.setConfig(config());
	_ampE.setConfig(config());
}

AmplitudeProcessor_ML2h::AmplitudeProcessor_ML2h(const Core::Time &trigger)
: Processing::AmplitudeProcessor(trigger, "ML") {
	setSignalEnd(150.);
	setMinSNR(0);
	// Maximum distance is 8 degrees
	setMaxDist(8);
	// Maximum depth is 80 km
	setMaxDepth(80);

	setUsedComponent(Horizontal);

	_combiner = TakeAverage;

	_ampN.setUsedComponent(FirstHorizontal);
	_ampE.setUsedComponent(SecondHorizontal);

	_ampE.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));
	_ampN.setPublishFunction(boost::bind(&AmplitudeProcessor_ML2h::newAmplitude, this, _1, _2));

	// Propagate configuration to single processors
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_ampN.setTrigger(trigger);
	_ampE.setTrigger(trigger);
}

const AmplitudeProcessor *AmplitudeProcessor_ML2h::componentProcessor(Component comp) const {
	switch ( comp ) {
		case FirstHorizontalComponent:
			return &_ampN;
		case SecondHorizontalComponent:
			return &_ampE;
		default:
			break;
	}

	return NULL;
}

const DoubleArray *AmplitudeProcessor_ML2h::processedData(Component comp) const {
	switch ( comp ) {
		case FirstHorizontalComponent:
			return _ampN.processedData(comp);
		case SecondHorizontalComponent:
			return _ampE.processedData(comp);
		default:
			break;
	}

	return NULL;
}

void AmplitudeProcessor_ML2h::reprocess(OPT(double) searchBegin, OPT(double) searchEnd) {
	setStatus(WaitingForData, 0);
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_results[0] = _results[1] = Core::None;

	_ampN.reprocess(searchBegin, searchEnd);
	_ampE.reprocess(searchBegin, searchEnd);

	if ( !isFinished() ) {
		if ( _ampN.status() > Finished )
			setStatus(_ampN.status(), _ampN.statusValue());
		else
			setStatus(_ampE.status(), _ampE.statusValue());
	}
}

int AmplitudeProcessor_ML2h::capabilities() const {
	return _ampN.capabilities() | Combiner;
}

AmplitudeProcessor::IDList
AmplitudeProcessor_ML2h::capabilityParameters(Capability cap) const {
	if ( cap == Combiner ) {
		IDList params;
		params.push_back("Average");
		params.push_back("Max");
		params.push_back("Min");
		params.push_back("Geometric mean");
		return params;
	}

	return _ampN.capabilityParameters(cap);
}

bool AmplitudeProcessor_ML2h::setParameter(Capability cap, const std::string &value) {
	if ( cap == Combiner ) {
		if ( value == "Min" ) {
			_combiner = TakeMin;
			return true;
		}
		else if ( value == "Max" ) {
			_combiner = TakeMax;
			return true;
		}
		else if ( value == "Average" ) {
			_combiner = TakeAverage;
			return true;
		}
		else if ( value == "Geometric mean" ) {
			_combiner = TakeGeometricMean;
			return true;
		}

		return false;
	}

	_ampN.setParameter(cap, value);
	return _ampE.setParameter(cap, value);
}

bool AmplitudeProcessor_ML2h::setup(const Settings &settings) {
	// Copy the stream configurations (gain, orientation, responses, ...) to
	// the horizontal processors
	_ampN.streamConfig(FirstHorizontalComponent) = streamConfig(FirstHorizontalComponent);
	_ampE.streamConfig(SecondHorizontalComponent) = streamConfig(SecondHorizontalComponent);

	_combiner = TakeAverage;

	try {
		std::string s = settings.getString("amplitudes." + _type + ".combiner");
		if ( s == "average" )
			_combiner = TakeAverage;
		else if ( s == "max" )
			_combiner = TakeMax;
		else if ( s == "min" )
			_combiner = TakeMin;
		else if ( s == "geometric_mean" )
			_combiner = TakeGeometricMean;
		else {
			SEISCOMP_ERROR("%s: invalid combiner type for station %s.%s: %s",
			               _type.c_str(),
			               settings.networkCode.c_str(), settings.stationCode.c_str(),
			               s.c_str());
			return false;
		}
	}
	catch ( ... ) {}

	if ( !AmplitudeProcessor::setup(settings) ) return false;

	// Setup each component
	if ( !_ampN.setup(settings) || !_ampE.setup(settings) ) return false;

	return true;
}

void AmplitudeProcessor_ML2h::setTrigger(const Core::Time& trigger) {
	AmplitudeProcessor::setTrigger(trigger);
	_ampE.setTrigger(trigger);
	_ampN.setTrigger(trigger);
}

void AmplitudeProcessor_ML2h::computeTimeWindow() {
	// Copy configuration to each component
	_ampN.setConfig(config());
	_ampE.setConfig(config());

	_ampE.computeTimeWindow();
	_ampN.computeTimeWindow();
	setTimeWindow(_ampE.timeWindow() | _ampN.timeWindow());
}

double AmplitudeProcessor_ML2h::timeWindowLength(double distance_deg) const {
	double endN = _ampN.timeWindowLength(distance_deg);
	double endE = _ampE.timeWindowLength(distance_deg);
	_ampN.setSignalEnd(endN);
	_ampE.setSignalEnd(endE);
	return std::max(endN, endE);
}

void AmplitudeProcessor_ML2h::reset() {
	AmplitudeProcessor::reset();

	_results[0] = _results[1] = Core::None;

	_ampE.reset();
	_ampN.reset();
}

void AmplitudeProcessor_ML2h::close() {
	// TODO: Check for best available amplitude here
}

bool AmplitudeProcessor_ML2h::feed(const Record *record) {
	// Both processors finished already?
	if ( _ampE.isFinished() && _ampN.isFinished() ) return false;

	// Did an error occur?
	if ( status() > WaveformProcessor::Finished ) return false;

	if ( record->channelCode() == _streamConfig[FirstHorizontalComponent].code() ) {
		if ( !_ampN.isFinished() ) {
			_ampN.feed(record);
			if ( _ampN.status() == InProgress )
				setStatus(WaveformProcessor::InProgress, _ampN.statusValue());
			else if ( _ampN.isFinished() && _ampE.isFinished() ) {
				if ( !isFinished() ) {
					if ( _ampE.status() == Finished )
						setStatus(_ampN.status(), _ampN.statusValue());
					else
						setStatus(_ampE.status(), _ampE.statusValue());
				}
			}
		}
	}
	else if ( record->channelCode() == _streamConfig[SecondHorizontalComponent].code() ) {
		if ( !_ampE.isFinished() ) {
			_ampE.feed(record);
			if ( _ampE.status() == InProgress )
				setStatus(WaveformProcessor::InProgress, _ampE.statusValue());
			else if ( _ampE.isFinished() && _ampN.isFinished() ) {
				if ( !isFinished() ) {
					if ( _ampN.status() == Finished )
						setStatus(_ampE.status(), _ampE.statusValue());
					else
						setStatus(_ampN.status(), _ampN.statusValue());
				}
			}
		}
	}

	return true;
}

bool AmplitudeProcessor_ML2h::computeAmplitude(const DoubleArray &data,
                                               size_t i1, size_t i2,
                                               size_t si1, size_t si2,
                                               double offset,
                                               AmplitudeIndex *dt,
                                               AmplitudeValue *amplitude,
                                               double *period, double *snr) {
	return false;
}

void AmplitudeProcessor_ML2h::newAmplitude(const AmplitudeProcessor *proc,
                                           const AmplitudeProcessor::Result &res) {

	if ( isFinished() ) return;

	int idx = 0;

	if ( proc == &_ampE ) {
		idx = 0;
	}
	else if ( proc == &_ampN ) {
		idx = 1;
	}

	_results[idx] = ComponentResult();
	_results[idx]->value = res.amplitude;
	_results[idx]->time = res.time;
	_results[idx]->snr = res.snr;

	if ( _results[0] && _results[1] ) {
		setStatus(Finished, 100.);
		Result newRes;
		newRes.record = res.record;

		switch ( _combiner ) {
			case TakeAverage:
				newRes.amplitude = average(_results[0]->value, _results[1]->value);
				newRes.time = average(_results[0]->time, _results[1]->time);
				newRes.component = Horizontal;
				break;
			case TakeGeometricMean:
				newRes.amplitude = gmean(_results[0]->value, _results[1]->value);
				newRes.time = average(_results[0]->time, _results[1]->time);
				newRes.component = Horizontal;
				break;
			case TakeMin:
				if ( _results[0]->value.value <= _results[1]->value.value ) {
					newRes.amplitude = _results[0]->value;
					newRes.time = _results[0]->time;
					newRes.component = _ampE.usedComponent();
				}
				else {
					newRes.amplitude = _results[1]->value;
					newRes.time = _results[1]->time;
					newRes.component = _ampN.usedComponent();
				}
				break;
			case TakeMax:
				if ( _results[0]->value.value >= _results[1]->value.value ) {
					newRes.amplitude =  _results[0]->value;
					newRes.time = _results[0]->time;
					newRes.component = _ampE.usedComponent();
				}
				else {
					newRes.amplitude =  _results[1]->value;
					newRes.time = _results[1]->time;
					newRes.component = _ampN.usedComponent();
				}
				break;
		};

		newRes.period = -1;
		newRes.snr = std::min(_results[0]->snr, _results[1]->snr);
		emitAmplitude(newRes);
	}
}

}
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/poses/CPoseRandomSampler.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::random;

/*---------------------------------------------------------------
		Constructor
  ---------------------------------------------------------------*/
CPoseRandomSampler::CPoseRandomSampler() = default;
CPoseRandomSampler::CPoseRandomSampler(const CPoseRandomSampler& o)
{
	*this = o;
}

CPoseRandomSampler& CPoseRandomSampler::operator=(const CPoseRandomSampler& o)
{
	if (&o == this) return *this;
	m_pdf2D.reset();
	m_pdf3D.reset();
	if (o.m_pdf2D) m_pdf2D.reset(dynamic_cast<CPosePDF*>(o.m_pdf2D->clone()));
	if (o.m_pdf3D) m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(o.m_pdf3D->clone()));
	m_fastdraw_gauss_Z3 = o.m_fastdraw_gauss_Z3;
	m_fastdraw_gauss_Z6 = o.m_fastdraw_gauss_Z6;
	m_fastdraw_gauss_M_2D = o.m_fastdraw_gauss_M_2D;
	m_fastdraw_gauss_M_3D = o.m_fastdraw_gauss_M_3D;
	return *this;
}

CPoseRandomSampler::CPoseRandomSampler(CPoseRandomSampler&& o)
	: m_pdf2D(nullptr), m_pdf3D(nullptr)
{
	if (o.m_pdf2D)
	{
		m_pdf2D = std::move(o.m_pdf2D);
		o.m_pdf2D = nullptr;
	}
	if (o.m_pdf3D)
	{
		m_pdf3D = std::move(o.m_pdf3D);
		o.m_pdf3D = nullptr;
	}
	m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
	m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
	m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
	m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
}
CPoseRandomSampler& CPoseRandomSampler::operator=(CPoseRandomSampler&& o)
{
	if (this == &o) return *this;
	this->clear();
	if (o.m_pdf2D)
	{
		m_pdf2D = std::move(o.m_pdf2D);
		o.m_pdf2D = nullptr;
	}
	if (o.m_pdf3D)
	{
		m_pdf3D = std::move(o.m_pdf3D);
		o.m_pdf3D = nullptr;
	}
	m_fastdraw_gauss_Z3 = std::move(o.m_fastdraw_gauss_Z3);
	m_fastdraw_gauss_Z6 = std::move(o.m_fastdraw_gauss_Z6);
	m_fastdraw_gauss_M_2D = std::move(o.m_fastdraw_gauss_M_2D);
	m_fastdraw_gauss_M_3D = std::move(o.m_fastdraw_gauss_M_3D);
	return *this;
}

/*---------------------------------------------------------------
			clear
  ---------------------------------------------------------------*/
void CPoseRandomSampler::clear()
{
	m_pdf2D.reset();
	m_pdf3D.reset();
}

/*---------------------------------------------------------------
						setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPosePDF& pdf)
{
	MRPT_START

	clear();
	m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf.clone()));

	// According to the PDF type:
	if (IS_CLASS(pdf, CPosePDFGaussian))
	{
		const auto& gPdf = dynamic_cast<const CPosePDFGaussian&>(pdf);
		const CMatrixDouble33& cov = gPdf.cov;

		m_fastdraw_gauss_M_2D = gPdf.mean;

		std::vector<double> eigVals;
		cov.eig_symmetric(m_fastdraw_gauss_Z3, eigVals);

		// Scale eigenvectors with eigenvalues:
		mrpt::math::CMatrixDouble33 D;
		D.setDiagonal(eigVals);
		D = D.asEigen().array().sqrt().matrix();
		m_fastdraw_gauss_Z3.matProductOf_AB(m_fastdraw_gauss_Z3, D);
	}
	else if (IS_CLASS(pdf, CPosePDFParticles))
	{
		return;	 // Nothing to prepare.
	}
	else
	{
		THROW_EXCEPTION_FMT(
			"Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className);
	}

	MRPT_END
}

/*---------------------------------------------------------------
						setPosePDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPose3DPDF& pdf)
{
	MRPT_START

	clear();
	m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf.clone()));

	// According to the PDF type:
	if (IS_CLASS(pdf, CPose3DPDFGaussian))
	{
		const auto& gPdf = dynamic_cast<const CPose3DPDFGaussian&>(pdf);
		const CMatrixDouble66& cov = gPdf.cov;

		m_fastdraw_gauss_M_3D = gPdf.mean;

		std::vector<double> eigVals;
		cov.eig_symmetric(m_fastdraw_gauss_Z6, eigVals);

		// Scale eigenvectors with eigenvalues:
		mrpt::math::CMatrixDouble66 D;
		D.setDiagonal(eigVals);

		// Scale eigenvectors with eigenvalues:
		D = D.asEigen().array().sqrt().matrix();
		m_fastdraw_gauss_Z6.matProductOf_AB(m_fastdraw_gauss_Z6, D);
	}
	else if (IS_CLASS(pdf, CPose3DPDFParticles))
	{
		return;	 // Nothing to prepare.
	}
	else
	{
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
	}

	MRPT_END
}

/*---------------------------------------------------------------
					drawSample
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::drawSample(CPose2D& p) const
{
	MRPT_START

	if (m_pdf2D) { do_sample_2D(p); }
	else if (m_pdf3D)
	{
		CPose3D q;
		do_sample_3D(q);
		p.x(q.x());
		p.y(q.y());
		p.phi(q.yaw());
	}
	else
		THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

	return p;
	MRPT_END
}

/*---------------------------------------------------------------
					drawSample
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::drawSample(CPose3D& p) const
{
	MRPT_START

	if (m_pdf2D)
	{
		CPose2D q;
		do_sample_2D(q);
		p.setFromValues(q.x(), q.y(), 0, q.phi(), 0, 0);
	}
	else if (m_pdf3D)
	{
		do_sample_3D(p);
	}
	else
		THROW_EXCEPTION("No associated pdf: setPosePDF must be called first.");

	return p;
	MRPT_END
}

/*---------------------------------------------------------------
				  do_sample_2D: Sample from a 2D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_2D(CPose2D& p) const
{
	MRPT_START
	ASSERT_(m_pdf2D);

	// According to the PDF type:
	if (IS_CLASS(*m_pdf2D, CPosePDFGaussian))
	{
		// ------------------------------
		//      A single gaussian:
		// ------------------------------
		CVectorDouble rndVector(3);
		rndVector.setZero();
		for (size_t i = 0; i < 3; i++)
		{
			double rnd = getRandomGenerator().drawGaussian1D_normalized();
			for (size_t d = 0; d < 3; d++)
				rndVector[d] += (m_fastdraw_gauss_Z3(d, i) * rnd);
		}

		p.x(m_fastdraw_gauss_M_2D.x() + rndVector[0]);
		p.y(m_fastdraw_gauss_M_2D.y() + rndVector[1]);
		p.phi(m_fastdraw_gauss_M_2D.phi() + rndVector[2]);
		p.normalizePhi();
	}
	else if (IS_CLASS(*m_pdf2D, CPosePDFSOG))
	{
		// -------------------------------------
		//      			SOG
		// -------------------------------------
		THROW_EXCEPTION("TODO");
	}
	else if (IS_CLASS(*m_pdf2D, CPosePDFParticles))
	{
		// -------------------------------------
		//      Particles: just sample as usual
		// -------------------------------------
		const auto& pdf = dynamic_cast<const CPosePDFParticles&>(*m_pdf2D);
		pdf.drawSingleSample(p);
	}
	else
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf2D->GetRuntimeClass()->className);

	MRPT_END
}

/*---------------------------------------------------------------
				  do_sample_3D: Sample from a 3D PDF
  ---------------------------------------------------------------*/
void CPoseRandomSampler::do_sample_3D(CPose3D& p) const
{
	MRPT_START
	ASSERT_(m_pdf3D);

	// According to the PDF type:
	if (IS_CLASS(*m_pdf3D, CPose3DPDFGaussian))
	{
		// ------------------------------
		//      A single gaussian:
		// ------------------------------
		CVectorDouble rndVector(6);
		rndVector.setZero();
		for (size_t i = 0; i < 6; i++)
		{
			double rnd = getRandomGenerator().drawGaussian1D_normalized();
			for (size_t d = 0; d < 6; d++)
				rndVector[d] += (m_fastdraw_gauss_Z6(d, i) * rnd);
		}

		p.setFromValues(
			m_fastdraw_gauss_M_3D.x() + rndVector[0],
			m_fastdraw_gauss_M_3D.y() + rndVector[1],
			m_fastdraw_gauss_M_3D.z() + rndVector[2],
			m_fastdraw_gauss_M_3D.yaw() + rndVector[3],
			m_fastdraw_gauss_M_3D.pitch() + rndVector[4],
			m_fastdraw_gauss_M_3D.roll() + rndVector[5]);
	}
	else if (IS_CLASS(*m_pdf3D, CPose3DPDFSOG))
	{
		// -------------------------------------
		//      			SOG
		// -------------------------------------
		THROW_EXCEPTION("TODO");
	}
	else if (IS_CLASS(*m_pdf3D, CPose3DPDFParticles))
	{
		// -------------------------------------
		//      Particles: just sample as usual
		// -------------------------------------
		const auto& pdf = dynamic_cast<const CPose3DPDFParticles&>(*m_pdf3D);
		pdf.drawSingleSample(p);
	}
	else
		THROW_EXCEPTION_FMT(
			"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);

	MRPT_END
}

/*---------------------------------------------------------------
				  isPrepared
  ---------------------------------------------------------------*/
bool CPoseRandomSampler::isPrepared() const { return m_pdf2D || m_pdf3D; }
/*---------------------------------------------------------------
				  getOriginalPDFCov2D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov2D(CMatrixDouble33& cov3x3) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D) { m_pdf2D->getCovariance(cov3x3); }
	else
	{
		ASSERT_(m_pdf3D);

		CPosePDFGaussian P;
		P.copyFrom(*m_pdf3D);
		cov3x3 = P.cov;
	}

	MRPT_END
}

/*---------------------------------------------------------------
				  getOriginalPDFCov3D
  ---------------------------------------------------------------*/
void CPoseRandomSampler::getOriginalPDFCov3D(CMatrixDouble66& cov6x6) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D)
	{
		CPose3DPDFGaussian P;
		P.copyFrom(*m_pdf2D);
		cov6x6 = P.cov;
	}
	else
	{
		ASSERT_(m_pdf3D);
		m_pdf3D->getCovariance(cov6x6);
	}

	MRPT_END
}

/*---------------------------------------------------------------
				  getSamplingMean2D
  ---------------------------------------------------------------*/
CPose2D& CPoseRandomSampler::getSamplingMean2D(CPose2D& out_mean) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf2D) out_mean = m_fastdraw_gauss_M_2D;
	else
		out_mean = CPose2D(m_fastdraw_gauss_M_3D);

	return out_mean;
	MRPT_END
}

/*---------------------------------------------------------------
				  getSamplingMean3D
  ---------------------------------------------------------------*/
CPose3D& CPoseRandomSampler::getSamplingMean3D(CPose3D& out_mean) const
{
	MRPT_START
	ASSERT_(this->isPrepared());

	if (m_pdf3D) out_mean = m_fastdraw_gauss_M_3D;
	else
		out_mean = CPose3D(m_fastdraw_gauss_M_2D);

	return out_mean;
	MRPT_END
}

void CPoseRandomSampler::getOriginalPDFCov2D(
	mrpt::math::CMatrixDouble& cov3x3) const
{
	mrpt::math::CMatrixDouble33 M;
	this->getOriginalPDFCov2D(M);
	cov3x3 = mrpt::math::CMatrixDouble(M);
}

void CPoseRandomSampler::getOriginalPDFCov3D(
	mrpt::math::CMatrixDouble& cov6x6) const
{
	mrpt::math::CMatrixDouble66 M;
	this->getOriginalPDFCov3D(M);
	cov6x6 = M;
}


/**
 * @brief AVL木
 * @note  AVL木(AVL tree)は高さ平衡(height
 * balanced)2分探索木である.すなわち、各節点xの左右の部分木の高さは高々1しか違わない
 *        AVL木を実現するために各節点に特別な属性を管理する. x.hはxの高さである
 *        他の任意の2分探索木Tと同様、T.rootはその根を指すものと仮定する
 *
 * @note
 * Fhをh番目のフィボナッチ数とするとき、高さhのAVL木には少なくともFh個の節点があることから
 *        n個のAVL木の高さはΟ(lgn)であることがわかる
 *
 * @note  今回は属性pを省略してコードの簡略化を図ることにする
 */

#ifndef AVL_TREE_HPP
#define AVL_TREE_HPP

#include "container.hpp"
#include <algorithm>
#include <boost/assert.hpp>
#include <boost/container/pmr/polymorphic_allocator.hpp>
#include <memory>
#include <optional>
#include <utility>

namespace container {

template <class Key, class T> struct avl_tree_node {
  using height_t = std::int32_t;
  union {
    struct {
      avl_tree_node *l; /**< 左の子    */
      avl_tree_node *r; /**< 右の子    */
    };
    avl_tree_node *c[2]; /**< 左右の子(0:左, 1:右) */
  };
  height_t h; /**< 高さ */
  Key key;    /**< キー    */
  T v;        /**< 付属データ */

  constexpr explicit avl_tree_node(const Key &k, const T &v) noexcept
      : l(nullptr), r(nullptr), h(1), key(k), v(v) {}
};

/**
 * @brief  AVL木
 * @tparam Key     キーの型
 * @tparam T       付属データの型
 * @tparam Compare キーを引数にとる比較述語の型
 */
template <class Key, class T, class Compare = std::less<Key>,
          class Allocator = boost::container::pmr::polymorphic_allocator<
              avl_tree_node<Key, T>>>
struct avl_tree {
  static_assert(std::is_nothrow_constructible_v<Key> &&
                std::is_nothrow_constructible_v<T>);
  using alloc = std::allocator_traits<Allocator>;
  using sides_t = std::int32_t;
  using pair_t = std::pair<const Key, T>;
  using node = avl_tree_node<Key, T>;
  using height_t = typename node::height_t;

  explicit avl_tree(std::size_t n = 32) { allocate_pool(n); }
  ~avl_tree() noexcept { free_pool(); } // 確保した記憶領域の解放

  /**
   * @brief  AVL木からキーkに対応する付属データを返す
   * @note   実行時間はΟ(lgn)
   * @param  const Key& k キーk
   * @return キーkに対応する付属データ
   */
  std::optional<T> find(const Key &k) const {
    const node *x = find(root_, k);
    return x == nullptr ? std::nullopt : std::make_optional(x->v);
  }

  /**
   * @brief AVL木Tにキーkの挿入を行う
   * @note  実行時間はΟ(lgn)
   * @param const Key& k キーk
   * @param const T& v   付属データv
   * @return キーkに対応していた付属データ
   */
  std::optional<T> insert(const Key &k, const T &v) {
    std::optional<T> opt = std::nullopt;
    root_ = insert(root_, k, v, opt);
    return opt;
  }

  /**
   * @brief AVL木Tからキーkを持つ節点の削除を行う
   * @note  実行時間はΟ(lgn)
   * @param const Key&git  k キーk
   * @return キーkに対応していた付属データ
   */
  std::optional<T> erase(const Key &k) {
    std::optional<T> opt = std::nullopt;
    root_ = erase(root_, k, opt);
    return opt;
  }

  /**
   * @brief  中間順木巡回を行う
   * @note   n個の節点を持つ2分探索木の巡回はΘ(n)かかる
   * @tparam class F const Key&を引数に取る関数オブジェクトの型
   * @param F fn     const T&を引数に取る関数オブジェクト
   */
  template <class F> void inorder(F fn) { inorder(root_, fn); }

private:
  /**
   * @brief  中間順木巡回を行う
   * @note   n個の節点を持つ2分探索木の巡回はΘ(n)かかる
   * @tparam class F const Key&を引数に取る関数の型
   * @param F fn     const Key&を引数に取る関数
   * @param node*x   巡回を行う部分木の根
   */
  template <class F> void inorder(node *x, F fn) {
    if (x == nullptr) {
      return;
    } // xがNILを指すとき、再帰は底をつく
    inorder(x->l, fn);
    fn(x->key, x->v);
    inorder(x->r,
            fn); // xの左右の子を根とする部分木に対して中間順木巡回を行う
  }

  /**
   * @brief  AVL木からキーkに対応する付属データを返す
   * @note   実行時間はΟ(lgn)
   * @param  const Key& k キーk
   * @return キーkに対応する付属データへのポインタ
   */
  node *find(node *x, const Key &k) const {
    if (x == nullptr || eq(x->key, k)) {
      return x;
    } else {
      return find(cmp_(k, x->key) ? x->l : x->r, k);
    }
  }

  /**
   * @brief 節点xを根とする部分木にキーkの挿入を行う
   * @param node*x       節点x
   * @param node*z       キーkを持つ節点z
   */
  node *insert(node *x, const Key &k, const T &v, std::optional<T> &opt) {
    if (x == nullptr) {
      return create_node(k, v);
    }                      // xがNILを指すとき、再帰は底をつく
    if (cmp_(k, x->key)) { // xの適切な子に再帰し、
      x->l = insert(x->l, k, v, opt);
    } else if (cmp_(x->key, k)) {
      x->r = insert(x->r, k, v, opt);
    } else {
      opt = x->v;
      x->v = v;
      return x;
    }
    return balance(x); // xを根とする部分木を高さ平衡にする
  }

  /**
   * @brief 節点xを根とする部分木からキーkを削除する
   * @param node*x       節点x
   * @param const Key& k キーk
   */
  node *erase(node *x, const Key &k, std::optional<T> &opt) {
    if (x == nullptr) {
      return nullptr;
    } // キーkはAVL木Tに存在しなかった
    if (cmp_(k, x->key)) {
      x->l = erase(x->l, k, opt);
      return balance(x);
    } // xの適切な子に再帰し、
    if (cmp_(x->key, k)) {
      x->r = erase(x->r, k, opt);
      return balance(x);
    } // xを根とする部分木を高さ平衡にする
    opt = x->v;
    node *y = x->l,
         *z = x->r; // x.key == kのとき、yをxの左の子、zをxの右の子とし、
    destroy_node(x); // xを解放する
    if (z == nullptr) {
      return y;
    } // zがNILを指しているならば、新たな部分木の根としてyを返す
    node *w = leftmost(
        z); // zがNILを指していないならば、wをzを根とする部分木の中で最も左の子とする
    w->r = erase__(
        z); // zに対して再帰する.そして、wの右の子に新たな部分木を受け取る
    w->l = y;          // yをwの左の子にする
    return balance(w); // wを根とする部分木を高さ平衡にして戻る
  }

  /**
   * @brief
   * 節点xの左右の部分木はそれぞれ高さ平衡しており、その左右の子の高さの差は2以下であるとする
   *         xを入力とし、xを根とする部分木を高さ平衡になるように変換する
   * @note   回転は高々2回であるため実行時間はΟ(1)
   * @param  node*x 節点x
   * @return 高さ平衡な部分木の根
   */
  static node *balance(node *x) {
    x->h = reheight(x); // xの高さを更新する
    if (bias(x) > 1) {  // 左に2つ分偏っている場合、left-l
                        // caseおよびleft-r caseが考えられる
      if (bias(x->l) < 0) {
        x->l = left_rotate(x->l);
      } // l-r caseならば、左回転を行うことで、left-l caseに帰着させる
      return right_rotate(x); // 右回転を行うことでleft-l
                              // caseを解消し、高さ平衡を満たす部分木の根を返す
    }
    if (bias(x) < -1) { // 右に2つ分偏っている場合、right-r
                        // caseおよびright-l caseが考えられる
      if (bias(x->r) > 0) {
        x->r = right_rotate(x->r);
      } // r-l caseならば、右回転を行うことで、right-r
        // caseに帰着させる
      return left_rotate(x); // 左回転を行うことでright-r
                             // caseを解消し、高さ平衡を満たす部分木の根を返す
    }
    return x; // 高さ平衡の場合、xを返す
  }
  /**
   * @brief
   * 節点xからy(xのjの子)へのリンクを"ピボット"とするi回転で、回転の結果、
   *         yが部分木の新しい根となり、xがyのiの子、yのiの子がxのjの子になる
   * @note   x.c[j] != NILを仮定している. 実行時間はΟ(1)
   * @param  node*   x  節点x
   * @param  sides_t i  添字i(0のとき左、1のとき右)
   * @param  sides_t j  添字j(0のとき左、1のとき右)
   */
  static node *rotate(node *x, sides_t i, sides_t j) {
    node *y = x->c[j];  // yをxのjの子とする
    x->c[j] = y->c[i];  // yのi部分木をxのj部分木にする
    y->c[i] = x;        // xをyのiの子にする
    x->h = reheight(x); // xの高さを更新する
    y->h = reheight(y); // yの高さを更新する
    return y;           // 部分木の新しい根yを返す
  }
  static node *left_rotate(node *x) { return rotate(x, 0, 1); }
  static node *right_rotate(node *x) { return rotate(x, 1, 0); }

  /**
   * @brief 節点xを根とする部分木の中から最も左にある子を取得する
   * @param node*x 節点x
   */
  static node *leftmost(node *x) { return x->l ? leftmost(x->l) : x; }

  /**
   * @brief xを根とする部分木の最も左の子を削除する補助関数
   * @param node*x 節点x
   */
  static node *erase__(node *x) {
    if (x->l == nullptr) {
      return x->r;
    }
    x->l = erase__(x->l);
    return balance(x);
  }

private:
  /**< @brief 節点xの高さを取得する  */
  static constexpr height_t height(node *x) noexcept { return x ? x->h : 0; }
  /**< @brief 節点xの更新される高さを返す */
  static constexpr height_t reheight(node *x) noexcept {
    return std::max(height(x->l), height(x->r)) + 1;
  }
  /**< @brief 節点xの左右の子の高さの差(x.l - x.r)を返す */
  static constexpr height_t bias(node *x) noexcept {
    return height(x->l) - height(x->r);
  }

private:
  /**< @brief 節点xの記憶領域の確保を行う */
  node *create_node(const Key &k, const T &v) {
    BOOST_ASSERT_MSG(size_ < cap_, "AVL tree capacity over.");
    node *x = pool_ + size_;
    alloc::construct(alloc_, x, k, v);
    size_++;
    return x;
  }

  /**< @brief 節点xの記憶領域の解放を行う */
  void destroy_node(node *x) noexcept {
    alloc::destroy(alloc_, x);
    size_--;
  }

  /**< @brief 節点xを根とした部分木を再帰的に解放する*/
  void postorder_destroy_nodes(node *x) noexcept {
    if (x == nullptr) {
      return;
    }
    postorder_destroy_nodes(x->l);
    postorder_destroy_nodes(x->r);
    destroy_node(x);
  }

  /**< @brief メモリプールの解放 */
  void free_pool() noexcept {
    postorder_destroy_nodes(root_);
    alloc::deallocate(alloc_, pool_, cap_);
    root_ = pool_ = nullptr;
    size_ = cap_ = 0;
  }

  /**< @brief メモリプールの確保 */
  void allocate_pool(std::size_t n) {
    pool_ = alloc::allocate(alloc_, n);
    cap_ = n;
  }

private:
  /**< ＠brief キーlとキーrの非同値判定を行う */
  inline bool neq(const Key &l, const Key &r) const {
    return (cmp_(l, r) || cmp_(r, l));
  }
  /**< ＠brief キーlとキーrの同値判定を行う */
  inline bool eq(const Key &l, const Key &r) const { return !neq(l, r); }

private:
  node *root_ = nullptr; /**< AVL木の根 */
  std::size_t cap_ = 0;  /**< AVL木のバッファサイズ    */
  std::size_t size_ = 0; /**< AVL木のサイズ           */
  node *pool_ = nullptr; /**< AVL木の節点用メモリプール */
  Compare cmp_;          /**< 比較述語  */
  Allocator alloc_;      /**< アロケータ */
};

} // namespace container

#endif // end of AVL_TREE_HPP


#pragma once

#include <cassert>
#include <list>
#include <unordered_map>
#include <vector>

#include <boost/heap/fibonacci_heap.hpp>

namespace hypergraphlib {

/* A collection of distinct values, ordered by their keys. The key for every
 * value is initially zero.
 *
 * Supports incrementing the key of a value in constant time as well as removing
 * one of the values with maximum key in time linear to the number of buckets.
 *
 * Useful for computing vertex orderings for unweighted hypergraphs. See [M'05]
 * for more details.
 */
class BucketHeap {
public:
  /* Create a bucket list with the given values and capacity (number of
   * buckets). The values should be unique.
   *
   * Time complexity: O(n), where n in the number of values.
   */
  BucketHeap(std::vector<int> values, size_t capacity);

  /* Increment the key of the value.
   *
   * Time complexity: O(1).
   */
  void increment(int value);

  /* Pop an arbitrary value with a maximum key.
   *
   * Time complexity: Expected is O(1), worst-case is O(b), where b is the
   * number of buckets.
   */
  int pop();

  /* Pop a (key, value) pair with a maximum key.
   *
   * Time complexity: Expected is O(1), worst-case is O(b), where b is the
   * number of buckets.
   */
  std::pair<size_t, int> pop_key_val();

private:
  const size_t capacity_;

  // buckets_[i] is a collection of all values with key = i
  std::vector<std::list<int>> buckets_;

  // A mapping of values to their keys
  std::unordered_map<int, size_t> val_to_keys_;

  // A mapping of values to their iterators (for fast deletion)
  std::unordered_map<int, std::list<int>::iterator> val_to_its_;

  // The current maximum key
  size_t max_key_;
};

/* Wrapper of boost::fibonacci_heap that conforms to the BucketHeap interface
 */
template<typename EdgeWeightType>
class FibonacciHeap {
public:
  FibonacciHeap(const std::vector<int> &values, [[maybe_unused]] size_t capacity) {
    for (const int v : values) {
      auto handle = heap_.push({0, v});
      const auto[it, inserted] = handles_.insert({v, handle});
      assert(inserted);
    }
  }

  /* Increment the key of the value.
   *
   * Time complexity: constant
   */
  void increment(const int value, const EdgeWeightType amount) {
    auto handle = handles_.at(value);
    auto[key, val] = *handle;
    *handle = {key + amount, val};
    heap_.increase(handle);
  }

  /* Pop an arbitrary value with a maximum key.
   *
   * Time complexity: constant
   */
  int pop() {
    return pop_key_val().second;
  }

  std::pair<EdgeWeightType, int> pop_key_val() {
    const auto pair = heap_.top();
    heap_.pop();
    return pair;
  }

private:
  // Elements are ordered by their edge weight, and keep a reference to the vertex ID
  using element_t = std::pair<EdgeWeightType, int>;
  using handle_t = typename boost::heap::fibonacci_heap<element_t>::handle_type;

  std::unordered_map<int, handle_t> handles_;

  boost::heap::fibonacci_heap<element_t> heap_;
};

}


/*
Copyright (c) 2015-2019, Florian Sittel (www.lettis.net) and Daniel Nagel
All rights reserved.

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.

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.
*/
#pragma once

#include <vector>
#include <map>
#include <set>
#include <stdexcept>
#include <string>

#include <boost/program_options.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "tools.hpp"
/*! \file
 * \brief Most Probable Path Clustering
 *
 * \sa \link Clustering::MPP
 */

namespace Clustering {
  /*!
   * \brief functions related to "Most Probable Path"-clustering
   *
   * This module contains all function for dynamical clustering. In contrast to
   * density-based clustering, can it only be applied to previously clustered
   * trajectories. The idea is to create, based on a microstate input, a
   * coarse-grained model (macrostates).
   * The most probable path depends strongly on the selected timescale (mpp
   * time). If the input was dynamically cored, the mpp time needs to be
   * greater than the coring time.
   */
  namespace MPP {
    //! BOOST implementation of a sparse matrix for floats
    using SparseMatrixF = boost::numeric::ublas::mapped_matrix<float>;
    //! Neighborhood per frame
    using Neighborhood = Clustering::Tools::Neighborhood;
    //! read (row-normalized) transition matrix from file
    SparseMatrixF
    read_transition_probabilities(std::string fname);
    //! count transitions from one to the other cluster with certain lag
    //! and return as count matrix (row/col := from/to)
    SparseMatrixF
    transition_counts(std::vector<std::size_t> trajectory
                    , std::vector<std::size_t> concat_limits
                    , std::size_t n_lag_steps
                    , std::size_t i_max = 0);
    //! same as 'transition_counts', but with reweighting account for
    //! differently sized trajectory chunks (as given by concat_limits)
    SparseMatrixF
    weighted_transition_counts(std::vector<std::size_t> trajectory
                             , std::vector<std::size_t> concat_limits
                             , std::size_t n_lag_steps);
    //! compute transition matrix from counts by normalization of rows
    SparseMatrixF
    row_normalized_transition_probabilities(SparseMatrixF count_matrix
                                          , std::set<std::size_t> microstate_names);
    //! update transition matrix after lumping states into sinks
    SparseMatrixF
    updated_transition_probabilities(SparseMatrixF transition_matrix
                                   , std::map<std::size_t, std::size_t> sinks
                                   , std::map<std::size_t, std::size_t> pops);
    //! compute immediate future (i.e. without lag) of every state from highest probable transitions;
    //! exclude self-transitions.
    std::map<std::size_t, std::size_t>
    single_step_future_state(SparseMatrixF transition_matrix,
                             std::set<std::size_t> cluster_names,
                             float q_min,
                             std::map<std::size_t, float> min_free_energy);
    //! for every state, compute most probable path by following
    //! the 'future_state'-mapping recursively
    std::map<std::size_t, std::vector<std::size_t>>
    most_probable_path(std::map<std::size_t, std::size_t> future_state, std::set<std::size_t> cluster_names);
    //! compute cluster populations
    std::map<std::size_t, std::size_t>
    microstate_populations(std::vector<std::size_t> clusters, std::set<std::size_t> cluster_names);
    //! assign every state the lowest free energy value
    //! of all of its frames.
    std::map<std::size_t, float>
    microstate_min_free_energy(const std::vector<std::size_t>& clustering,
                               const std::vector<float>& free_energy);
    //! compute path sinks, i.e. states of highest metastability,
    //! and lowest free energy per path. these sinks will be states all other
    //! states of the given path will be lumped into.
    std::map<std::size_t, std::size_t>
    path_sinks(std::vector<std::size_t> clusters,
               std::map<std::size_t, std::vector<std::size_t>> mpp,
               SparseMatrixF transition_matrix,
               std::set<std::size_t> cluster_names,
               float q_min,
               std::vector<float> free_energy);
    //! lump states based on path sinks and return new trajectory.
    //! new microstates will have IDs of sinks.
    std::vector<std::size_t>
    lumped_trajectory(std::vector<std::size_t> trajectory,
                      std::map<std::size_t, std::size_t> sinks);
    //! run clustering for given Q_min value
    std::tuple<std::vector<std::size_t>
             , std::map<std::size_t, std::size_t>
             , SparseMatrixF>
    fixed_metastability_clustering(std::vector<std::size_t> initial_trajectory,
                                   SparseMatrixF trans_prob,
                                   float q_min,
                                   std::vector<float> free_energy);
    /*!
     * \brief MPP clustering control function and user interface
     *
     * \param input input file with microstate trajectory
     * \param basename name format for output files
     * \param lagtime lag for transition estimation in units of frame numbers
     * \param qmin-from lower limit for metastability (Q_min)
     * \param qmin-to upper limit for metastability (Q_min)
     * \param qmin-step stepping for metastability (Q_min)
     * \param concat-nframes no. of frames per trajectory.
     * \param concat-limits length of concated trajectories.
     * \return void
     * \note Lagtime should be greater than the coring time/ smallest
     *       timescale.
     */
    void
    main(boost::program_options::variables_map args);
  } // end namespace Clustering::MPP
} // end namespace Clustering


#include "OT/Base/PvwBaseOT.h"
#include "OT/Base/Math/DMC.h"
#include "Crypto/PRNG.h"
#include "Crypto/sha1.h"
#include "Network/Channel.h"
#include "cryptopp/osrng.h"
#include "Common/Log.h"

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <thread>
#include <mutex>

//#include <boost/thread/tss.hpp>
//void Cleanup_Miracl(Miracl* miracl)
//{
//	if (miracl)
//	{
//		bOPRF::Log::out << "deleting Miracle" << bOPRF::Log::endl;
//		delete miracl;
//	}
//}

using namespace std;
namespace bOPRF {

	const char* role_to_str(OTRole role)
	{
		if (role == Receiver)
			return "Receiver";
		if (role == Sender)
			return "Sender";
		return "Both";
	}

	OTRole INV_ROLE(OTRole role)
	{
		if (role == Receiver)
			return Sender;
		if (role == Sender)
			return Receiver;
		else
			return Both;
	}

	void send_if_ot_sender(Channel& channel, vector<ByteStream>& os, OTRole role)
	{

		if (role == Sender)
		{
			channel.asyncSendCopy(os[0].data(), os[0].size());
			//os[0].Send(channel);
		}
		else if (role == Receiver)
		{

			channel.recv(os[1]);
			//os[1].setp(0);
			//os[1].Receive(channel);
		}
		else
		{
			// both sender + receiver

			channel.asyncSendCopy(os[0]);
			channel.recv(os[1]);
			//os[0].Send(channel);
			//os[1].setp(0);
			//os[1].Receive(channel);
			//P->send_receive_player(os);
		}
	}

	void send_if_ot_receiver(Channel& channel, vector<ByteStream>& os, OTRole role)
	{
		if (role == Receiver)
		{
			channel.asyncSendCopy(os[0]);
		}
		else if (role == Sender)
		{
			channel.recv(os[1]);
			//P->receive(os[1]);
		}
		else
		{
			// both
			channel.asyncSendCopy(os[0]);
			channel.recv(os[1]);
			//P->send_receive_player(os);
		}
	}

	/*
	 * pack/unpack routines for Miracl data types
	 */

	void pack(ByteStream& s, const Big& z)
	{
		u8 data[50];
		i32 len = to_binary(z, 50, (char*)data, false);
		s.append((u8*)&len, sizeof(u32));
		s.append(data, len);
	}

	void unpack(Big& z, ByteStream& s)
	{
		u8 data[50];
		i32 len;
		s.consume((u8*)&len, sizeof(i32));
		s.consume(data, len);
		z = from_binary(len, (char*)data);
	}

	void pack(ByteStream& s, const ECn& P)
	{
		Big x, y;
		P.get(x, y);
		pack(s, x);
		pack(s, y);
	}

	void unpack(ECn& P, ByteStream& s)
	{
		Big x, y;
		unpack(x, s);
		unpack(y, s);
		P = ECn(x, y);
	}

	//#define BASEOT_DEBUG

		// Run the PVW OTs
	void Exec_OT(vector< vector<ECn> >& Miracl_Sender_Inputs,
		BitVector& OT_Receiver_Inputs,
		vector<ECn>& Miracl_Receiver_Outputs,
		const CRS& crs,
		csprng& Miracl_RNG,
		PRNG& G,
		Channel& channel,
		OTRole role)
	{
		u64 n = OT_Receiver_Inputs.size();
		//Log::out << Log::PvwBaseOT << "Starting base OTs as " << role_to_str(role) << ", n = " << n << Log::endl;
		vector<ByteStream> strm(2);

		Big z;
		vector<SK> sk(n);
		PK pk;
		if (role & Receiver)
		{
			// Generate my receiver inputs
			OT_Receiver_Inputs.randomize(G);
			for (u64 i = 0; i < n; i++)
			{
				// Generate public keys on my branch
				KeyGen(sk[i], pk, OT_Receiver_Inputs[i], crs, Miracl_RNG);

				pack(strm[0], pk.g);
				pack(strm[0], pk.h);
			}
		}
		strm[1].setp(0);

		// Send receiver's public keys over
		send_if_ot_receiver(channel, strm, role);

		ECn c0, c1;

		if (role & Sender)
		{
			strm[0].setp(0);
			for (u64 i = 0; i < n; i++)
			{
				// Generate sender inputs
				z = strong_rand(&Miracl_RNG, crs.bit_size(), 2);
				Miracl_Sender_Inputs[i][0] = z*crs.get_g(0);
				z = strong_rand(&Miracl_RNG, crs.bit_size(), 2);
				Miracl_Sender_Inputs[i][1] = z*crs.get_g(0);

				// Unpack public keys, encrypt my two messages on the correct
				// branch and send them back
				unpack(pk.g, strm[1]);
				unpack(pk.h, strm[1]);

				pk.Encrypt(c0, c1, Miracl_Sender_Inputs[i][0], 0, crs, Miracl_RNG);
				pack(strm[0], c0);
				pack(strm[0], c1);
#ifdef BASEOT_DEBUG
				//Log::out << "m[" << i << ", 0] = " << Miracl_Sender_Inputs[i][0] << Log::endl;
				pack(strm[0], Miracl_Sender_Inputs[i][0]);
#endif
				pk.Encrypt(c0, c1, Miracl_Sender_Inputs[i][1], 1, crs, Miracl_RNG);
				pack(strm[0], c0);
				pack(strm[0], c1);
#ifdef BASEOT_DEBUG
				//Log::out << "m[" << i << ", 1] = " << Miracl_Sender_Inputs[i][1] << Log::endl;
				pack(strm[0], Miracl_Sender_Inputs[i][1]);
#endif
			}
		}

		// Sender sends ciphertexts over
		strm[1].setp(0);
		send_if_ot_sender(channel, strm, role);

#ifdef BASEOT_DEBUG
		ECn m0, m1;
#endif

		if (role & Receiver)
		{
			// Now unpack the received ciphertexts, decrypt the one we want
			// and store it
			for (u64 i = 0; i < n; i++)
			{
				unpack(c0, strm[1]);
				unpack(c1, strm[1]);
#ifdef BASEOT_DEBUG
				unpack(m0, strm[1]);
#endif
				if (OT_Receiver_Inputs[i] == 0)
				{
					sk[i].Decrypt(Miracl_Receiver_Outputs[i], c0, c1);
#ifdef BASEOT_DEBUG
					if (Miracl_Receiver_Outputs[i] != m0)
					{
						Log::out << "Bad PvwBaseOT message received" << Log::endl;
						Log::out << "Received   " << Miracl_Receiver_Outputs[i] << Log::endl;
						Log::out << "wanted m0  " << m0 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}
#endif
				}
				unpack(c0, strm[1]);
				unpack(c1, strm[1]);
#ifdef BASEOT_DEBUG
				unpack(m1, strm[1]);
#endif
				if (OT_Receiver_Inputs[i] == 1)
				{
					sk[i].Decrypt(Miracl_Receiver_Outputs[i], c0, c1);
#ifdef BASEOT_DEBUG
					if (Miracl_Receiver_Outputs[i] != m1)
					{
						Log::out << "Bad PvwBaseOT message " << i << " received" << Log::endl;
						Log::out << "Received   " << Miracl_Receiver_Outputs[i] << Log::endl;
						Log::out << "wanted m1  " << m1 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}
#endif
				}

				//Log::out << Log::PvwBaseOT << "m"<< OT_Receiver_Inputs.get_bit(i) <<"[" << i << "] = " << Miracl_Receiver_Outputs[i] << Log::endl;

			}
		}
		//Log::out << Log::PvwBaseOT << "Exit base OT" << Log::endl;
	}

	void PvwBaseOT::exec_base(PRNG& G)
	{
		// Set up crs
	  CRS crs(&(*GetPrecision()));
		//Log::out << "check 0" << Log::endl;

		// Initialize a secure random number generator for Miracl
		csprng Miracl_RNG;
		u8 data[100];
		//Log::out << "check 1" << Log::endl;
		G.get_u8s(data, 100);
		//CryptoPP::OS_GenerateRandomBlock(false, data, sizeof(u8) * 100);

		strong_init(&Miracl_RNG, 100, (char*)data, 0L);
		///Log::out << "check 2" << Log::endl;

		vector< vector<ECn> > Miracl_Sender_Inputs(nOT, vector<ECn>(2));
		vector<ECn>           Miracl_Receiver_Outputs(nOT);

		Exec_OT(Miracl_Sender_Inputs, receiver_inputs, Miracl_Receiver_Outputs, crs, Miracl_RNG, G, mChannel, mOTRole);

		ByteStream s;
		u8 buff[SHA1::HashSize];
		//CBC_MAC cbc;
		SHA1 sha;
		// Hash PVW output into byte strings
		for (int i = 0; i < nOT; i++)
		{
			if (mOTRole & Sender)
			{
				s.setp(0);
				pack(s, Miracl_Sender_Inputs[i][0]);

				sha.Reset();
				sha.Update(s.data(), s.size());
				sha.Final(buff);
				sender_inputs[i][0] = *(block*)buff;

				//cbc.zero_key();
				//cbc.Update(s);
				//cbc.Finalize(sender_inputs[i][0]);
#ifdef BASEOT_DEBUG
				s.setp(0);
				pack(s, Miracl_Sender_Inputs[i][0]);
				Log::out << Log::PvwBaseOT << "m[" << i << ", 0] s = " << s << Log::endl;
				mChannel.Send(s);
				s.setp(0);
				s.append(sender_inputs[i][0]);
				mChannel.AsyncSendCopy(s);
				Log::out << Log::PvwBaseOT << "m[" << i << ", 0]   = " << sender_inputs[i][0] << Log::endl;
#endif
				s.setp(0);
				pack(s, Miracl_Sender_Inputs[i][1]);
				sha.Reset();
				sha.Update(s.data(), s.size());
				sha.Final(buff);

				//cbc.zero_key();
				//cbc.Update(s);
				//cbc.Finalize(sender_inputs[i][1]);
				sender_inputs[i][1] = *(block*)buff;
#ifdef BASEOT_DEBUG
				s.setp(0);
				pack(s, Miracl_Sender_Inputs[i][1]);
				mChannel.Send(s);
				Log::out << Log::PvwBaseOT << "m[" << i << ", 1] s = " << s << Log::endl;

				s.setp(0);
				s.append(sender_inputs[i][1]);
				mChannel.AsyncSendCopy(s);
				Log::out << Log::PvwBaseOT << "m[" << i << ", 1]   = " << sender_inputs[i][1] << Log::endl;
#endif
			}
			if (mOTRole & Receiver)
			{
				s.setp(0);
				pack(s, Miracl_Receiver_Outputs[i]);
				sha.Reset();
				sha.Update(s.data(), s.size());
				sha.Final(buff);

				receiver_outputs[i] = *(block*)buff;

				//cbc.zero_key();
				//cbc.Update(s);
				//cbc.Finalize(receiver_outputs[i]);

#ifdef BASEOT_DEBUG
				Log::out << Log::PvwBaseOT << "s " << s << Log::endl;
				ECn ecm0, ecm1;
				block m0, m1;

				s.setp(0);
				mChannel.recv(s);
				unpack(ecm0, s);
				s.setp(0);
				mChannel.recv(s);
				s.consume(m0);

				s.setp(0);
				mChannel.recv(s);
				unpack(ecm1, s);
				s.setp(0);
				mChannel.recv(s);
				s.consume(m1);

				if (receiver_inputs[i])
				{
					if (Miracl_Receiver_Outputs[i] != ecm1)
					{
						Log::out << "Bad PvwBaseOT Miracl message " << i << " received" << Log::endl;
						Log::out << "Received   " << Miracl_Receiver_Outputs[i] << Log::endl;
						Log::out << "wanted m1  " << ecm1 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}

					if (receiver_outputs[i] != m1)
					{
						Log::out << "Bad PvwBaseOT message " << i << " received" << Log::endl;
						Log::out << "Received   " << receiver_outputs[i] << Log::endl;
						Log::out << "wanted m1  " << m1 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}
				}
				else
				{

					if (Miracl_Receiver_Outputs[i] != ecm0)
					{
						Log::out << "Bad PvwBaseOT Miracl message " << i << " received" << Log::endl;
						Log::out << "Received   " << Miracl_Receiver_Outputs[i] << Log::endl;
						Log::out << "wanted m0  " << ecm0 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}

					if (receiver_outputs[i] != m0)
					{
						Log::out << "Bad PvwBaseOT message " << i << " received" << Log::endl;
						Log::out << "Received   " << receiver_outputs[i] << Log::endl;
						Log::out << "wanted m0  " << m0 << Log::endl;
						throw std::runtime_error("rt error at " LOCATION);
					}
				}
#endif
			}

		}
		deletePercision();
		//delete GetPrecision();
		//Log::out << "base ots done" << Log::endl;
	}

	void PvwBaseOT::check()
	{
		ByteStream os;
		block tmp;

		for (int i = 0; i < nOT; i++)
		{
			if (mOTRole & Sender)
			{
				// send both inputs over
				os.append(sender_inputs[i][0]);
				os.append(sender_inputs[i][1]);

				if (eq(sender_inputs[i][0], sender_inputs[i][1]))
					throw std::runtime_error("rt error at " LOCATION);

				mChannel.asyncSendCopy(os);
			}

			if (mOTRole & Receiver)
			{
				mChannel.recv(os);

				os.consume((u8*)&tmp, sizeof(block));

				if (receiver_inputs[i] == 1)
				{
					os.consume((u8*)&tmp, sizeof(block));
				}

				if (neq(tmp, receiver_outputs[i]))
				{
					Log::out << "Base Incorrect OT" << Log::endl;
					Log::out << "I        have " << receiver_outputs[i] << Log::endl;
					Log::out << "but they have " << tmp << Log::endl;

					throw std::runtime_error("Exit");;
				}
			}
			os.setp(0);
		}
	}

}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_WHIncoherentPhotonScatteringDistribution.cpp
//! \author Alex Robinson
//! \brief  The Waller-Hartree incoherent photon scattering distribution def.
//!
//---------------------------------------------------------------------------//

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

// FRENSIE Includes
#include "MonteCarlo_WHIncoherentPhotonScatteringDistribution.hpp"
#include "MonteCarlo_PhotonKinematicsHelpers.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_DesignByContract.hpp"

namespace MonteCarlo{

// Constructor
WHIncoherentPhotonScatteringDistribution::WHIncoherentPhotonScatteringDistribution(
	  const std::shared_ptr<const ScatteringFunction>& scattering_function,
	  const double kahn_sampling_cutoff_energy )
  : IncoherentPhotonScatteringDistribution( kahn_sampling_cutoff_energy ),
    d_scattering_function( scattering_function )
{
  // Make sure the scattering function is valid
  testPrecondition( scattering_function.get() );
}

// Evaluate the distribution
double WHIncoherentPhotonScatteringDistribution::evaluate(
				   const double incoming_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  const double scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy,
				      scattering_angle_cosine );

  const double diff_kn_cross_section =
    this->evaluateKleinNishinaDist( incoming_energy,
				    scattering_angle_cosine );

  return diff_kn_cross_section*scattering_function_value;
}

// Evaluate the integrated cross section (b)
double WHIncoherentPhotonScatteringDistribution::evaluateIntegratedCrossSection(
						  const double incoming_energy,
						  const double precision) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Evaluate the integrated cross section
  boost::function<double (double x)> diff_cs_wrapper =
    boost::bind<double>( &WHIncoherentPhotonScatteringDistribution::evaluate,
			 boost::cref( *this ),
			 incoming_energy,
			 _1 );

  double abs_error, integrated_cs;

  Utility::GaussKronrodIntegrator<double> quadrature_gkq_set( precision );

  quadrature_gkq_set.integrateAdaptively<15>( diff_cs_wrapper,
					     -1.0,
					     1.0,
					     integrated_cs,
					     abs_error );

  // Make sure the integrated cross section is valid
  testPostcondition( integrated_cs > 0.0 );

  return integrated_cs;
}

// Sample an outgoing energy and direction from the distribution
/*! \details This function will only sample a Compton line energy (no
 * Doppler broadening).
 */
void WHIncoherentPhotonScatteringDistribution::sample(
				     const double incoming_energy,
				     double& outgoing_energy,
				     double& scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  Counter trial_dummy;

  return this->sampleAndRecordTrials( incoming_energy,
				      outgoing_energy,
				      scattering_angle_cosine,
				      trial_dummy );
}

// Sample an outgoing energy and direction and record the number of trials
/*! \details This function will only sample a Compton line energy (no
 * Doppler broadening).
 */
void WHIncoherentPhotonScatteringDistribution::sampleAndRecordTrials(
					    const double incoming_energy,
					    double& outgoing_energy,
					    double& scattering_angle_cosine,
					    Counter& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Evaluate the maximum scattering function value
  const double max_scattering_function_value =
    this->evaluateScatteringFunction( incoming_energy, -1.0 );

  while( true )
  {
    this->sampleAndRecordTrialsKleinNishina( incoming_energy,
					     outgoing_energy,
					     scattering_angle_cosine,
					     trials );

    const double scattering_function_value =
      this->evaluateScatteringFunction( incoming_energy,
					scattering_angle_cosine );

    const double scaled_random_number = max_scattering_function_value*
      Utility::RandomNumberGenerator::getRandomNumber<double>();

    if( scaled_random_number <= scattering_function_value )
      break;
  }

  // Make sure the scattering angle cosine is valid
  testPostcondition( scattering_angle_cosine >= -1.0 );
  testPostcondition( scattering_angle_cosine <= 1.0 );
  // Make sure the compton line energy is valid
  testPostcondition( outgoing_energy <= incoming_energy );
}

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// end MonteCarlo_WHIncoherentPhotonScatteringDistribution.cpp
//---------------------------------------------------------------------------//


#ifndef STAN_MATH_PRIM_SCAL_PROB_BINOMIAL_LOGIT_LPMF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BINOMIAL_LOGIT_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/log_inv_logit.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/**
 * Binomial log PMF in logit parametrization. Binomial(n|n, inv_logit(alpha))
 *
 * If given vectors of matching lengths, returns
 * the log sum of probabilities.
 *
 * @param n successes variable
 * @param N population size parameter
 * @param alpha logit transformed probability parameter
 *
 * @return log probability or log sum of probabilities
 *
 * @throw std::domain_error if N is negative or probability parameter is invalid
 * @throw std::invalid_argument if vector sizes do not match
 */
template <bool propto, typename T_n, typename T_N, typename T_prob>
typename return_type<T_prob>::type binomial_logit_lpmf(const T_n& n,
                                                       const T_N& N,
                                                       const T_prob& alpha) {
  typedef typename stan::partials_return_type<T_n, T_N, T_prob>::type
      T_partials_return;

  static const char* function = "binomial_logit_lpmf";

  if (size_zero(n, N, alpha))
    return 0.0;

  T_partials_return logp = 0;
  check_bounded(function, "Successes variable", n, 0, N);
  check_nonnegative(function, "Population size parameter", N);
  check_finite(function, "Probability parameter", alpha);
  check_consistent_sizes(function, "Successes variable", n,
                         "Population size parameter", N,
                         "Probability parameter", alpha);

  if (!include_summand<propto, T_prob>::value)
    return 0.0;

  scalar_seq_view<T_n> n_vec(n);
  scalar_seq_view<T_N> N_vec(N);
  scalar_seq_view<T_prob> alpha_vec(alpha);
  size_t size = max_size(n, N, alpha);

  operands_and_partials<T_prob> ops_partials(alpha);

  if (include_summand<propto>::value) {
    for (size_t i = 0; i < size; ++i)
      logp += binomial_coefficient_log(N_vec[i], n_vec[i]);
  }

  VectorBuilder<true, T_partials_return, T_prob> log_inv_logit_alpha(
      length(alpha));
  for (size_t i = 0; i < length(alpha); ++i)
    log_inv_logit_alpha[i] = log_inv_logit(value_of(alpha_vec[i]));

  VectorBuilder<true, T_partials_return, T_prob> log_inv_logit_neg_alpha(
      length(alpha));
  for (size_t i = 0; i < length(alpha); ++i)
    log_inv_logit_neg_alpha[i] = log_inv_logit(-value_of(alpha_vec[i]));

  for (size_t i = 0; i < size; ++i)
    logp += n_vec[i] * log_inv_logit_alpha[i]
            + (N_vec[i] - n_vec[i]) * log_inv_logit_neg_alpha[i];

  if (length(alpha) == 1) {
    T_partials_return temp1 = 0;
    T_partials_return temp2 = 0;
    for (size_t i = 0; i < size; ++i) {
      temp1 += n_vec[i];
      temp2 += N_vec[i] - n_vec[i];
    }
    if (!is_constant_struct<T_prob>::value) {
      ops_partials.edge1_.partials_[0]
          += temp1 * inv_logit(-value_of(alpha_vec[0]))
             - temp2 * inv_logit(value_of(alpha_vec[0]));
    }
  } else {
    if (!is_constant_struct<T_prob>::value) {
      for (size_t i = 0; i < size; ++i)
        ops_partials.edge1_.partials_[i]
            += n_vec[i] * inv_logit(-value_of(alpha_vec[i]))
               - (N_vec[i] - n_vec[i]) * inv_logit(value_of(alpha_vec[i]));
    }
  }

  return ops_partials.build(logp);
}

template <typename T_n, typename T_N, typename T_prob>
inline typename return_type<T_prob>::type binomial_logit_lpmf(
    const T_n& n, const T_N& N, const T_prob& alpha) {
  return binomial_logit_lpmf<false>(n, N, alpha);
}

}  // namespace math
}  // namespace stan
#endif