/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2005 Aurelien Chanudet
 Copyright (C) 2005 Plamen Neykov
 Copyright (C) 2005, 2006 Eric Ehlers
 Copyright (C) 2006, 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#ifdef HAVE_CONFIG_H
#include <qlo/config.hpp>
#endif

#include <qlo/yieldtermstructures.hpp>
#include <qlo/ratehelpers.hpp>

#include <ql/time/date.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/termstructures/yield/zerocurve.hpp>
#include <ql/termstructures/yield/impliedtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/yield/forwardspreadedtermstructure.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/interpolations/forwardflatinterpolation.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>

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

using boost::algorithm::to_upper_copy;
using boost::shared_ptr;

using ObjectHandler::ValueObject;

using QuantLib::CubicInterpolation;
using QuantLib::InterpolatedDiscountCurve;
using QuantLib::InterpolatedZeroCurve;
using QuantLib::InterpolatedForwardCurve;

namespace QuantLibAddin {

    DiscountCurve::DiscountCurve(
        const shared_ptr<ValueObject>& prop,
        const std::vector<QuantLib::Date>& dates,
        const std::vector<QuantLib::DiscountFactor>& dfs,
        const QuantLib::DayCounter& dayCounter,
        bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::DiscountCurve(dates, dfs, dayCounter));
    }

    ZeroCurve::ZeroCurve(const shared_ptr<ValueObject>& prop,
                         const std::vector<QuantLib::Date>& dates,
                         const std::vector<QuantLib::Rate>& zeroRates,
                         const QuantLib::DayCounter& dayCounter,
                         bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ZeroCurve(dates, zeroRates, dayCounter));
    }

    ForwardCurve::ForwardCurve(const shared_ptr<ValueObject>& prop,
                               const std::vector<QuantLib::Date>& dates,
                               const std::vector<QuantLib::Rate>& fwdRates,
                               const QuantLib::DayCounter& dayCounter,
                               bool perm) : YieldTermStructure(prop, perm)
    {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ForwardCurve(dates, fwdRates, dayCounter));
    }

    FlatForward::FlatForward(const shared_ptr<ValueObject>& prop,
                             QuantLib::Natural nDays,
                             const QuantLib::Calendar& calendar,
                             const QuantLib::Handle<QuantLib::Quote>& forward,
                             const QuantLib::DayCounter& dayCounter,
                             QuantLib::Compounding compounding,
                             QuantLib::Frequency frequency,
                             bool perm)
    : YieldTermStructure(prop, perm)
    {
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::FlatForward(nDays, calendar, forward, dayCounter,
                                  compounding, frequency));
    }

    ForwardSpreadedTermStructure::ForwardSpreadedTermStructure(
            const shared_ptr<ValueObject>& prop,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& hYTS,
            const QuantLib::Handle<QuantLib::Quote>& spread,
            bool perm) : YieldTermStructure(prop, perm) {

        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ForwardSpreadedTermStructure(hYTS, spread));
    }

    ImpliedTermStructure::ImpliedTermStructure(
            const shared_ptr<ValueObject>& prop,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& hYTS,
            const QuantLib::Date& referenceDate,
            bool perm)
    : YieldTermStructure(prop, perm)
    {
        libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
            QuantLib::ImpliedTermStructure(hYTS, referenceDate));
    }

    InterpolatedYieldCurve::InterpolatedYieldCurve(
            const shared_ptr<ValueObject>& prop,
            const std::vector<QuantLib::Date>& dates,
            const std::vector<QuantLib::Real>& data,
            const QuantLib::Calendar& calendar,
            const QuantLib::DayCounter& dayCounter,
            const std::vector<QuantLib::Handle<QuantLib::Quote> >& jumps,
            const std::vector<QuantLib::Date>& jumpDates,
            const std::string& traitsID,
            const std::string& interpolatorID,
            bool perm)
    : YieldTermStructure(prop, perm),
      traitsID_(to_upper_copy(traitsID)),
      interpolatorID_(to_upper_copy(interpolatorID))
    {
        if (traitsID_=="DISCOUNT") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedDiscountCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else if (traitsID_=="ZEROYIELD") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedZeroCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else if (traitsID_=="FORWARDRATE") {
            if (interpolatorID_=="BACKWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::BackwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="FORWARDFLAT") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::ForwardFlat>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Linear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="LOGLINEAR") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogLinear>(
                        dates, data, dayCounter, calendar, jumps, jumpDates));
            } else if (interpolatorID_=="CUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, false,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, false,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Spline, true,
                                        CubicInterpolation::SecondDerivative, 0.0,
                                        CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Spline, true,
                                           CubicInterpolation::SecondDerivative, 0.0,
                                           CubicInterpolation::SecondDerivative, 0.0)));
            } else if (interpolatorID_=="KRUGERCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="KRUGERLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Kruger)));
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::FritschButland)));
            } else if (interpolatorID_=="PARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic)));
            } else if (interpolatorID_=="LOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, false)));
            } else if (interpolatorID_=="MONOTONICPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::Cubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::Cubic(CubicInterpolation::Parabolic, true)));
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") {
                libraryObject_ = shared_ptr<QuantLib::Extrapolator>(new
                    InterpolatedForwardCurve<QuantLib::LogCubic>(
                        dates, data, dayCounter, calendar, jumps, jumpDates,
                        QuantLib::LogCubic(CubicInterpolation::Parabolic, true)));
            } else
                QL_FAIL("unknown interpolatorID: " << interpolatorID_);
        } else
            QL_FAIL("unknown traitsID: " << traitsID_);

    }

    #define RESOLVE_TEMPLATE(NAME) \
        if (traitsID_=="DISCOUNT") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedDiscountCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else if (traitsID_=="ZEROYIELD") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedZeroCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else if (traitsID_=="FORWARDRATE") { \
            if (interpolatorID_=="BACKWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::BackwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FORWARDFLAT") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::ForwardFlat> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Linear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGLINEAR") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogLinear> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="CUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGCUBICNATURALSPLINE") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="KRUGERLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="FRITSCHBUTLANDLOGCUBIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="PARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="LOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::Cubic> >(libraryObject_)->NAME(); \
            } else if (interpolatorID_=="MONOTONICLOGPARABOLIC") { \
                return boost::dynamic_pointer_cast<InterpolatedForwardCurve<QuantLib::LogCubic> >(libraryObject_)->NAME(); \
            } else \
                QL_FAIL("unknown interpolatorID: " << interpolatorID_); \
        } else \
            QL_FAIL("unknown traitsID: " << traitsID_);

    const std::vector<QuantLib::Time>& InterpolatedYieldCurve::times() const {
        RESOLVE_TEMPLATE(times)
    }

    const std::vector<QuantLib::Date>& InterpolatedYieldCurve::dates() const {
        RESOLVE_TEMPLATE(dates)
    }

    const std::vector<QuantLib::Real>& InterpolatedYieldCurve::data() const {
        RESOLVE_TEMPLATE(data)
    }

    const std::vector<QuantLib::Time>& InterpolatedYieldCurve::jumpTimes() const {
        RESOLVE_TEMPLATE(jumpTimes)
    }

    const std::vector<QuantLib::Date>& InterpolatedYieldCurve::jumpDates() const {
        RESOLVE_TEMPLATE(jumpDates)
    }

    InterpolatedYieldCurve::InterpolatedYieldCurve(
            const shared_ptr<ValueObject>& prop,
            const std::string& traitsID,
            const std::string& interpolatorID,
            bool perm)
    : YieldTermStructure(prop, perm),
      traitsID_(to_upper_copy(traitsID)),
      interpolatorID_(to_upper_copy(interpolatorID))
    {
    }

    // Stream operator to write a InterpolatedYieldCurvePair to a stream - for logging / error handling.
    std::ostream &operator<<(std::ostream &out,
                             InterpolatedYieldCurvePair tokenPair)
    {
        out << "InterpolatedYieldCurve<";

        switch (tokenPair.first) {
            case InterpolatedYieldCurve::Discount:
                out << "<Discount, ";
                break;
            case InterpolatedYieldCurve::ForwardRate:
                out << "<ForwardRate, ";
                break;
            case InterpolatedYieldCurve::ZeroYield:
                out << "<ZeroYield, ";
                break;
            default:
                OH_FAIL("Unknown value for enumeration QuantLibAddin::InterpolatedYieldCurve::Traits");
        }

        switch (tokenPair.second) {
            case InterpolatedYieldCurve::BackwardFlat:
                out << "BackwardFlat>";
                break;
            case InterpolatedYieldCurve::ForwardFlat:
                out << "ForwardFlat>";
                break;
            case InterpolatedYieldCurve::Linear:
                out << "Linear>";
                break;
            case InterpolatedYieldCurve::LogLinear:
                out << "LogLinear>";
                break;
            case InterpolatedYieldCurve::CubicNaturalSpline:
                out << "CubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::LogCubicNaturalSpline:
                out << "LogCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::MonotonicCubicNaturalSpline:
                out << "MonotonicCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::MonotonicLogCubicNaturalSpline:
                out << "MonotonicLogCubicNaturalSpline>";
                break;
            case InterpolatedYieldCurve::KrugerCubic:
                out << "KrugerCubic>";
                break;
            case InterpolatedYieldCurve::KrugerLogCubic:
                out << "KrugerLogCubic>";
                break;
            case InterpolatedYieldCurve::FritschButlandCubic:
                out << "FritschButlandCubic>";
                break;
            case InterpolatedYieldCurve::FritschButlandLogCubic:
                out << "FritschButlandLogCubic>";
                break;
            case InterpolatedYieldCurve::Parabolic:
                out << "Parabolic>";
                break;
            case InterpolatedYieldCurve::LogParabolic:
                out << "LogParabolic>";
                break;
            case InterpolatedYieldCurve::MonotonicParabolic:
                out << "MonotonicParabolic>";
                break;
            case InterpolatedYieldCurve::MonotonicLogParabolic:
                out << "MonotonicLogParabolic>";
                break;
            default:
                OH_FAIL("Unknown value for enumeration QuantLibAddin::InterpolatedYieldCurve::Interpolator");
        }

        return out;
    }

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_MEANOF_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_MEANOF_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/meanof.hpp>
#include <boost/simd/include/functions/simd/average.hpp>
#include <boost/simd/include/functions/simd/is_finite.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/dispatch/attributes.hpp>

//no overflow average for floating numbers

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( meanof_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 m = min(a0, a1);
      return if_else( logical_and(is_finite(a0),is_finite(a1)),
                      m + (max(a0, a1)-m)*Half<result_type>(),
                      average(a0, a1)
                    );
    }
  };
  BOOST_DISPATCH_IMPLEMENT          ( meanof_, tag::cpu_
                                    , (A0)
                                    , (generic_< arithmetic_<A0> >)
                                      (generic_< arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return (a0 & a1) + ((a0 ^ a1) >> 1);
    }
  };
} } }

#endif


#pragma once

#include <functional>

#include <boost/math/special_functions/prime.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <exrandom/discrete_normal_distribution.hpp>
#include <nfl/params.hpp>

#include <gmpxx.h>
#include <sodium/randombytes.h>
#include "Common.hpp"

/** Overriding abs for basic int type **/
namespace boost {
namespace multiprecision {
RegularInteger abs(RegularInteger in) { return in; }
}  // namespace multiprecision
}  // namespace boost

namespace ligero {
namespace math {

/** Scaling function to bring randomness within a desired range
 * @param val input, modulusIdx raw randomness, prime index
 * @return scaled randomness
 */

uint64_t scaleToPrime(uint64_t input, size_t modulusIdx) {
    double scaleFactor = static_cast<double>(nfl::params<uint64_t>::P[modulusIdx])/static_cast<double>(1ull << 62);
    uint64_t input62 = input & ((1ull << 62) - 1);
    return static_cast<uint64_t>(static_cast<double>(input62)*scaleFactor);
}

uint64_t scaleToUpperBound(uint64_t input, size_t upperBound) {
    float scaleFactor = static_cast<double>(upperBound)/static_cast<double>(1ull << 62);
    uint64_t input62 = input & ((1ull << 62) - 1);
    return static_cast<uint64_t>(static_cast<double>(input62)*scaleFactor);
}

/** Compute hash for given mpz value
 * @param val MPZ_class value
 * @return Hash of @p val
 */
std::string computeHash(mpz_class val) {
    std::string s = val.get_str();
    std::string os = val.get_str();

    unsigned char hash[crypto_generichash_BYTES + 1];
    const unsigned char *message = reinterpret_cast<const unsigned char *> (s.c_str());
    crypto_generichash(hash, crypto_generichash_BYTES,
            message, s.length(),
            NULL, 0);
    hash[crypto_generichash_BYTES] = 0;
    DBG("For value os = " << os << " s = " << s << " generating hash = >>>" << hash << "<<<");
    return std::move(std::string(reinterpret_cast<char*>(hash)));
}

/** Compute hash for given NFL polynomial
 * @param val NFL polynomial
 * @return Hash of @p val
 */
template <typename T, size_t Degree, size_t NbPrimesQ>
std::string computeHash(nfl::poly_p<T, Degree, NbPrimesQ> val) {
    std::stringstream ss(std::stringstream::out | std::stringstream::binary);
    val.serialize_manually(ss);
    std::string s = ss.str();

    unsigned char hash[crypto_generichash_BYTES + 1];
    const unsigned char *message = reinterpret_cast<const unsigned char *> (s.c_str());
    crypto_generichash(hash, crypto_generichash_BYTES,
            message, s.length(),
            NULL, 0);
    hash[crypto_generichash_BYTES] = 0;
    std::string result = std::string(reinterpret_cast<char*>(hash));
    return result;
}

/** Compute hash for given value
 * @param val ((a, b), c), where a, b is vector of mpz_class
 * @return Hash of c
 */
std::string computeHash(std::pair<std::pair<std::vector<mpz_class>, std::vector<mpz_class>>, mpz_class> val) {
    mpz_class v = val.second;
    return computeHash(v);
}

/** Default hash function for any type, will throw a runtime exception */
template <typename T>
std::string computeHash(T val) {
    LOG(FATAL) << "computeHash for this type is not implemented yet";
}

/** Generate a vector of random numbers
 * @param seed Seed to initialize the random generator
 * @param size Length of the output random vector
 * @param numBits Number of bits of each value, default to 128 bits
 * @return A vector of random numbers
 **/
std::vector<mpz_class> generateRandomVector(mpz_class seed, size_t size, size_t numBits = 128) {
    std::vector<mpz_class> result(size);

    const int numBytes = lround(double(numBits) / 8);
    const size_t bufferSize = size * (numBytes + 1);

    char* buf = new char[bufferSize];

    size_t seedSize = (mpz_sizeinbase (seed.get_mpz_t(), 2) + CHAR_BIT-1) / CHAR_BIT;
    unsigned char seedArray[32U];
    std::vector<unsigned char> seedVector(seedSize);

    mpz_export(&seedVector[0], &seedSize, 1, 1, 0, 0, seed.get_mpz_t());
    for (size_t i = 0; i < seedSize && i < 32; ++i) {
        seedArray[i] = seedVector[i];
    }
    for (size_t i = seedSize; i < 32; ++i) {
        seedArray[i] = static_cast<unsigned char>(0);
    }

    randombytes_buf_deterministic((void*)buf, bufferSize, seedArray);

    size_t k = 0;
    for (size_t i = 0; i < result.size(); ++i) {
        assert(k < bufferSize);
        mpz_import(result[i].get_mpz_t(), numBytes, 1, 1, 0, 0, &buf[k]);
        k += numBytes;
        assert(result[i] > 0);
    }
    delete[] buf;

    return result;
}

/** Generate a single mpz random value
 * @param seed Seed to initialize random generator
 * @param numBits Number of bits of generated value
 * @return @p numBits bits random value
 */
mpz_class generateRandomValue(mpz_class seed, size_t numBits = 128) {
    mpz_class result;
    {
        std::vector t = generateRandomVector(seed, 1, numBits);
        result = t[0];
    }
    return result;
}

/** A quick wrapper around GMP's powm implementation for different types. */
mpz_class powm(const mpz_class& b, const mpz_class& p, const mpz_class& m) {
    mpz_class x(b);
    mpz_powm(x.get_mpz_t(), x.get_mpz_t(), p.get_mpz_t(), m.get_mpz_t());

    return x;
}

/** A quick wrapper around GMP's powm implementation for different types. */
template <typename NumberType>
NumberType powm(const NumberType& b, const NumberType& p, const NumberType& m) {
    MPInt x(b);
    MPInt y(p);
    MPInt z(m);

    MPInt o;

    mpz_powm(o.backend().data(),
             x.backend().data(),
             y.backend().data(),
             z.backend().data());

    return NumberType(o);
}

/** A quick wrapper around GMP's powm implementation for different types. */
RegularInteger powm (const RegularInteger& b, const long p, const RegularInteger& m) {
    MPInt x (b);
    MPInt y (p);
    MPInt z (m);

    mpz_powm(
        x.backend().data(),
        x.backend().data(),
        y.backend().data(),
        z.backend().data()
    );

    return RegularInteger(x);
}

/** Divide and round the quotient to nearest integer
 * @param a divident
 * @param b divisor
 * @return quotient rounded to nearest integer
 */
Int64 divideAndRound(const Int64& a, const Int64& b) {
    Int64 quotient, remainder;
    boost::multiprecision::divide_qr(a, b, quotient, remainder);
    if (remainder * 2 >= b) {
        quotient++;
    }
    return quotient;
}

/** 128 bits power
 * @param a 64 bits integer
 * @param exp 64 bits integer
 * @return 128 bits @p a ^ @p exp
 */
WideInteger pow(RegularInteger a, RegularInteger exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return WideInteger(x);
}

WideInteger pow(RegularInteger a, int exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return WideInteger(x);
}

/** N bits power
 * @param a N bits number of type T
 * @param exp 64 bits integer
 * @return N bits @p a ^ @p exp of type T
 */
template <typename NumberType>
NumberType pow(NumberType a, uint64_t exp) {
    MPInt x(0);
    MPInt y(a);

    mpz_pow_ui(
        x.backend().data(),
        y.backend().data(),
        exp);

    return NumberType(x);
}

/** Divide and round the quotient to nearest integer
 * @param a divident of type mpz_int
 * @param b divisor of type mpz_int
 * @return quotient rounded to nearest integer
 */
MPInt divideAndRound(const MPInt& a, const MPInt& b) {
    MPInt quotient, remainder;
    boost::multiprecision::divide_qr(a, b, quotient, remainder);
    if (remainder * 2 >= b) {
        quotient++;
    }
    return quotient;
}

/** Divide and round the quotient to nearest integer
 * @param a divident of type int
 * @param b divisor of type int
 * @return quotient rounded to nearest integer
 */
int64_t divideAndRound(int64_t a, int64_t b) {
    int64_t remainder = a % b;
    int64_t result = static_cast<int64_t>(a / b);
    if (remainder * 2 >= b) {
        result++;
    }
    return result;
}

/** Mod function overloading
 * @param a mpz_class
 * @param b mpz_class
 * @return @p a % @p b
 */
mpz_class mod(const mpz_class& a, const mpz_class& b) {
    return powm(a, 1, b);
}

/** Mod function overloading
 * @param a boost mpz_int
 * @param b boost mpz_int
 * @return @p a % @p b
 */
MPInt mod(MPInt a, MPInt b) {
    return boost::multiprecision::powm(a, 1, b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
RegularInteger mod(RegularInteger a, RegularInteger b) {
// return static_cast<RegularInteger>(boost::multiprecision::powm(MPInt(a), 1, MPInt(b)));
   return static_cast<RegularInteger>((static_cast<WideInteger>(a % b) + static_cast<WideInteger>(b)) % static_cast<WideInteger>(b));
}

RegularInteger
mod(WideInteger a, WideInteger b) {
// return static_cast<RegularInteger>(boost::multiprecision::powm(MPInt(a), 1, MPInt(b)));
   return (((a % b) + b) % b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
Int64 mod(Int64 a, Int64 b) {
    return boost::multiprecision::powm(a, 1, b);
}

/** Mod function overloading
 * @param a 64 bits integer
 * @param b 64 bits integer
 * @return @p a % @p b
 */
int64_t mod(int64_t a, int64_t b) {
    return ((a % b) + b) % b;
}

template <typename NumberType>
struct ChiDist {

    // This constant defines distributions standard deviation

    std::mt19937 g;
    double chiStd;

    ChiDist(int seed, double _chiStd) : g(seed), chiStd(_chiStd){ }
    ChiDist(int seed) : g(seed), chiStd(ligero::kDefaultChiStd){ }

    ChiDist() : g(std::random_device()()), chiStd(ligero::kDefaultChiStd){}

    NumberType operator()() {
        exrandom::discrete_normal_distribution N(0, 1, lround(chiStd * 10), 10);
        return boost::multiprecision::abs(NumberType(N(g)));
    }
};

/** Sample a vector of random field element
 * @param m Size of total samples
 * @param q Boundary
 * @param chiStd Standard deviation
 * @return A vector of random sample
 */
template <typename FieldT>
std::vector<FieldT> sampleRandomVectorOverField(size_t m, FieldT q, double chiStd) {
    //std::function<typename FieldT::underlyingType>
    auto dist = ligero::math::ChiDist<typename FieldT::underlyingType>(std::random_device()(), chiStd);
    std::vector<FieldT> e(m);

    for (int i = 0; i < static_cast<int>(m); i++) {
        typename FieldT::underlyingType tmp;
        do {
            tmp = static_cast<typename FieldT::underlyingType>(dist());
        } while (tmp>q.getValue());

        e[i] = FieldT(tmp);
    }

    return e;
}

/** A quick wrapper around GMP's modular inverse implementation for different types.
 *
 *  Note that the implementation here promotes arbitrary number types to our MPInt
 *  type so that we can call into `mpz_invert` directly with mpz types. This allows
 *  us to leverage GMP's existing implementation, but obviously has overhead. May
 *  be an optimization opportunity later if we want to consider writing our own modular
 *  inverse implementation.
 */
mpz_class
mod_inverse (const mpz_class& a, const mpz_class& b) {
    mpz_t z;
    mpz_init(z);

    int r = mpz_invert(z, a.get_mpz_t(), b.get_mpz_t());

    if (r == 0)
        throw std::runtime_error("Modular inverse does not exist for the given operands.");

    auto result = mpz_class(z);
    mpz_clear(z);

    return result;
}

/** Deconstruct input `v` by Chinese Remainder Theorem given a vector of moduli
 *  in the input `alphas`.
 */
std::vector<mpz_class> crt_deconstruct (mpz_class v, const std::vector<mpz_class>& alphas)
{
    std::vector<mpz_class> ds (alphas.size());

    for (int i = 0; i < alphas.size(); ++i) {
        ds[i] = mod(v, alphas[i]);
    }

    return ds;
}

/** Reconstruct a value `v` by Chinese Remainder Theorem given a vector of its
 *  components and a vector of moduli.
 */
mpz_class crt_reconstruct (const std::vector<mpz_class>& ds, std::vector<mpz_class>& coeffs, const std::vector<mpz_class>& alphas)
{
    if (ds.size() != alphas.size())
        throw std::runtime_error("Reconstruction vector lengths don't match.");

    coeffs.resize(ds.size());

    //mpz_class p = alphas.prod();
    mpz_class p = 1;
    for (size_t i = 0; i < alphas.size(); ++i) {
        p *= alphas[i];
    }

    for (size_t i = 0; i < ds.size(); ++i) {
        mpz_class pa = p / alphas[i];
        mpz_class x = mod(pa, alphas[i]);
        coeffs[i] = pa * mod_inverse(x, alphas[i]);
    }

    mpz_class dotProduct = 0;
    for (size_t i = 0; i < ds.size(); ++i) {
        dotProduct += ds[i] * coeffs[i];
    }

    return dotProduct % p;
}

/** A quick helper function for calculating the product of a vector of numbers. */
mpz_class vectorProduct(const std::vector<mpz_class>& xs) {
    mpz_class product = 1;

    for (auto& x : xs)
        product = product * x;

    return product;
}
/* fixed version */
std::pair<std::vector<mpz_class>, std::vector<size_t>>
fixed_bucket_n_primes (int productBitThreshold, size_t degree, int tauLimitBit)
{
    int number_of_buckets = std::ceil(double(productBitThreshold)/double(tauLimitBit)); // This rounds down.
    std::vector<mpz_class> alphas(number_of_buckets);
    std::vector<size_t> bucketSize(number_of_buckets);

    gmp_randclass grc(gmp_randinit_default);
    grc.seed(0);

    for (int i = 0; i < number_of_buckets; ++i) {
        mpz_class r = grc.get_z_bits(tauLimitBit);
        while (!boost::multiprecision::miller_rabin_test(MPInt(r.get_mpz_t()), 64) || mpz_sizeinbase(r.get_mpz_t(),2) < tauLimitBit) {
            r = grc.get_z_bits(tauLimitBit);
        }
        alphas[i] = r;
        // DBG("alphas[" << i << "] = " << r);
    }

    for (size_t i = 0; i < number_of_buckets; ++i) {
        bucketSize[i] = lrint(floor(double(degree) / double(number_of_buckets)));
        DBG("bucketSize[ " << i << "] = " << bucketSize[i]);
    }

    return std::pair{alphas, bucketSize};
}

/** Implements a prime bucketing algorithm, returning a vector of numbers such that
 *  each number is a product of subsequent primes that is at least 18bits, and such
 *  that the product of numbers in the vector is at least `productBitThreshold` bits.
 */
std::pair<std::vector<mpz_class>, std::vector<size_t>>
balanced_bucket_n_primes (int productBitThreshold, size_t degree, int tauLimitBit, int startJ = 1)
{
    int number_of_buckets = std::ceil(double(productBitThreshold)/double(tauLimitBit)); // This rounds down.
    std::vector<mpz_class> alphas(number_of_buckets);
    std::vector<mpz_class> primes;
    mpz_class alpha = 1;
    int j = startJ;
    while(1)
    {
        alpha*= boost::math::prime(j);
        primes.push_back(boost::math::prime(j));
        if(mpz_sizeinbase(alpha.get_mpz_t(), 2) >= static_cast<unsigned int>(productBitThreshold)) break;
        j++;
    }
    DBG("Maximum size of prime = " << mpz_sizeinbase((primes[primes.size()-1]).get_mpz_t(),2));

    std::vector<double> weights(number_of_buckets);
    for ( int i = 0; i < number_of_buckets; i++)
    {
      alphas[i] = 1;
      weights[i] = 1;
    }
    double sum = 0.0;
    for ( int i = primes.size()-1;i >= 0;  i--)
    {
      double maxweight = weights[0];
      int maxpos = 0;
      for (int j = 1; j < number_of_buckets; j++)
      {
	      mpz_class temp = alphas[j];
        if(weights[j] > maxweight && mpz_sizeinbase(temp.get_mpz_t(),2) <= tauLimitBit)
        {
          maxweight = weights[j];
          maxpos = j;
        }
      }
      alphas[maxpos] *= primes[i];
      weights[maxpos] *= (double(1.0) - (double(1)/primes[i].get_d()));
    }
    for ( int i = 0; i < number_of_buckets; i++)
    {
      weights[i] = double(1)/weights[i];
      sum += weights[i];
    }

    std::vector<size_t> bucketSize(weights.size());
    size_t total = 0;
    for (size_t i = 0; i < weights.size() - 1; ++i) {
        bucketSize[i] = lround(double(weights[i] * double(degree)) / double(sum));
        total += bucketSize[i];
        DBG("bucketSize[ " << i << "] = " << bucketSize[i]);
    }
    bucketSize[bucketSize.size() - 1] = degree - total;
    DBG("bucketSize[ " << bucketSize.size() - 1 << "] = " << bucketSize[bucketSize.size() - 1]);

    return std::pair{alphas, bucketSize};
    }

}//namespace math
}//namespace ligero


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

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "Transformation.hpp"

#include "../core/Assert.hpp"

#include <boost/math/constants/constants.hpp>

#include <polypartition/polypartition.h>

#include <list>

namespace openstudio{
  /// convert degrees to radians
  double degToRad(double degrees)
  {
    return degrees*boost::math::constants::pi<double>()/180.0;
  }

  /// convert radians to degrees
  double radToDeg(double radians)
  {
    return radians*180.0/boost::math::constants::pi<double>();
  }

  /// compute area from surface as Point3dVector
  boost::optional<double> getArea(const Point3dVector& points)
  {
    boost::optional<double> result;
    OptionalVector3d newall = getNewallVector(points);
    if (newall){
      result = newall->length() / 2.0;
    }
    return result;
  }

  // compute Newall vector from Point3dVector, direction is same as outward normal
  // magnitude is twice the area
  OptionalVector3d getNewallVector(const Point3dVector& points)
  {
    OptionalVector3d result;
    unsigned N = points.size();
    if (N >= 3){
      Vector3d vec;
      for (unsigned i = 1; i < N-1; ++i){
        Vector3d v1 = points[i] - points[0];
        Vector3d v2 = points[i+1] - points[0];
        vec += v1.cross(v2);
      }
     result = vec;
   }
   return result;
  }

  // compute outward normal from Point3dVector
  OptionalVector3d getOutwardNormal(const Point3dVector& points)
  {
    OptionalVector3d result = getNewallVector(points);
    if (result){
      if (!result->normalize()){
        result.reset();
      }
    }
    return result;
  }

  /// compute centroid from surface as Point3dVector
  OptionalPoint3d getCentroid(const Point3dVector& points)
  {
    OptionalPoint3d result;

    if (points.size() >= 3){
      // convert to face coordinates
      Transformation alignFace = Transformation::alignFace(points);
      Point3dVector surfacePoints = alignFace.inverse()*points;

      unsigned N = surfacePoints.size();
      double A = 0;
      double cx = 0;
      double cy = 0;
      for (unsigned i = 0; i < N; ++i){
        double x1, x2, y1, y2;
        if (i == N-1){
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[0].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[0].y();
        }else{
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[i+1].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[i+1].y();
        }

        double dA = (x1*y2-x2*y1);
        A += 0.5*dA;
        cx += (x1+x2)*dA;
        cy += (y1+y2)*dA;
      }

      if (A > 0){
        // centroid in face coordinates
        Point3d surfaceCentroid(cx/(6.0*A), cy/(6.0*A), 0.0);

        // centroid
        result = alignFace*surfaceCentroid;
      }
    }
    return result;
  }

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    unsigned N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

  std::vector<Point3d> removeCollinear(const Point3dVector& points, double tol)
  {
    unsigned N = points.size();
    if (N < 3){
      return points;
    }

    std::vector<Point3d> result;
    Point3d lastPoint = points[0];
    result.push_back(lastPoint);

    for (unsigned i = 1; i < N; ++i){
      Point3d currentPoint = points[i];
      Point3d nextPoint = points[0];
      if (i < N-1){
        nextPoint = points[i+1];
      }

      Vector3d a = (currentPoint - lastPoint);
      Vector3d b = (nextPoint - currentPoint);

      // if these fail to normalize we have zero length vectors (e.g. adjacent points)
      if (a.normalize()){
        if (b.normalize()){

          Vector3d c = a.cross(b);
          if (c.length() >= tol){
            // cross product is significant
            result.push_back(currentPoint);
            lastPoint = currentPoint;
          }else{
            // see if dot product is near -1
            double d = a.dot(b);
            if (d <= -1.0 + tol){
              // this is a line reversal
              result.push_back(currentPoint);
              lastPoint = currentPoint;
            }
          }
        }
      }
    }

    return result;
  }

  double getDistance(const Point3d& point1, const Point3d& point2) {
    double dx = point1.x() - point2.x();
    double dy = point1.y() - point2.y();
    double dz = point1.z() - point2.z();
    double result = std::sqrt(dx*dx + dy*dy + dz*dz);
    return result;
  }

  double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment)
  {
    if (lineSegment.size() != 2){
      return 0;
    }

    // http://paulbourke.net/geometry/pointlineplane/

    Point3d point1 = lineSegment[0];
    Point3d point2 = lineSegment[1];

    Vector3d p2p1 = point2-point1;
    Vector3d p3p1 = point-point1;

    double d12 = p2p1.length();
    if (d12 < 1.0e-12){
      return p3p1.length();
    }

    Point3d closestPoint;
    double u = p3p1.dot(p2p1) / (d12*d12);
    if (u < 0){
      closestPoint = point1;
    }else if (u > 1){
      closestPoint = point2;
    }else{
      closestPoint = point1 + u*p2p1;
    }

    Vector3d diff = point - closestPoint;

    return diff.length();
  }

  double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle)
  {
    if (triangle.size() != 3){
      return 0;
    }

    //Distance Between Point and Triangle in 3D
    //David Eberly
    //Geometric Tools, LLC
    //http://www.geometrictools.com/

    //T(s; t) = B+sE0+tE1

    Point3d B = triangle[0];
    Vector3d E0 = triangle[1] - triangle[0];
    Vector3d E1 = triangle[2] - triangle[0];
    Vector3d BminusP = B - point;

    double b = E0.dot(E1);

    if (std::abs(b) > 1.0-1.0E-12){
      // triangle is collinear
      return 0;
    }

    double a = E0.dot(E0);
    double c = E1.dot(E1);
    double d = E0.dot(BminusP);
    double e = E1.dot(BminusP);
    // double f = BminusP.dot(BminusP); // unused

    double det = a*c-b*b;
    double s = b*e-c*d;
    double t = b*d-a*e;

    Point3d closestPoint;

    if ( s+t <= det ) {
      if ( s < 0 ) {
        if ( t < 0 ) {
          //region 4, closest to point triangle[0]
          return getDistance(point, triangle[0]);
        } else {
          //region 3, closest to line triangle[0] to triangle[2]
          std::vector<Point3d> line;
          line.push_back(triangle[0]);
          line.push_back(triangle[2]);
          return getDistancePointToLineSegment(point, line);
        }
      } else if ( t < 0 ) {
        //region 5, closest to line triangle[0] to triangle[1]
        std::vector<Point3d> line;
        line.push_back(triangle[0]);
        line.push_back(triangle[1]);
        return getDistancePointToLineSegment(point, line);
      } else {
        //region 0, closest point is inside triangle
        double invDet = 1.0/det;
        closestPoint = B + invDet*s*E0 + invDet*t*E1;
      }
    } else {
      if ( s < 0 ) {
        //region 2, closest to point triangle[2]
        return getDistance(point, triangle[2]);
      } else if ( t < 0 ) {
        //region 6, closest to point triangle[1]
        return getDistance(point, triangle[1]);
      } else {
        //region 1, closest to line triangle[1] to triangle[2]
        std::vector<Point3d> line;
        line.push_back(triangle[1]);
        line.push_back(triangle[2]);
        return getDistancePointToLineSegment(point, line);
      }
    }

    Vector3d diff = point-closestPoint;
    return diff.length();
  }

  double getAngle(const Vector3d& vector1, const Vector3d& vector2) {
    Vector3d working1(vector1);
    working1.normalize();
    Vector3d working2(vector2);
    working2.normalize();
    return acos(working1.dot(working2));
  }

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2)
  {

    // for more accuracy would want to use WGS-84 ellipsoid params and Vincenty formula

    // Haversine formula
    double R = 6371000; // Earth radius meters
    double deltaLat = degToRad(lat2-lat1);
    double deltaLon = degToRad(lon2-lon1);
    double a = sin(deltaLat/2) * sin(deltaLat/2) +
               cos(degToRad(lat1)) * cos(degToRad(lat2)) *
               sin(deltaLon/2) * sin(deltaLon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = R * c;

    return d;
  }

  bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol)
  {
    unsigned N = points1.size();
    if (N != points2.size()){
      return false;
    }

    if (N == 0){
      return true;
    }

    bool result = false;

    // look for a common starting point
    for (unsigned i = 0; i < N; ++i){
      if (getDistance(points1[0], points2[i]) <= tol){

        result = true;

        // check all other points
        for (unsigned j = 0; j < N; ++j){
          if (getDistance(points1[j], points2[(i + j) % N]) > tol){
            result = false;
            break;
          }
        }
      }

      if (result){
        return result;
      }
    }

    return result;
  }

  Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    for (const Point3d& otherPoint : allPoints){
      if (std::sqrt(std::pow(point3d.x()-otherPoint.x(), 2) + std::pow(point3d.y()-otherPoint.y(), 2) + std::pow(point3d.z()-otherPoint.z(), 2)) < tol){
        return otherPoint;
      }
    }
    allPoints.push_back(point3d);
    return point3d;
  }

  std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // check input
    if (vertices.size () < 3){
      return result;
    }

    boost::optional<Vector3d> normal = getOutwardNormal(vertices);
    if (!normal || normal->z() > -0.999){
      return result;
    }

    for (const auto& hole : holes){
      normal = getOutwardNormal(hole);
      if (!normal || normal->z() > -0.999){
        return result;
      }
    }

    std::vector<Point3d> allPoints;

    // PolyPartition does not support holes which intersect the polygon or share an edge
    // if any hole is not fully contained we will use boost to remove all the holes
    bool polyPartitionHoles = true;
    for (const std::vector<Point3d>& hole : holes){
      if (!within(hole, vertices, tol)){
        // PolyPartition can't handle this
        polyPartitionHoles = false;
        break;
      }
    }

    if (!polyPartitionHoles){
      // use boost to do all the intersections
      std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
      std::vector<std::vector<Point3d> > noHoles;
      for (const std::vector<Point3d>& face : allFaces){
        std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
        result.insert(result.end(), temp.begin(), temp.end());
      }
      return result;
    }

    // convert input to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
    outerPoly.Init(vertices.size());
    outerPoly.SetHole(false);
    unsigned n = vertices.size();
    for(unsigned i = 0; i < n; ++i){

      // should all have zero z coordinate now
      double z = vertices[n-i-1].z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
        return result;
      }

      Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
      outerPoly[i].x = point.x();
      outerPoly[i].y = point.y();
    }
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    for (const std::vector<Point3d>& holeVertices : holes){

      if (holeVertices.size () < 3){
        LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
        continue;
      }

      TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
      innerPoly.Init(holeVertices.size());
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < holeVertices.size(); ++i){

        // should all have zero z coordinate now
        double z = holeVertices[i].z();
        if (abs(z) > tol){
          LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
          return result;
        }

        Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
        innerPoly[i].x = point.x();
        innerPoly[i].y = point.y();
      }
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.Triangulate_EC(&polys,&resultPolys);
    if (test == 0){
      test = pp.Triangulate_MONO(&polys, &resultPolys);
    }
    if (test == 0){
      LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
      return result;
    }

    // convert back to vertices
    std::list<TPPLPoly>::iterator it, itend;
    //std::cout << "Start" << std::endl;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){

      it->SetOrientation(TPPL_CW);

      std::vector<Point3d> triangle;
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        triangle.push_back(Point3d(point.x, point.y, 0));
      }
      //std::cout << triangle << std::endl;
      result.push_back(triangle);
    }
    //std::cout << "End" << std::endl;

    return result;
  }

  std::vector<Point3d> moveVerticesTowardsPoint(const Point3dVector& vertices, const Point3d& point, double distance)
  {
    Point3dVector result;
    for (const Point3d& vertex : vertices){
      Vector3d vector = point-vertex;
      vector.setLength(distance);
      result.push_back(vertex+vector);
    }
    return result;
  }

  std::vector<Point3d> reverse(const Point3dVector& vertices)
  {
    std::vector<Point3d> result(vertices);
    std::reverse(result.begin(), result.end());
    return result;
  }

} // openstudio


// Copyright 2005-2008 Daniel James.
// 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)

#include "./config.hpp"

#ifdef TEST_EXTENSIONS
#  ifdef TEST_STD_INCLUDES
#    include <functional>
#  else
#    include <boost/functional/hash.hpp>
#  endif
#endif

#include <boost/detail/lightweight_test.hpp>

#ifdef TEST_EXTENSIONS

#include <complex>
#include <sstream>
#include <boost/limits.hpp>

#if defined(BOOST_MSVC)
#pragma warning(push)
#pragma warning(disable:4244) // conversion from 'unsigned long' to 'unsigned short', possible loss of data
#pragma warning(disable:4512) // assignment operator could not be generated
#endif

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#if defined(BOOST_MSVC)
#pragma warning(pop)
#endif

template <class T>
void generic_complex_tests(std::complex<T> v)
{
    HASH_NAMESPACE::hash<std::complex<T> > complex_hasher;

    BOOST_TEST(complex_hasher(v) == complex_hasher(v));

    HASH_NAMESPACE::hash<T> real_hasher;
    T real = v.real();
    T imag = v.imag();

    BOOST_TEST(real_hasher(real) == complex_hasher(std::complex<T>(real)));

    if(imag != 0 && real_hasher(real) == complex_hasher(v)) {
        std::ostringstream os;
        os<<"real_hasher("<<real<<") == complex_hasher("
            <<v.real()<<" + "<<v.imag()<<"i) == "
            <<real_hasher(real)<<" (This might not be a bug).";
        BOOST_ERROR(os.str().c_str());
    }
}

template <class Float>
void complex_float_tests(Float*)
{
    boost::mt19937 rng;
    boost::uniform_real<Float> uniform;
    boost::variate_generator<boost::mt19937&, boost::uniform_real<Float> >
        uniform_generator(rng, uniform);

    for(int i = 0; i < 100; ++i)
    {
        std::complex<Float> v(uniform_generator(), uniform_generator());
        generic_complex_tests(v);
    }
}

template <class Integer>
void complex_integral_tests(Integer*)
{
    boost::mt19937 rng;
    boost::uniform_int<Integer> uniform(
            (std::numeric_limits<Integer>::min)(),
            (std::numeric_limits<Integer>::max)());
    boost::variate_generator<boost::mt19937&, boost::uniform_int<Integer> >
        uniform_generator(rng, uniform);

    for(int i = 0; i < 100; ++i)
    {
        std::complex<Integer>v(uniform_generator(), uniform_generator());
        generic_complex_tests(v);
    }
}

int main()
{
    complex_float_tests((float*) 0);
    complex_float_tests((double*) 0);
    complex_float_tests((long double*) 0);
    complex_integral_tests((short*) 0);
    complex_integral_tests((int*) 0);
    complex_integral_tests((long*) 0);
    complex_integral_tests((unsigned short*) 0);
    complex_integral_tests((unsigned int*) 0);
    complex_integral_tests((unsigned long*) 0);

    return boost::report_errors();
}

#endif


#include "ValueWithError.hpp"
#include "precompiled.hpp"

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wglobal-constructors"
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/test/unit_test.hpp>

#include <iostream>

using namespace error_propagation;

namespace {

  typedef ValueWithError<double> VD;
  using boost::math::pow;

  const double D_EPS             = std::numeric_limits<double>::epsilon();
  const double D_LARGE_EPS       = 1000.0 * std::numeric_limits<double>::epsilon();

  const double half_pi           = boost::math::constants::half_pi<double>();
  const double pi                = boost::math::constants::pi<double>();
  const double e                 = boost::math::constants::e<double>();
  const double ln_two            = boost::math::constants::ln_two<double>();
  const double ln_ten            = boost::math::constants::ln_ten<double>();
  const double one_div_root_two  = boost::math::constants::one_div_root_two<double>();
  const double root_two          = boost::math::constants::root_two<double>();
  const double three_quarters_pi = boost::math::constants::three_quarters_pi<double>();
  const double quarter_pi        = boost::math::constants::pi<double>() / 4.0;
  const double one_div_root_pi   = boost::math::constants::one_div_root_pi<double>();
  const double two_div_root_pi   = boost::math::constants::one_div_root_pi<double>() * 2.0;
  const double euler             = boost::math::constants::euler<double>();

  struct Fixture
  {
    Fixture()
      :
      pvMinus(-2.0,4.0),
      pvPlus(2.0,4.0)
    {}

    const VD pvMinus, pvPlus, pvZero;
  };

} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(Test_ValueWithError_MathOverloads,Fixture)

BOOST_AUTO_TEST_SUITE(_abs)

BOOST_AUTO_TEST_CASE(positive)
{
  VD result = abs(pvPlus);
  BOOST_CHECK_EQUAL(result.GetValue(),pvPlus.GetValue());
  BOOST_CHECK_EQUAL(result.GetError(),pvPlus.GetError());
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD result = abs(pvZero);
  BOOST_CHECK_EQUAL(result.GetValue(),pvZero.GetValue());
  BOOST_CHECK_EQUAL(result.GetError(),pvZero.GetError());
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD result = abs(pvMinus);
  BOOST_CHECK_EQUAL(result.GetValue(),std::abs(pvMinus.GetValue()));
  BOOST_CHECK_EQUAL(result.GetError(),pvMinus.GetError());
}

BOOST_AUTO_TEST_SUITE_END() // _abs

BOOST_AUTO_TEST_SUITE(_fabs)

BOOST_AUTO_TEST_CASE(positive)
{
  VD result = fabs(pvPlus);
  BOOST_CHECK_EQUAL(result.GetValue(),pvPlus.GetValue());
  BOOST_CHECK_EQUAL(result.GetError(),pvPlus.GetError());
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD result = fabs(pvZero);
  BOOST_CHECK_EQUAL(result.GetValue(),pvZero.GetValue());
  BOOST_CHECK_EQUAL(result.GetError(),pvZero.GetError());
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD result = fabs(pvMinus);
  BOOST_CHECK_EQUAL(result.GetValue(),std::abs(pvMinus.GetValue()));
  BOOST_CHECK_EQUAL(result.GetError(),pvMinus.GetError());
}
BOOST_AUTO_TEST_SUITE_END() // _fabs

BOOST_AUTO_TEST_SUITE(_sin)

BOOST_AUTO_TEST_CASE(positive)
{
  VD pv(half_pi,1.0);
  VD result = sin(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_SMALL(result.GetError(),D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD pv(0.0,1.0);
  VD result = sin(pv);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD pv(-1.0 * half_pi,1.0);
  VD result = sin(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),-1.0,D_EPS);
  BOOST_CHECK_SMALL(result.GetError(),D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _sin

BOOST_AUTO_TEST_SUITE(_cos)

BOOST_AUTO_TEST_CASE(positive)
{
// using namespace boost::test_tools;
// fraction_tolerance_t<double> tol(D_EPS);
// close_at_tolerance<double> p(tol,FPC_WEAK);
  VD pv(half_pi,1.0);
  VD result = cos(pv);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),pv.GetError(),D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD pv(0.0,1.0);
  VD result = cos(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_SMALL(result.GetError(),D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD pv(-1.0 * half_pi,1.0);
  VD result = cos(pv);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),pv.GetError(),D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _cos

BOOST_AUTO_TEST_SUITE(_exp)

BOOST_AUTO_TEST_CASE(positive)
{
  VD pv(1.0,2.0);
  VD result = exp(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),e,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0 * e,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD pv(0.0,2.0);
  VD result = exp(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD pv(-1.0,2.0);
  VD result = exp(pv);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0 / e,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0 / e,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _exp

BOOST_AUTO_TEST_SUITE(_pow_basePV_expDOUBLE)

BOOST_AUTO_TEST_CASE(positive)
{
  VD base(2.0,2.0);
  double exponent = 3.0;
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),8.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),3.0 * 4.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD base(0.0,2.0);
  double exponent = 3.0;
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),0.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),0.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD base(-2.0,2.0);
  double exponent = 3.0;
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),-8.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),3.0 * 4.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _pow_basePV_expDOUBLE

BOOST_AUTO_TEST_SUITE(_pow_baseDOUBLE_expPV)

BOOST_AUTO_TEST_CASE(positive)
{
  double base = 2.0;
  VD exponent(3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),8.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), ln_two * 8.0 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  double base = 0.0;
  VD exponent(3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),0.0,D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  double base = -2.0;
  VD exponent(3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(), -8.0, D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // _pow_baseDOUBLE_expPV

BOOST_AUTO_TEST_SUITE(_pow_basePV_expPV)

BOOST_AUTO_TEST_CASE(both_positive)
{
  VD base(2.0,1.5);
  VD exponent(3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),8.0,D_EPS);
  double error = detail::hypot(3.0 * 4.0 * 1.5, ln_two * 8.0 * 2.0);
  BOOST_CHECK_CLOSE(result.GetError(),error, D_EPS);
}

BOOST_AUTO_TEST_CASE(both_negative)
{
  VD base(-2.0,1.5);
  VD exponent(-3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),-1.0/8.0,D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(base_zero_exponent_positive)
{
  VD base(0.0,1.5);
  VD exponent(3.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),0.0,D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(base_zero_exponent_zero)
{
  VD base(0.0,1.5);
  VD exponent(0.0,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(base_zero_exponent_negative)
{
  VD base(0.0,1.5);
  VD exponent(-2.5,2.0);
  VD result = pow(base, exponent);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(base_positive_exponent_zero)
{
  VD base(2.0,1.5);
  VD exponent(0.0,3.0);
  VD result = pow(base, exponent);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), ln_two * 1.0 * 3.0, D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _pow_baseDOUBLE_expPV

BOOST_AUTO_TEST_SUITE(_sqrt)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(4.0,2.0);
  VD result = sqrt(val);
  BOOST_CHECK_CLOSE(result.GetValue(),2.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 0.5 / 2.0 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = sqrt(val);
  BOOST_CHECK_CLOSE(result.GetValue(),0.0,D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,1.0);
  VD result = sqrt(val);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_SUITE_END() // _sqrt

BOOST_AUTO_TEST_SUITE(_log)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(2.0,2.0);
  VD result = log(val);
  BOOST_CHECK_CLOSE(result.GetValue(),ln_two,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 0.5 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = log(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-2.0,2.0);
  VD result = log(val);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK_CLOSE(result.GetError(), 0.5 * 2.0, D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _log

BOOST_AUTO_TEST_SUITE(_log10)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(10.0,2.0);
  VD result = log10(val);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 0.2 / ln_ten, D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = log10(val);
  BOOST_CHECK(boost::math::isinf(result.GetValue()));
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-2.0,2.0);
  VD result = log10(val);
  BOOST_CHECK(boost::math::isnan(result.GetValue()));
  BOOST_CHECK_CLOSE(result.GetError(), 0.5 / ln_ten * 2.0, D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _log10

BOOST_AUTO_TEST_SUITE(_acos)

BOOST_AUTO_TEST_CASE(positive_special)
{
  VD val(1.0,2.0);
  VD result = acos(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(one_div_root_two,2.0);
  VD result = acos(val);
  BOOST_CHECK_CLOSE(result.GetValue(),pi / 4.0,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),root_two * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = acos(val);
  BOOST_CHECK_CLOSE(result.GetValue(),half_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-one_div_root_two,2.0);
  VD result = acos(val);
  BOOST_CHECK_CLOSE(result.GetValue(),three_quarters_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),root_two * 2.0,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _acos

BOOST_AUTO_TEST_SUITE(_asin)

BOOST_AUTO_TEST_CASE(positive_special)
{
  VD val(1.0,2.0);
  VD result = asin(val);
  BOOST_CHECK_CLOSE(result.GetValue(),half_pi,D_EPS);
  BOOST_CHECK(boost::math::isinf(result.GetError()));
}

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(one_div_root_two,2.0);
  VD result = asin(val);
  BOOST_CHECK_CLOSE(result.GetValue(),quarter_pi ,D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),root_two * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = asin(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-one_div_root_two,2.0);
  VD result = asin(val);
  BOOST_CHECK_CLOSE(result.GetValue(),-quarter_pi,D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),root_two * 2.0,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _asin

BOOST_AUTO_TEST_SUITE(_atan)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = atan(val);
  BOOST_CHECK_CLOSE(result.GetValue(),quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = atan(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = atan(val);
  BOOST_CHECK_CLOSE(result.GetValue(),-quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _atan

BOOST_AUTO_TEST_SUITE(_atan2_pv_pv)

// error is symmetric in x and y, so we need to test only three cases instead of nine
BOOST_AUTO_TEST_CASE(positive)
{
  VD val1(1.0,2.0);
  VD val2(1.0,4.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),std::sqrt(5.0) ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val1(0.0,1.0);
  VD val2(0.0,2.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val1(-1.0,2.0);
  VD val2(1.0,4.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),-quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),std::sqrt(5.0) ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _atan2_pv_pv

BOOST_AUTO_TEST_SUITE(_atan2_pv_double)

// error is symmetric in x and y, so we need to test only three cases instead of nine
BOOST_AUTO_TEST_CASE(positive)
{
  VD val1(1.0,2.0);
  double val2 = 1.0;
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val1(0.0,1.0);
  double val2 = 0.0;
  VD result = atan2(val1,val2);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val1(-1.0,2.0);
  double val2 = 1.0;
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),-quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _atan2_pv_double

BOOST_AUTO_TEST_SUITE(_atan2_double_pv)

// error is symmetric in x and y, so we need to test only three cases instead of nine
BOOST_AUTO_TEST_CASE(positive)
{
  double val1 = 1.0;
  VD val2(1.0,2.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  double val1 = 0.0;
  VD val2(0.0,2.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK(boost::math::isnan(result.GetError()));
}

BOOST_AUTO_TEST_CASE(negative)
{
  double val1 = -1.0;
  VD val2(1.0,2.0);
  VD result = atan2(val1,val2);
  BOOST_CHECK_CLOSE(result.GetValue(),-quarter_pi,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _atan2_double_pv

BOOST_AUTO_TEST_SUITE(_sinh)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = sinh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),( e - 1.0 / e )/ 2.0 ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),( e + 1.0 / e )/ 2.0 * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = sinh(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = sinh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),-( e - 1.0 / e )/ 2.0 ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),( e + 1.0 / e )/ 2.0 * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _sinh

BOOST_AUTO_TEST_SUITE(_cosh)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = cosh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),( e + 1.0 / e )/ 2.0 ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),( e - 1.0 / e )/ 2.0 * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = cosh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),1.0, D_EPS);
  BOOST_CHECK_SMALL(result.GetError(), D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = cosh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),( e + 1.0 / e )/ 2.0 ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),( e - 1.0 / e )/ 2.0 * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _cosh

BOOST_AUTO_TEST_SUITE(_tanh)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(1.0,2.0);
  VD result = tanh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),( e - 1.0 / e )/ ( e + 1.0 / e ) ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / pow<2>( ( e + 1.0 / e )/ 2.0) * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = tanh(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(),1.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-1.0,2.0);
  VD result = tanh(val);
  BOOST_CHECK_CLOSE(result.GetValue(),-( e - 1.0 / e )/ ( e + 1.0 / e ) ,D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 / pow<2>( ( e + 1.0 / e )/ 2.0) * 2.0 ,D_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _tanh

BOOST_AUTO_TEST_SUITE(_tan)

BOOST_AUTO_TEST_CASE(positive)
{
  VD val(quarter_pi,2.0);
  VD result = tan(val);
  BOOST_CHECK_CLOSE(result.GetValue(), 1.0 ,D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 2.0 * 2.0 ,D_LARGE_EPS);
}

BOOST_AUTO_TEST_CASE(zero)
{
  VD val(0.0,2.0);
  VD result = tan(val);
  BOOST_CHECK_SMALL(result.GetValue(),D_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 1.0 * 2.0,D_EPS);
}

BOOST_AUTO_TEST_CASE(negative)
{
  VD val(-quarter_pi,2.0);
  VD result = tan(val);
  BOOST_CHECK_CLOSE(result.GetValue(), -1.0 ,D_LARGE_EPS);
  BOOST_CHECK_CLOSE(result.GetError(), 2.0 * 2.0 ,D_LARGE_EPS);
}

BOOST_AUTO_TEST_SUITE_END() // _tan

BOOST_AUTO_TEST_SUITE_END() // Test_MathOverloads

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic pop
#endif


// Compute IPM
// Author: Hafez Farazi <farazi@ais.uni-bonn.de>

#pragma once
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <boost/iterator/iterator_concepts.hpp>
#include <vector>
#include <vision_module/Tools/Parameters.hpp>
#include <vision_module/Tools/LineSegment.hpp>
#include <vision_module/Inputs/Camera.hpp>
#include <vision_module/Tools/General.hpp>
using namespace std;
using namespace cv;
/**
* @ingroup VisionModule
*
* @brief For Inverse Perspective Mapping
**/
class IPM
{
public:
	inline IPM(float diagonalAngleView)
	{

		InitIPM( diagonalAngleView);
	}
	virtual ~IPM()
	{

	}
	bool GetHomographyNoYaw(float diagonalAngleView,Point3d cameraLocation, Point3d cameraOrintation,
			Mat &topHomoFor, Mat &topHomoBack,
			double &picYawDegree);
	bool GetHomographyUseYaw(float diagonalAngleView,Point3d cameraLocation, Point3d cameraOrintation,
			 Mat &realHomoFor,Mat &realHomoBack,
		 Point2d outerCornetrsRes[4], Point2d gPoints[4],
			Point3d transformedPoints[4], Point2d physicalCornersRes[4]);
private:
	double ta, tb, tg;
	Point3d GetPointOnZPlain(Point3d point1, Point3d point2, double z = 0);
	Mat GetMultiplyExtrinsicRotationMatrix(Point3d vec, bool inv);
	bool GetPoints(Point3d cameraLocation, Point3d cameraOrintation,
			Point2d gPoints[4], Point2d cPoints[4],
			Point3d transformedPoints[4],bool useYaw);

	bool InitIPM(float diagonalAngleView);

};


///////////////////////////////////////////////////////////////////////////////
// kernel::scalar::crtp.hpp                                                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_CRTP_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_KERNELS_SCALAR_CRTP_HPP_ER_2009
#include <boost/format.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{
namespace scalar{

// Overview: provides common operations for a kernel whose implementation
// is specified by a derived class, D.
//
// Let d note an instance of D, and T == D::result_type
//
// Requirements
// crtp<D,T> is a base of D
// D::result_type is defined
// D::core_profile(const T&)        returns an object of type T
// D::core_nc                       returns an object of type T (normalzing c)
// D d;                             Constructor Forwards to crtp<D,T>
// D d(r)                           Constructor Forwards to crtp<D,T>
template<typename D,typename T>
class crtp{
    public:
    typedef T result_type;

    // Construction
    crtp();
    crtp(const result_type& bandwidth);

    // Evaluate
    template<typename X> result_type profile(const X& x)const;
    template<typename X> result_type operator()(const X& x)const;
    template<typename X,typename X1>
    result_type operator()(const X& x,const X1& x1)const;

    // Access
    result_type bandwidth()const;
    result_type normalizing_constant()const;

    private:
    result_type bandwidth_;
    result_type normalizing_constant_;
    result_type comp_nc(result_type bandwidth);
};

template<typename D,typename T>
std::ostream& operator<<(std::ostream& out,const crtp<D,T>& k){
    const char* str = "(%1%,%2%)";
    format f(str); f%k.bandwidth()%k.normalizing_constant();
    out << f.str();
    return out;
}

// Construction
template<typename D,typename T> crtp<D,T>::crtp()
:bandwidth_(static_cast<result_type>(1)),
normalizing_constant_(this->comp_nc(bandwidth())){}

template<typename D,typename T> crtp<D,T>::crtp(const result_type& bandwidth)
:bandwidth_(bandwidth),
normalizing_constant_(this->comp_nc(this->bandwidth())){}

// Evaluate
template<typename D,typename T>
template<typename X>
typename crtp<D,T>::result_type
crtp<D,T>::profile(const X& x)const{
    result_type u = x / this->bandwidth();
    return D::core_profile(u);
}

template<typename D,typename T>
template<typename X>
typename crtp<D,T>::result_type
crtp<D,T>::operator()(const X& x)const{
    return ( this->profile(x) ) / ( this->normalizing_constant()) ;
}

template<typename D,typename T>
template<typename X,typename X1>
typename crtp<D,T>::result_type
crtp<D,T>::operator()(const X& x,const X1& x1)const{
    return (*this)(x-x1);
}

// Access
template<typename D,typename T>
typename crtp<D,T>::result_type crtp<D,T>::bandwidth()const{
    return this->bandwidth_;
}

template<typename D,typename T>
typename crtp<D,T>::result_type crtp<D,T>::normalizing_constant()const{
    return this->normalizing_constant_;
}

// Implem
template<typename D,typename T>
typename crtp<D,T>::result_type crtp<D,T>::comp_nc(result_type bandwidth){
    return D::core_nc * bandwidth;
}

}// scalar
}// kernel
}// detail
}// statistics
}// boost

#endif