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

/*
Copyright (C) 2003 RiskMap srl
Copyright (C) 2006, 2007 Ferdinando Ametrano
Copyright (C) 2006 Marco Bianchetti
Copyright (C) 2006 Cristina Duminuco
Copyright (C) 2007, 2008 StatPro Italia srl
Copyright (C) 2015 CompatibL

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.
*/

// based on swaption.cpp file from test-suite

#ifndef cl_adjoint_swaption_impl_hpp
#define cl_adjoint_swaption_impl_hpp
#pragma once

#include <ql/quantlib.hpp>
#include "utilities.hpp"
#include "adjointswaptiontest.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <boost/shared_ptr.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointSwaption"

namespace
{
    struct Variation
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "param", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type& operator << (stream_type& stm, Variation& v)
        {
            stm << v.param_
                << ";" << v.swaptionNpv_
                << std::endl;
            return stm;
        }

        Real param_;
        Real swaptionNpv_;
    };

    struct SwaptionData
    {
        SwaptionData()
        : settlementDays_(2)
        , nominal_(1000000.0)
        , fixedConvention_(Unadjusted)
        , fixedFrequency_(Annual)
        , fixedDayCount_(Thirty360())
        , type_(VanillaSwap::Payer)
        {
            index_ = boost::make_shared<Euribor6M>(termStructure_);
            floatingTenor_ = index_->tenor();
            floatingConvention_ = index_->businessDayConvention();
            calendar_ = index_->fixingCalendar();
            today_ = calendar_.adjust(Date::todaysDate());
            Settings::instance().evaluationDate() = today_;
            settlement_ = calendar_.advance(today_, settlementDays_, Days);
            termStructure_.linkTo(flatRate(settlement_, 0.05, Actual365Fixed()));

            exercises_ = { 1 * Years, 3 * Years, 7 * Years, 10 * Years };
            lengths_ = { 1 * Years, 2 * Years, 5 * Years, 10 * Years, 20 * Years };
        }

        // Make swaption with given parameters.
        boost::shared_ptr<Swaption> makeSwaption(const boost::shared_ptr<VanillaSwap>& swap
                                                 , const Date& exercise
                                                 , Volatility volatility
                                                 , Settlement::Type settlementType = Settlement::Physical)
        {
            // Set volatility from simple quote.
            Handle<Quote> vol(boost::make_shared<SimpleQuote>(volatility));

            // Create pricing engine.
            boost::shared_ptr<PricingEngine> engine(new BlackSwaptionEngine(termStructure_, vol));

            // Create swaption using vanilla swap.
            boost::shared_ptr<Swaption> result(new Swaption(swap
                , boost::make_shared<EuropeanExercise>(exercise)
                , settlementType));

            // Set pricing engine in swaption.
            result->setPricingEngine(engine);
            return result;
        }

        // Calculate sum of swaption NPV for given spreads.
        std::vector<Real> swaptionNpvOnSpreads(Real spread)
        {
            std::vector<Real> Y(1);
            for (Size i = 0; i < exercises_.size(); i++)
            {
                for (Size j = 0; j < lengths_.size(); j++)
                {
                    // Set date of exercize.
                    Date exerciseDate = calendar_.advance(today_, exercises_[i]);

                    // Set start date.
                    Date startDate = calendar_.advance(exerciseDate, settlementDays_, Days);

                    // Create vanilla swap.
                    boost::shared_ptr<VanillaSwap> swap =
                        MakeVanillaSwap(lengths_[j], index_, 0.06)
                        .withFixedLegTenor(1 * Years)
                        .withFixedLegDayCount(fixedDayCount_)
                        .withEffectiveDate(startDate)
                        .withFloatingLegSpread(spread)
                        .withType(type_);

                    // Create swaption using vanilla swap.
                    boost::shared_ptr<Swaption> swaption =
                        makeSwaption(swap, exerciseDate, 0.20);

                    Y[0] += swaption->NPV();
                }
            }
            return Y;
        }

        // Calculate sum of swaption NPV for given spreads.
        std::vector<Real> swaptionNpvWithSpreadCorrection(Real spread)
        {
            std::vector<Real> Y(4, 0);

            for (Size i = 0; i < exercises_.size(); i++)
            {
                for (Size j = 0; j < lengths_.size(); j++)
                {
                    // Set date of exercize.
                    Date exerciseDate = calendar_.advance(today_, exercises_[i]);

                    // Set start date.
                    Date startDate = calendar_.advance(exerciseDate, settlementDays_, Days);

                    // Create vanilla swap.
                    boost::shared_ptr<VanillaSwap> swap =
                        MakeVanillaSwap(lengths_[j], index_, 0.06)
                        .withFixedLegTenor(1 * Years)
                        .withFixedLegDayCount(fixedDayCount_)
                        .withEffectiveDate(startDate)
                        .withFloatingLegSpread(spread)
                        .withType(type_);

                    // Calculate spread correction.
                    Spread correction = spread *
                        swap->floatingLegBPS() /
                        swap->fixedLegBPS();

                    // Create equivalent vanilla swap.
                    boost::shared_ptr<VanillaSwap> equivalentSwap =
                        MakeVanillaSwap(lengths_[j], index_, 0.06 + correction)
                        .withFixedLegTenor(1 * Years)
                        .withFixedLegDayCount(fixedDayCount_)
                        .withEffectiveDate(startDate)
                        .withFloatingLegSpread(0.0)
                        .withType(type_);

                    // Create swaption using vanilla swap.
                    boost::shared_ptr<Swaption> swaption1 =
                        makeSwaption(swap, exerciseDate, 0.20);

                    Y[0] += swaption1->NPV();

                    // Create swaption using equivalent vanilla swap.
                    boost::shared_ptr<Swaption> swaption2 =
                        makeSwaption(equivalentSwap, exerciseDate, 0.20);

                    Y[1] += swaption2->NPV();

                    // Create swaption with cash settlement using vanilla swap.
                    boost::shared_ptr<Swaption> swaption1_cash =
                        makeSwaption(swap, exerciseDate, 0.20,
                        Settlement::Cash);

                    Y[2] += swaption1_cash->NPV();

                    // Create swaption with cash settlement using equivalent vanilla swap.
                    boost::shared_ptr<Swaption> swaption2_cash =
                        makeSwaption(equivalentSwap, exerciseDate, 0.20,
                        Settlement::Cash);

                    Y[3] += swaption2_cash->NPV();

                    // Check swaption NPV and equivalent swaption NPV
                    if (std::fabs(swaption1->NPV() - swaption2->NPV()) > 1.0e-6)
                        BOOST_ERROR("wrong spread treatment:" <<
                        "\nexercise: " << exerciseDate <<
                        "\nlength:   " << lengths_[j] <<
                        "\ntype      " << type_ <<
                        "\nspread:   " << io::rate(spread) <<
                        "\noriginal swaption value:   " << swaption1->NPV() <<
                        "\nequivalent swaption value: " << swaption2->NPV());

                    if (std::fabs(swaption1_cash->NPV() - swaption2_cash->NPV()) > 1.0e-6)
                        BOOST_ERROR("wrong spread treatment:" <<
                        "\nexercise date: " << exerciseDate <<
                        "\nlength: " << lengths_[j] <<
                        "\npay " << (type_ ? "fixed" : "floating") <<
                        "\nspread: " << io::rate(spread) <<
                        "\nvalue of original swaption:   " << swaption1_cash->NPV() <<
                        "\nvalue of equivalent swaption: " << swaption2_cash->NPV());

                }
            }
            return Y;
        }

        // Calculate sum of swaption NPV for given volatilities.
        std::vector<Real> swaptionNpvOnCachedValues(Real volatility)
        {
            today_ = Date(13, March, 2002);
            settlement_ = Date(15, March, 2002);
            Settings::instance().evaluationDate() = today_;
            termStructure_.linkTo(flatRate(settlement_, 0.05, Actual365Fixed()));

            // Set date of exercize.
            Date exerciseDate = calendar_.advance(settlement_, 5 * Years);

            // Set start date.
            Date startDate = calendar_.advance(exerciseDate,
                                               settlementDays_, Days);

            // Create vanilla swap.
            boost::shared_ptr<VanillaSwap> swap =
                MakeVanillaSwap(10 * Years, index_, 0.06)
                .withEffectiveDate(startDate)
                .withFixedLegTenor(1 * Years)
                .withFixedLegDayCount(fixedDayCount_);

            std::vector<Real> Y(1);
            // Create swaption using vanilla swap.
            boost::shared_ptr<Swaption> swaption =
                makeSwaption(swap, exerciseDate, volatility);
            Y[0] += swaption->NPV();

            return Y;
        }

        template <class Func>
        void calculateFinDiff(std::vector<Real>& X
                              , Size dep_number
                              , Real h
                              , std::vector<Real>& sf_Finite
                              , Func swaptionNpv)
        {
            Size sizeX = X.size();

            std::vector<Real> swaptionPortfolioNpv(sizeX * dep_number);
            for (int i = 0; i < sizeX; i++)
            {
                std::vector<Real> temp = swaptionNpv(X[i]);
                for (int j = 0; j < dep_number; j++)
                    swaptionPortfolioNpv[sizeX*j + i] = temp[j];
            }

            sf_Finite.resize(swaptionPortfolioNpv.size());

            for (int i = 0; i < sizeX; i++)
            {
                std::vector<Real> temp = swaptionNpv(X[i] + h);
                for (int j = 0; j < dep_number; j++)
                {
                    int curIndex = sizeX*j + i;
                    sf_Finite[curIndex] = (temp[j] - swaptionPortfolioNpv[curIndex]) / h;
                }
            }
        }

        // Global data.
        Date today_;
        Date settlement_;
        Real nominal_;
        Calendar calendar_;

        BusinessDayConvention fixedConvention_;
        Frequency fixedFrequency_;
        DayCounter fixedDayCount_;

        BusinessDayConvention floatingConvention_;
        Period floatingTenor_;
        boost::shared_ptr<IborIndex> index_;

        Natural settlementDays_;
        RelinkableHandle<YieldTermStructure> termStructure_;

        std::vector<Period> exercises_;
        std::vector<Period> lengths_;
        VanillaSwap::Type type_;

        // Cleanup
        SavedSettings backup_;

    };

    struct SpreadDependencyTestData
        : public SwaptionData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, SpreadDependencyTestData* data)
            : size_(size)
            , data_(data)
            , swaptionNpv_(1)
            , spread_(size)
            , iterNumFactor_(1)
            {
                setLogger(&data_->outPerform_);

                Real startSpread = -0.01;
                Real maxSpread = 0.01;
                Real step = (maxSpread - startSpread) / size_;

                for (Size i = 0; i < size_; i++)
                {
                    spread_[i] = (startSpread + i*step == 0) ? 0.00001 : startSpread + i*step;
                }

            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

            void recordTape()
            {
                cl::Independent(spread_);
                calculateTotalSwaptionNpv();
                f_ = std::make_unique<cl::tape_function<double>>(spread_, swaptionNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwaptionNpv()
            {
                for (int i = 0; i < size_; i++)
                    swaptionNpv_[0] += data_->swaptionNpvOnSpreads(spread_[i])[0];
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-10;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFinDiff(spread_
                                        , depVarNumber()
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> std::vector<Real>
                {
                    return data_->swaptionNpvOnSpreads(v);
                });
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-10; }

            Size size_;
            Size iterNumFactor_;
            SpreadDependencyTestData* data_;
            std::vector<cl::tape_double> spread_;
            std::vector<cl::tape_double> swaptionNpv_;
        };

        SpreadDependencyTestData()
            : SwaptionData()

            , outPerform_(OUTPUT_FOLDER_NAME "//SpreadDependency"
            , { { "filename", "AdjointPerformance" }
        , { "not_clear", "Not" }
        , { "line_box_width", "-5" }
        , { "title", "Swaption NPV differentiation performance with respect to spread" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of spreads" }
        , { "smooth", "default" }
        , { "cleanlog", "true" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME "//SpreadDependency"
            , { { "filename", "Adjoint" }
        , { "not_clear", "Not" }
        , { "smooth", "default" }
        , { "title", "Swaption NPV adjoint differentiation performance with respect to spread" }
        , { "cleanlog", "false" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of spreads" } })

            , outSize_(OUTPUT_FOLDER_NAME "//SpreadDependency"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "title", "Tape size dependence on number of spreads" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "ylabel", "Size (MB)" }
        , { "xlabel", "Number of spreads" } })

            , out_(OUTPUT_FOLDER_NAME "//SpreadDependency//output"
            , { { "filename", "SwaptNPVonSpreads" }
        , { "ylabel", "Swaption NPV" }
        , { "not_clear", "Not" }
        , { "title", "Swaption NPV dependence on spread" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "xlabel", "Spread" } })

#if defined CL_GRAPH_GEN
            , pointNo_(50)
            , iterNo_(10)
            , step_(5)
#else
            , pointNo_(1)
            , iterNo_(1)
            , step_(1)
#endif
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo_ > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo_, step_);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<Variation> outData(pointNo_);
            auto test = getTest(pointNo_);
            for (Size i = 0; i < pointNo_; i++)
            {
                outData[i].param_ = test->spread_[i];
                outData[i].swaptionNpv_ = test->data_->swaptionNpvOnSpreads(test->spread_[i])[0];
            }
            out_ << outData;
            return true;
        }

        Size pointNo_;
        Size iterNo_;
        Size step_;

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
    typedef SpreadDependencyTestData::Test SpreadDependencyTest;

    struct SpreadCorrectionTestData
        : public SwaptionData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            static const CalcMethod default_method = other;

            Test(Size size, SpreadCorrectionTestData* data)
                : size_(size)
                , data_(data)
                , spread_(size)
                , doubleSpread_(size)
                , iterNumFactor_(1)
            {
                setLogger(&data_->outPerform_);

                double startSpread = -0.01;
                double maxSpread = 0.01;
                double step = (maxSpread - startSpread) / size_;

                for (Size i = 0; i < size_; i++)
                {
                    doubleSpread_[i] = (startSpread + i*step == 0) ? 0.00001 : startSpread + i*step;
                }

                std::copy(doubleSpread_.begin(), doubleSpread_.end(), spread_.begin());
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 4; }

            Size minPerfIteration() { return iterNumFactor_; }

            void recordTape()
            {
                cl::Independent(spread_);
                calculateTotalSwaptionNpv();
                f_ = std::make_unique<cl::tape_function<double>>(spread_, swaptionNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwaptionNpv()
            {
                swaptionNpv_.resize(depVarNumber());
                for (int i = 0; i < size_; i++)
                {
                    std::vector<cl::tape_double> temp = data_->swaptionNpvWithSpreadCorrection(spread_[i]);
                    for (int j = 0; j < depVarNumber(); j++)
                        swaptionNpv_[j] += temp[j];
                }
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-10;  // shift for finite diff. method
                analyticalResults_.resize(size_ * depVarNumber());
                data_->calculateFinDiff(spread_
                                        , depVarNumber()
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> std::vector<Real>
                {
                    return data_->swaptionNpvWithSpreadCorrection(v);
                });
            }

            // Calculates derivatives using adjoint.
            void calcAdjoint()
            {
                adjointResults_ = f_->Jacobian(doubleSpread_);
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-10; }

            Size size_;
            Size iterNumFactor_;
            SpreadCorrectionTestData* data_;
            std::vector<double> doubleSpread_;
            std::vector<cl::tape_double> spread_;
            std::vector<cl::tape_double> swaptionNpv_;
        };

        SpreadCorrectionTestData()
            : SwaptionData()

            , outPerform_(OUTPUT_FOLDER_NAME "//SpreadCorrection"
            , { { "filename", "AdjointPerformance" }
        , { "not_clear", "Not" }
        , { "line_box_width", "-5" }
        , { "title", "Swaption NPV differentiation performance with respect to spread" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of spreads" }
        , { "smooth", "default" }
        , { "cleanlog", "true" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME "//SpreadCorrection"
            , { { "filename", "Adjoint" }
        , { "not_clear", "Not" }
        , { "smooth", "default" }
        , { "title", "Swaption NPV adjoint differentiation performance with respect to spread" }
        , { "cleanlog", "false" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of spreads" } })

            , outSize_(OUTPUT_FOLDER_NAME "//SpreadCorrection"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "title", "Tape size dependence on number of spreads" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "ylabel", "Size (MB)" }
        , { "xlabel", "Number of spreads" } })

            , out_(OUTPUT_FOLDER_NAME "//SpreadCorrection//output"
            , { { "filename", "SwaptNPVonSpreads" }
        , { "ylabel", "Swaption NPV" }
        , { "not_clear", "Not" }
        , { "title", "Swaption NPV dependence on spread" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "xlabel", "Spread" } })

#if defined CL_GRAPH_GEN
            , pointNo_(20)
            , iterNo_(20)
            , step_(1)
#else
            , pointNo_(1)
            , iterNo_(1)
            , step_(1)
#endif
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo_ > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo_, step_);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<Variation> outData(pointNo_);
            auto test = getTest(pointNo_);
            for (Size i = 0; i < pointNo_; i++)
            {
                outData[i].param_ = test->spread_[i];
                outData[i].swaptionNpv_ = test->data_->swaptionNpvWithSpreadCorrection(test->spread_[i])[0];
            }
            out_ << outData;
            return true;
        }

        Size pointNo_;
        Size iterNo_;
        Size step_;

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
    typedef SpreadCorrectionTestData::Test SpreadCorrectionTest;

    struct CachedValueTestData
        : public SwaptionData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {

            Test(Size size, CachedValueTestData* data)
            : size_(size)
            , data_(data)
            , volatility_(size)
            , swaptionNpv_(1)
            , iterNumFactor_(1)
            {
                setLogger(&data_->outPerform_);

                Real startVol = 0.15;
                Real maxVol = 0.25;
                Real step = (maxVol - startVol) / size_;

                for (Size i = 0; i < size_; i++)
                {
                    volatility_[i] = startVol + i*step;
                }

            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

            void recordTape()
            {
                cl::Independent(volatility_);
                calculateTotalSwaptionNpv();
                f_ = std::make_unique<cl::tape_function<double>>(volatility_, swaptionNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwaptionNpv()
            {
                for (int i = 0; i < size_; i++)
                    swaptionNpv_[0] += data_->swaptionNpvOnCachedValues(volatility_[i])[0];
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-10;  // shift for finite diff. method
                analyticalResults_.resize(size_ * depVarNumber());
                data_->calculateFinDiff(volatility_
                                        , depVarNumber()
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> std::vector<Real>
                {
                    return data_->swaptionNpvOnCachedValues(v);
                });
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-10; }

            Size size_;
            Size iterNumFactor_;
            CachedValueTestData* data_;
            std::vector<cl::tape_double> volatility_;
            std::vector<cl::tape_double> swaptionNpv_;
        };

        CachedValueTestData()
            : SwaptionData()

            , outPerform_(OUTPUT_FOLDER_NAME "//CachedValue"
            , { { "filename", "AdjointPerformance" }
        , { "not_clear", "Not" }
        , { "line_box_width", "-5" }
        , { "title", "Swaption NPV differentiation performance with respect to volatility" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of volatilities" }
        , { "smooth", "default" }
        , { "cleanlog", "true" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME "//CachedValue"
            , { { "filename", "Adjoint" }
        , { "not_clear", "Not" }
        , { "smooth", "default" }
        , { "title", "Swaption NPV adjoint differentiation performance with respect to volatility" }
        , { "cleanlog", "false" }
        , { "ylabel", "Time (s)" }
        , { "xlabel", "Number of volatilities" } })

            , outSize_(OUTPUT_FOLDER_NAME "//CachedValue"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "title", "Tape size dependence on number of volatilities" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "ylabel", "Size (MB)" }
        , { "xlabel", "Number of volatilities" } })

            , out_(OUTPUT_FOLDER_NAME "//CachedValue//output"
            , { { "filename", "SwaptNPVonVol" }
        , { "ylabel", "Swaption NPV" }
        , { "not_clear", "Not" }
        , { "title", "Swaption NPV dependence on volatilities" }
        , { "cleanlog", "false" }
        , { "smooth", "default" }
        , { "xlabel", "Volatility" } })

#if defined CL_GRAPH_GEN
            , pointNo_(400)
            , iterNo_(16)
            , step_(25)
#else
            , pointNo_(1)
            , iterNo_(1)
            , step_(1)
#endif
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo_ > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo_, step_);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<Variation> outData(pointNo_);
            auto test = getTest(pointNo_);
            for (Size i = 0; i < pointNo_; i++)
            {
                outData[i].param_ = test->volatility_[i];
                outData[i].swaptionNpv_ = test->data_->swaptionNpvOnCachedValues(test->volatility_[i])[0];
            }
            out_ << outData;
            return true;
        }

        Size pointNo_;
        Size iterNo_;
        Size step_;

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };
    typedef CachedValueTestData::Test CachedValueTest;
}

#endif


/**
 *           c++11-only implementation of the L-BFGS-B algorithm
 *
 * Copyright (c) 2014 Patrick Wieschollek
 *               https://github.com/PatWie/LBFGSB
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef LBFGSB_H_
#define LBFGSB_H_

#include "meta.h"

#include <list>
#include <stdio.h>
#include <iostream>
#include <functional>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

/* coded from scratch !!!
 * based on the paper
 * A LIMITED MEMORY ALGORITHM FOR BOUND CONSTRAINED OPTIMIZATION
 * (Byrd, Lu, Nocedal, Zhu)
 */

namespace PatWieLBFGSB {

class LBFGSB {

	// contains options for optimization process
	Options Options_;

	// oracles for function value and gradient
	FunctionOracleType FunctionObjectiveOracle_;
	GradientOracleType FunctionGradientOracle_;

	Matrix W, M;
	Vector lb, ub;
	double theta;
	int DIM;

	std::list<Vector> xHistory;

public:

	Vector XOpt;

	LBFGSB(const Vector &l, const Vector &u) :
			lb(l), ub(u), theta(1.0), DIM(l.rows()) {
		lb = l;
		ub = u;
		theta = 1.0;
		DIM = l.rows();
		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);
	}

	LBFGSB(Options &Options, const Vector &l, const Vector &u) {
		Options_ = Options;
		lb = l;
		ub = u;
		theta = 1.0;
		DIM = l.rows();
		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);

	}

	/// <summary>
	/// find cauchy point in x
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	void GetGeneralizedCauchyPoint(Vector &x, Vector &g, Vector &x_cauchy,
			Vector &c) {
		const int DIM = x.rows();
		// PAGE 8
		// Algorithm CP: Computation of the generalized Cauchy point
		// Given x,l,u,g, and B = \theta I-WMW

		// {all t_i} = { (idx,value), ... }
		// TODO: use "std::set" ?
		std::vector<std::pair<int, double> > SetOfT;
		// the feasible set is implicitly given by "SetOfT - {t_i==0}"
		Vector d = Vector::Zero(DIM, 1);

		// n operations
		for (int j = 0; j < DIM; j++) {
			if (g(j) == 0) {
				SetOfT.push_back(std::make_pair(j, INF));
			} else {
				double tmp = 0;
				if (g(j) < 0) {
					tmp = (x(j) - ub(j)) / g(j);
				} else {
					tmp = (x(j) - lb(j)) / g(j);
				}
				d(j) = -g(j);
				SetOfT.push_back(std::make_pair(j, tmp));
			}

		}Debug(d.transpose());

		// paper: using heapsort
		// sortedindices [1,0,2] means the minimal element is on the 1th entry
		std::vector<int> SortedIndices = sort_indexes(SetOfT);

		x_cauchy = x;
		// Initialize
		// p := 	W^T*p
		Vector p = (W.transpose() * d);						// (2mn operations)
		// c := 	0
		c = Eigen::MatrixXd::Zero(M.rows(), 1);
		// f' := 	g^T*d = -d^Td
		double f_prime = -d.dot(d);							// (n operations)
		// f'' :=	\theta*d^T*d-d^T*W*M*W^T*d = -\theta*f' - p^T*M*p
		double f_doubleprime = (double) (-1.0 * theta) * f_prime - p.dot(M * p);// (O(m^2) operations)
		// \delta t_min :=	-f'/f''
		double dt_min = -f_prime / f_doubleprime;
		// t_old := 	0
		double t_old = 0;
		// b := 	argmin {t_i , t_i >0}
		int i = 0;
		for (int j = 0; j < DIM; j++) {
			i = j;
			if (SetOfT[SortedIndices[j]].second != 0)
				break;
		}
		int b = SortedIndices[i];
		// see below
		// t        			:= 	min{t_i : i in F}
		double t = SetOfT[b].second;
		// \delta t 			:= 	t - 0
		double dt = t - t_old;

		// examination of subsequent segments
		while ((dt_min >= dt) && (i < DIM)) {
			if (d(b) > 0)
				x_cauchy(b) = ub(b);
			else if (d(b) < 0)
				x_cauchy(b) = lb(b);

			// z_b = x_p^{cp} - x_b
			double zb = x_cauchy(b) - x(b);
			// c   :=  c +\delta t*p
			c += dt * p;
			// cache
			Vector wbt = W.row(b);

			f_prime += dt * f_doubleprime + (double) g(b) * g(b)
					+ (double) theta * g(b) * zb
					- (double) g(b) * wbt.transpose() * (M * c);
			f_doubleprime += (double) -1.0 * theta * g(b) * g(b)
					- (double) 2.0 * (g(b) * (wbt.dot(M * p)))
					- (double) g(b) * g(b) * wbt.transpose() * (M * wbt);
			p += g(b) * wbt.transpose();
			d(b) = 0;
			dt_min = -f_prime / f_doubleprime;
			t_old = t;
			++i;
			if (i < DIM) {
				b = SortedIndices[i];
				t = SetOfT[b].second;
				dt = t - t_old;
			}

		}

		dt_min = fmax(dt_min, 0.0);
		t_old += dt_min;

		Debug(SortedIndices[0]<< " "<< SortedIndices[1]);

#pragma omp parallel for
		for (int ii = i; ii < x_cauchy.rows(); ii++) {
			x_cauchy(SortedIndices[ii]) = x(SortedIndices[ii])
					+ t_old * d(SortedIndices[ii]);
		}Debug(x_cauchy.transpose());

		c += dt_min * p;
		Debug(c.transpose());

	}

	/// <summary>
	/// find valid alpha for (8.5)
	/// </summary>
	/// <parameter name="x_cp">cauchy point</parameter>
	/// <parameter name="du">unconstrained solution of subspace minimization</parameter>
	/// <parameter name="FreeVariables">flag (1 if is free variable and 0 if is not free variable)</parameter>
	double FindAlpha(Vector &x_cp, Vector &du,
			std::vector<int> &FreeVariables) {
		/* this returns
		 * a* = max {a : a <= 1 and  l_i-xc_i <= a*d_i <= u_i-xc_i}
		 */
		double alphastar = 1;
		const unsigned int n = FreeVariables.size();
		for (unsigned int i = 0; i < n; i++) {
			if (du(i) > 0) {
				alphastar = fmin(alphastar,
						(ub(FreeVariables[i]) - x_cp(FreeVariables[i]))
								/ du(i));
			} else {
				alphastar = fmin(alphastar,
						(lb(FreeVariables[i]) - x_cp(FreeVariables[i]))
								/ du(i));
			}
		}
		return alphastar;
	}

	/// <summary>
	/// using linesearch to determine step width
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	/// <parameter name="dx">direction</parameter>
	/// <parameter name="f">current value of objective (will be changed)</parameter>
	/// <parameter name="g">current gradient of objective (will be changed)</parameter>
	/// <parameter name="t">step width (will be changed)</parameter>
	void LineSearch(Vector &x, Vector dx, double &f, Vector &g, double &t) {

		const double alpha = 0.2;
		const double beta = 0.8;

		const double f_in = f;
		const Vector g_in = g;
		const double Cache = alpha * g_in.dot(dx);

		t = 1.0;
		f = FunctionObjectiveOracle_(x + t * dx);
		while (f > f_in + t * Cache) {
			t *= beta;
			f = FunctionObjectiveOracle_(x + t * dx);
		}
		FunctionGradientOracle_(x + t * dx, g);
		x += t * dx;

	}

	/// <summary>
	/// direct primal approach
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	void SubspaceMinimization(Vector &x_cauchy, Vector &x, Vector &c, Vector &g,
			Vector &SubspaceMin) {

		// cached value: ThetaInverse=1/theta;
		double theta_inverse = 1 / theta;

		// size of "t"
		std::vector<int> FreeVariablesIndex;
		Debug(x_cauchy.transpose());

		//std::cout << "free vars " << FreeVariables.rows() << std::endl;
		for (int i = 0; i < x_cauchy.rows(); i++) {
			Debug(x_cauchy(i) << " "<< ub(i) << " "<< lb(i));
			if ((x_cauchy(i) != ub(i)) && (x_cauchy(i) != lb(i))) {
				FreeVariablesIndex.push_back(i);
			}
		}
		const int FreeVarCount = FreeVariablesIndex.size();

		Matrix WZ = Matrix::Zero(W.cols(), FreeVarCount);

		for (int i = 0; i < FreeVarCount; i++)
			WZ.col(i) = W.row(FreeVariablesIndex[i]);

		Debug(WZ);

		// r=(g+theta*(x_cauchy-x)-W*(M*c));
		Debug(g);Debug(x_cauchy);Debug(x);
		Vector rr = (g + theta * (x_cauchy - x) - W * (M * c));
		// r=r(FreeVariables);
		Vector r = Matrix::Zero(FreeVarCount, 1);
		for (int i = 0; i < FreeVarCount; i++)
			r.row(i) = rr.row(FreeVariablesIndex[i]);

		Debug(r.transpose());

		// STEP 2: "v = w^T*Z*r" and STEP 3: "v = M*v"
		Vector v = M * (WZ * r);
		// STEP 4: N = 1/theta*W^T*Z*(W^T*Z)^T
		Matrix N = theta_inverse * WZ * WZ.transpose();
		// N = I - MN
		N = Matrix::Identity(N.rows(), N.rows()) - M * N;
		// STEP: 5
		// v = N^{-1}*v
		v = N.lu().solve(v);
		// STEP: 6
		// HERE IS A MISTAKE IN THE ORIGINAL PAPER!
		Vector du = -theta_inverse * r
				- theta_inverse * theta_inverse * WZ.transpose() * v;
		Debug(du.transpose());
		// STEP: 7
		double alpha_star = FindAlpha(x_cauchy, du, FreeVariablesIndex);

		// STEP: 8
		Vector dStar = alpha_star * du;

		SubspaceMin = x_cauchy;
		for (int i = 0; i < FreeVarCount; i++) {
			SubspaceMin(FreeVariablesIndex[i]) = SubspaceMin(
					FreeVariablesIndex[i]) + dStar(i);
		}
	}

	void Solve(Vector &x0, const FunctionOracleType& FunctionValue,
			const GradientOracleType& FunctionGradient) {
		FunctionObjectiveOracle_ = FunctionValue;
		FunctionGradientOracle_ = FunctionGradient;

		Assert(x0.rows() == lb.rows(), "lower bound size incorrect");
		Assert(x0.rows() == ub.rows(), "upper bound size incorrect");

		Debug(x0.transpose());Debug(lb.transpose());Debug(ub.transpose());

		Assert((x0.array() >= lb.array()).all(),
				"seed is not feasible (violates lower bound)");
		Assert((x0.array() <= ub.array()).all(),
				"seed is not feasible (violates upper bound)");

		const int DIM = x0.rows();

		xHistory.push_back(x0);

		Matrix yHistory = Matrix::Zero(DIM, 0);
		Matrix sHistory = Matrix::Zero(DIM, 0);

		Vector x = x0, g;
		int k = 0;

		double f = FunctionObjectiveOracle_(x);
		FunctionGradientOracle_(x, g);
		Debug(f);Debug(g.transpose());

		theta = 1.0;

		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);

		auto noConvergence =
				[&](Vector& x, Vector& g)->bool {
					return (((x - g).cwiseMax(lb).cwiseMin(ub) - x).lpNorm<Eigen::Infinity>()>= Options_.tol);
				};

		while (noConvergence(x, g) && (k < Options_.maxIter)) {
			Debug("iteration "<<k)
			double f_old = f;
			Vector x_old = x;
			Vector g_old = g;

			// STEP 2: compute the cauchy point by algorithm CP
			Vector CauchyPoint = Matrix::Zero(DIM, 1), c = Matrix::Zero(DIM, 1);
			GetGeneralizedCauchyPoint(x, g, CauchyPoint, c);
			// STEP 3: compute a search direction d_k by the primal method
			Vector SubspaceMin;
			SubspaceMinimization(CauchyPoint, x, c, g, SubspaceMin);

			Matrix H;
			double Length = 0;

			// STEP 4: perform linesearch and STEP 5: compute gradient
			LineSearch(x, SubspaceMin - x, f, g, Length);

			xHistory.push_back(x);

			// prepare for next iteration
			Vector newY = g - g_old;
			Vector newS = x - x_old;

			// STEP 6:
			double test = newS.dot(newY);
			test = (test < 0) ? -1.0 * test : test;

			if (test > EPS * newY.squaredNorm()) {
				if (k < Options_.m) {
					yHistory.conservativeResize(DIM, k + 1);
					sHistory.conservativeResize(DIM, k + 1);
				} else {

					yHistory.leftCols(Options_.m - 1) = yHistory.rightCols(
							Options_.m - 1).eval();
					sHistory.leftCols(Options_.m - 1) = sHistory.rightCols(
							Options_.m - 1).eval();
				}
				yHistory.rightCols(1) = newY;
				sHistory.rightCols(1) = newS;

				// STEP 7:
				theta = (double) (newY.transpose() * newY)
						/ (newY.transpose() * newS);

				W = Matrix::Zero(yHistory.rows(),
						yHistory.cols() + sHistory.cols());

				W << yHistory, (theta * sHistory);

				Matrix A = sHistory.transpose() * yHistory;
				Matrix L = A.triangularView<Eigen::StrictlyLower>();
				Matrix MM(A.rows() + L.rows(), A.rows() + L.cols());
				Matrix D = -1 * A.diagonal().asDiagonal();
				MM << D, L.transpose(), L, ((sHistory.transpose() * sHistory)
						* theta);

				M = MM.inverse();
			}

			Vector ttt = Matrix::Zero(1, 1);
			ttt(0) = f_old - f;
			Debug( "--> "<< ttt.norm());
			if (ttt.norm() < Options_.tol) {
				// successive function values too similar
				break;
			}
			k++;

		}

		XOpt = x;
		x0 = x;

	}
};

}

#endif /* LBFGSB_H_ */


#include <polyvec/api.hpp>

#include <polyvec/mc/get_bounding_box.hpp>

#include <Eigen/Geometry>

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(mc)

Eigen::AlignedBox<int, 2>
get_bounding_box(const std::vector<Eigen::Matrix2Xi>& regions) {
   Eigen::AlignedBox<int, 2> bbox;

    for ( int i = 0; i < ( int ) regions.size(); ++i ) {
        for ( int j = 0; j < ( int ) regions[i].cols(); ++j ) {
            bbox.extend ( regions[i].col ( j ) );
        }
    }
    return bbox;
}

NAMESPACE_END(mc)
NAMESPACE_END(polyfit)


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <fstream>
#include <iostream>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    no_property, property < edge_weight_t, int > > Graph;
  typedef graph_traits < Graph >::edge_descriptor Edge;
  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  typedef std::pair<int, int> E;

  const int num_nodes = 5;
  E edge_array[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
    E(3, 4), E(4, 0), E(4, 1)
  };
  int weights[] = { 1, 1, 2, 7, 3, 1, 1, 1 };
  int num_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  for (std::size_t j = 0; j < num_edges; ++j) {
    Edge e; bool inserted;
    tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edge_array, edge_array + num_edges, weights, num_nodes);
#endif
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  std::vector < Edge > spanning_tree;

  kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

  std::cout << "Print the edges in the MST:" << std::endl;
  for (std::vector < Edge >::iterator ei = spanning_tree.begin();
       ei != spanning_tree.end(); ++ei) {
    std::cout << source(*ei, g) << " <--> " << target(*ei, g)
      << " with weight of " << weight[*ei]
      << std::endl;
  }

  std::ofstream fout("figs/kruskal-eg.dot");
  fout << "graph A {\n"
    << " rankdir=LR\n"
    << " size=\"3,3\"\n"
    << " ratio=\"filled\"\n"
    << " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
  graph_traits<Graph>::edge_iterator eiter, eiter_end;
  for (tie(eiter, eiter_end) = edges(g); eiter != eiter_end; ++eiter) {
    fout << source(*eiter, g) << " -- " << target(*eiter, g);
    if (std::find(spanning_tree.begin(), spanning_tree.end(), *eiter)
        != spanning_tree.end())
      fout << "[color=\"black\"]\n";
    else
      fout << "[color=\"gray\"]\n";
  }
  fout << "}\n";
  return EXIT_SUCCESS;
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/foreach.hpp>

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

#include <boost/geometry/algorithms/detail/overlay/get_turn_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_relative_order.hpp>

#include <boost/geometry/geometries/point_xy.hpp>

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/extensions/io/svg/svg_mapper.hpp>
#endif

template <typename P, typename T>
void test_with_point(std::string const& caseid,
                T pi_x, T pi_y, T pj_x, T pj_y,
                T ri_x, T ri_y, T rj_x, T rj_y,
                T si_x, T si_y, T sj_x, T sj_y,
                int expected_order)
{
    P pi = bg::make<P>(pi_x, pi_y);
    P pj = bg::make<P>(pj_x, pj_y);
    P ri = bg::make<P>(ri_x, ri_y);
    P rj = bg::make<P>(rj_x, rj_y);
    P si = bg::make<P>(si_x, si_y);
    P sj = bg::make<P>(sj_x, sj_y);

    int order = bg::detail::overlay::get_relative_order<P>::apply(pi, pj, ri, rj, si, sj);

    BOOST_CHECK_EQUAL(order, expected_order);

    /*
    std::cout << caseid
        << (caseid.find("_") == std::string::npos ? "  " : "")
        << " " << method
        << " " << detected
        << " " << order
        << std::endl;
    */

/*#if defined(TEST_WITH_SVG)
    {
        std::ostringstream filename;
        filename << "get_turn_info_" << caseid
            << "_" << string_from_type<typename bg::coordinate_type<P>::type>::name()
            << ".svg";

        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<P> mapper(svg, 500, 500);
        mapper.add(bg::make<P>(0, 0));
        mapper.add(bg::make<P>(10, 10));

        bg::model::linestring<P> p; p.push_back(pi); p.push_back(pj); p.push_back(pk);
        bg::model::linestring<P> q; q.push_back(qi); q.push_back(qj); q.push_back(qk);
        mapper.map(p, "opacity:0.8;stroke:rgb(0,192,0);stroke-width:3");
        mapper.map(q, "opacity:0.8;stroke:rgb(0,0,255);stroke-width:3");

        std::string style =  ";font-family='Verdana';font-weight:bold";
        std::string align = ";text-anchor:end;text-align:end";
        int offset = 8;

        mapper.text(pi, "pi", "fill:rgb(0,192,0)" + style, offset, offset);
        mapper.text(pj, "pj", "fill:rgb(0,192,0)" + style, offset, offset);
        mapper.text(pk, "pk", "fill:rgb(0,192,0)" + style, offset, offset);

        mapper.text(qi, "qi", "fill:rgb(0,0,255)" + style + align, -offset, offset);
        mapper.text(qj, "qj", "fill:rgb(0,0,255)" + style + align, -offset, offset);
        mapper.text(qk, "qk", "fill:rgb(0,0,255)" + style + align, -offset, offset);

        int factor = 1; // second info, if any, will go left by factor -1
        int ch = '1';
        for (typename tp_vector::const_iterator it = info.begin();
            it != info.end();
            ++it, factor *= -1, ch++)
        {
            bool at_j = it->method == bg::detail::overlay::method_crosses;
            std::string op;
            op += operation_char(it->operations[0].operation);
            align = ";text-anchor:middle;text-align:center";
            mapper.text(at_j ? pj : pk, op, "fill:rgb(255,128,0)" + style + align, offset * factor, -offset);

            op.clear();
            op += operation_char(it->operations[1].operation);
            mapper.text(at_j ? qj : qk, op, "fill:rgb(255,128,0)" + style + align, offset * factor, -offset);

            // Map intersection point + method
            mapper.map(it->point, "opacity:0.8;fill:rgb(255,0,0);stroke:rgb(0,0,100);stroke-width:1");

            op.clear();
            op += method_char(it->method);
            if (info.size() != 1)
            {
                op += ch;
                op += " p:"; op += operation_char(it->operations[0].operation);
                op += " q:"; op += operation_char(it->operations[1].operation);
            }
            mapper.text(it->point, op, "fill:rgb(255,0,0)" + style, offset, -offset);
        }
    }
#endif
*/
}

template <typename P>
void test_all()
{
    test_with_point<P, double>("OLR1",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            3, 3,   7, 2, // s
            1);
    test_with_point<P, double>("OLR2",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            3, 7,   7, 6, // s
            -1);
    test_with_point<P, double>("OLR3",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            4, 2,   9, 6, // s
            1);
    test_with_point<P, double>("OLR4",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            3, 8,   9, 4, // s
            -1);
    test_with_point<P, double>("OLR5",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            4, 2,   8, 6, // s
            1);
    test_with_point<P, double>("OLR6",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            3, 7,   9, 4, // s
            -1);
    test_with_point<P, double>("OLR7",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            1, 4,   7, 7, // s
            -1);
    test_with_point<P, double>("OLR8",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            1, 6,   7, 3, // s
            1);

    test_with_point<P, double>("OD1",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            7, 2,   3, 3, // s
            1);

    test_with_point<P, double>("OD9",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            7, 5,   3, 3, // s
            1);
    test_with_point<P, double>("OD10",
            5, 1,   5, 8, // p
            3, 5,   7, 5, // r
            7, 5,   3, 7, // s
            -1);
    test_with_point<P, double>("OD11",
            5, 1,   5, 8, // p
            7, 5,   3, 5, // r
            3, 5,   7, 7, // s
            -1);
    test_with_point<P, double>("OD12",
            5, 1,   5, 8, // p
            7, 5,   3, 5, // r
            3, 5,   7, 3, // s
            1);
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();
    return 0;
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2021 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/detail/overlay/get_ring.hpp>

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

#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

namespace
{

bool g_debug = false;

std::string const simplex = "POLYGON((0 2,1 2,1 1,0 1,0 2))";
std::string const case_a = "POLYGON((1 0,0 5,5 2,1 0),(2 1,3 2,1 3,2 1))";
std::string const multi = "MULTIPOLYGON(((0 0,0 5,5 5,5 0,0 0),(1 1,2 1,2 2,1 2,1 1),(3 3,4 3,4 4,3 4,3 3)),((6 6,6 10,10 10,10 6,6 6),(7 7,8 7,8 8,7 7)))";
}

template <typename Geometry>
void test_get_ring(std::string const& case_id, std::string const& wkt,
                   bg::ring_identifier const& ring_id,
                   std::string const& expected_wkt)
{
    using tag = typename bg::tag<Geometry>::type;

    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    auto const ring = bg::detail::overlay::get_ring<tag>::apply(ring_id, geometry);

    if (g_debug)
    {
        std::cout << case_id
                  << " valid: " << bg::is_valid(geometry)
                  << " area: " << bg::area(geometry)
                  << " area(ring): " << bg::area(ring)
                  << " wkt(ring): " << bg::wkt(ring)
                  << std::endl;
    }

    std::ostringstream out;
    out << bg::wkt(ring);
    std::string const detected = out.str();
    BOOST_CHECK_MESSAGE(detected == expected_wkt, "get_ring: " << case_id
                        << " expected: " << expected_wkt
                        << " detected: " << detected);
}

template <typename Geometry>
void test_segment_count_on_ring(std::string const& case_id, std::string const& wkt,
                   bg::ring_identifier const& ring_id, bg::signed_size_type expected_count)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    auto const detected_count = bg::detail::overlay::segment_count_on_ring(geometry, ring_id);

    BOOST_CHECK_MESSAGE(detected_count == expected_count,
                        "test_get_ring: " << case_id
                        << " expected: " << expected_count
                        << " detected: " << detected_count);
}

template <typename Geometry>
void test_segment_distance(std::string const& case_id, int line, std::string const& wkt,
                           bg::segment_identifier const& id1,
                           bg::segment_identifier const& id2,
                           bg::signed_size_type expected_distance)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);

    auto const detected_distance = bg::detail::overlay::segment_distance(geometry, id1, id2);

    BOOST_CHECK_MESSAGE(detected_distance == expected_distance,
                        "segment_distance: " << case_id << " (" << line << ")"
                        << " expected: " << expected_distance
                        << " detected: " << detected_distance);
}

template <typename Point, bool Closed>
void test_get_ring()
{
    using ring = bg::model::ring<Point, Closed>;
    using polygon = bg::model::polygon<Point, Closed>;
    using multi_polygon = bg::model::multi_polygon<polygon>;

    test_get_ring<ring>("ring_simplex", simplex, {0, -1, -1}, simplex);
    test_get_ring<polygon>("polygon_simplex", simplex, {0, -1, -1}, simplex);
    test_get_ring<polygon>("case_a_outer", case_a, {0, -1, -1}, "POLYGON((1 0,0 5,5 2,1 0))");
    test_get_ring<polygon>("case_a_0", case_a, {0, -1, 0}, "POLYGON((2 1,3 2,1 3,2 1))");
    test_get_ring<multi_polygon>("multi_0_outer", multi, {0, 0, -1}, "POLYGON((0 0,0 5,5 5,5 0,0 0))");
    test_get_ring<multi_polygon>("multi_0_0", multi, {0, 0, 0}, "POLYGON((1 1,2 1,2 2,1 2,1 1))");
    test_get_ring<multi_polygon>("multi_0_1", multi, {0, 0, 1}, "POLYGON((3 3,4 3,4 4,3 4,3 3))");
    test_get_ring<multi_polygon>("multi_1_outer", multi, {0, 1, -1}, "POLYGON((6 6,6 10,10 10,10 6,6 6))");
    test_get_ring<multi_polygon>("multi_1_1", multi, {0, 1, 0}, "POLYGON((7 7,8 7,8 8,7 7))");
}

template <typename Point, bool Closed>
void test_segment_count_on_ring()
{
    using ring = bg::model::ring<Point, true, Closed>;
    using polygon = bg::model::polygon<Point, true, Closed>;
    using multi_polygon = bg::model::multi_polygon<polygon>;

    test_segment_count_on_ring<ring>("ring_simplex", simplex, {0, -1, -1}, 4);
    test_segment_count_on_ring<polygon>("polygon_simplex", simplex, {0, -1, -1}, 4);
    test_segment_count_on_ring<polygon>("case_a_outer", case_a, {0, -1, -1}, 3);
    test_segment_count_on_ring<polygon>("case_a_0", case_a, {0, -1, 0}, 3);
    test_segment_count_on_ring<multi_polygon>("multi_0_outer", multi, {0, 0, -1}, 4);
    test_segment_count_on_ring<multi_polygon>("multi_0_0", multi, {0, 0, 0}, 4);
    test_segment_count_on_ring<multi_polygon>("multi_0_1", multi, {0, 0, 1}, 4);
    test_segment_count_on_ring<multi_polygon>("multi_1_outer", multi, {0, 1, -1}, 4);
    test_segment_count_on_ring<multi_polygon>("multi_1_1", multi, {0, 1, 0}, 3);
}

template <typename Point, bool Closed>
void test_segment_distance()
{
    using ring = bg::model::ring<Point, true, Closed>;

    std::string const case_id = "ring_simplex";

    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 0}, {0, -1, -1, 0}, 0);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 0}, {0, -1, -1, 1}, 1);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 0}, {0, -1, -1, 2}, 2);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 0}, {0, -1, -1, 3}, 3);

    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 1}, {0, -1, -1, 0}, 3);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 1}, {0, -1, -1, 1}, 0);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 1}, {0, -1, -1, 2}, 1);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 1}, {0, -1, -1, 3}, 2);

    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 2}, {0, -1, -1, 0}, 2);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 2}, {0, -1, -1, 1}, 3);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 2}, {0, -1, -1, 2}, 0);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 2}, {0, -1, -1, 3}, 1);

    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 3}, {0, -1, -1, 0}, 1);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 3}, {0, -1, -1, 1}, 2);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 3}, {0, -1, -1, 2}, 3);
    test_segment_distance<ring>(case_id, __LINE__, simplex, {0, -1, -1, 3}, {0, -1, -1, 3}, 0);
}

int test_main(int, char* [])
{
    using point = bg::model::point<default_test_type, 2, bg::cs::cartesian>;
    test_get_ring<point, true>();
    test_segment_count_on_ring<point, true>();
    test_segment_count_on_ring<point, false>();
    test_segment_distance<point, true>();
    test_segment_distance<point, false>();
    return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101111    E. Iorfida        File created.
 *      101111    E. Iorfida        Implementation of all the equations up to the Newton method.
 *      101117    E. Iorfida        Velocities computations added.
 *      101126    E. Iorfida        Get/set codes deleted.
 *      101206    E. Iorfida        LambertTargetingElements class deleted,
 *                                  added setInitialState, modified punctuation. Set single
 *                                  variables, change variables names in more understandable ones.
 *      101209    E. Iorfida        Corrected some coding errors.
 *      101213    E. Iorfida        Deleted lambertAngle, added numberOfRevolution, modified
 *                                  implementation.
 *      101214    E. Iorfida        Implementation only for the case with numberOfRevolution = 0.
 *      110113    E. Iorfida        Added necessary elements to build pointer-to-member-function
 *                                  to RootFinderAlgorithms and NewtonRaphsonMethod classes.
 *      110124    E. Iorfida        Added necessary piece of code to be able to use the last
 *                                  version of Newton-Raphson code.
 *      110126    E. Iorfida        Initialized member functions.
 *      110130    J. Melman         Simplified variable names, e.g., 'normOfdeletVelocityVector'
 *                                  became 'speed'. Requested references to specific formulas. Also
 *                                  corrected 'transverse' to 'transverse'. Simplified computation
 *                                  of radial unit vector. Corrected computation of transverse
 *                                  heliocentric velocity.
 *      110201    E. Iorfida        Added pointerToCelestialBody and modified variable names (from
 *                                  heliocentric, to inertial). Added patch for negative case of
 *                                  initialLambertGuess. Added equations references.
 *      110206    E. Iorfida        Added unique function for Newton-Raphson method. Added
 *                                  computeAbsoluteValue to the initialLambertGuess for
 *                                  non-converging cases.
 *      110208    E. Iorfida        Added CartesianPositionElements objects as input and
 *                                  CartesianVelocityElements objects as output.
 *      110418    E. Iorfida        Added a new normal plane that take into account the case of two
 *                                  parallel position vector (with a relative angle of 180
 *                                  degrees). Better defined the pointers to the output
 *                                  CartesianVelocityElements.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120619    T. Secretin       Converted to free functions. Added Izzo's approach.
 *      120704    P. Musegaas       Various small changes during code check.
 *      120713    P. Musegaas       Changed tolerance to relative tolerance in Gooding's rootfinder.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *      Gooding, R.H. A procedure for the solution of Lambert's orbital boundary-value problem,
 *          Celestial Mechanics and Dynamical Astronomy, 48:145-165, 1990.
 *
 *    Notes
 *
 */

#include <cmath>
#include <iostream>
#include <sstream>
#include <stdexcept>

#include <boost/bind.hpp>
#include <boost/exception/all.hpp>
#include <boost/make_shared.hpp>
#include <boost/math/special_functions.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Solve Lambert Problem using Izzo's algorithm.
void solveLambertProblemIzzo( const Eigen::Vector3d& cartesianPositionAtDeparture,
                              const Eigen::Vector3d& cartesianPositionAtArrival,
                              const double timeOfFlight,
                              const double gravitationalParameter,
                              Eigen::Vector3d& cartesianVelocityAtDeparture,
                              Eigen::Vector3d& cartesianVelocityAtArrival,
                              const bool isRetrograde,
                              const double convergenceTolerance,
                              const unsigned int maximumNumberOfIterations )
{
    // Sanity check for specified time-of-flight.
    if ( timeOfFlight <= 0.0 )
    {
        // Define error message.
        std::stringstream errorMessage;
        errorMessage << "Specified time-of-flight must be strictly positive.\n"
                     << "Specified time-of-flight: " << timeOfFlight << " days." << std::endl;

        // Throw exception.
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error( errorMessage.str( ) ) ) );
    }

    // Compute normalizing values.
    const double distanceNormalizingValue = cartesianPositionAtDeparture.norm( );
    const double velocityNormalizingValue = std::sqrt( gravitationalParameter /
                                                       distanceNormalizingValue );
    const double timeNormalizingValue = distanceNormalizingValue / velocityNormalizingValue;

    // Compute transfer geometry parameters in adimensional units.
    // Cosine of transfer angle.
    const double cosineOfTransferAngle =
            cartesianPositionAtDeparture.dot( cartesianPositionAtArrival )
            / (distanceNormalizingValue * cartesianPositionAtArrival.norm( ) );

    // Normalized Cartesian position at arrival.
    const double normalizedRadiusAtArrival = cartesianPositionAtArrival.norm( )
            / distanceNormalizingValue;

    // Chord.
    const double chord = std::sqrt( 1.0 + normalizedRadiusAtArrival
                                    * ( normalizedRadiusAtArrival - 2.0 * cosineOfTransferAngle ) );

    // Semi-perimeter.
    const double semiPerimeter = ( 1.0 + normalizedRadiusAtArrival + chord ) / 2.0;

    // Assuming a prograde motion, determine whether the transfer corresponds to the long- or the
    // short-way solution: longway if x1*y2 - x2*y1 < 0.
    bool isLongway = false;
    if ( cartesianPositionAtDeparture.x( ) * cartesianPositionAtArrival.y( )
         - cartesianPositionAtDeparture.y( ) * cartesianPositionAtArrival.x( ) < 0.0 )
    {
        isLongway = true;
    }

    // If retrograde is true, switch longway flag.
    if ( isRetrograde )
    {
        isLongway = !isLongway;
    }

    // Semi-major axis of the minimum energy ellipse.
    const double semiMajorAxisOfTheMinimumEnergyEllipse = semiPerimeter / 2.0;

    // Transfer angle.
    double transferAngle = std::acos( cosineOfTransferAngle );
    if ( isLongway )
    {
        transferAngle = 2.0 * mathematical_constants::PI - transferAngle;
    }

    // Lambda parameter.
    const double lambdaParameter = std::sqrt( normalizedRadiusAtArrival )
            * std::cos( transferAngle / 2.0 ) / semiPerimeter;

    // Optimize log(t_spec).
    const double normalizedSpecifiedTimeOfFlight = timeOfFlight / timeNormalizingValue;
    const double logarithmOfTheSpecifiedTimeOfFlight = std::log( normalizedSpecifiedTimeOfFlight );

    // Secant Method.
    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1 = std::log( 0.5 ), x2 = std::log( 1.5 );

    double y1 = std::log( computeTimeOfFlightIzzo( -0.5, semiPerimeter, chord, isLongway,
                                                   semiMajorAxisOfTheMinimumEnergyEllipse ) )
            - logarithmOfTheSpecifiedTimeOfFlight;

    double y2 = std::log( computeTimeOfFlightIzzo( 0.5, semiPerimeter, chord, isLongway,
                                                   semiMajorAxisOfTheMinimumEnergyEllipse ) )
            - logarithmOfTheSpecifiedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    unsigned int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && (y1 != y2)
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = std::log( computeTimeOfFlightIzzo( std::exp( xNew ) - 1.0, semiPerimeter, chord,
                                                  isLongway,
                                                  semiMajorAxisOfTheMinimumEnergyEllipse ) )
                - logarithmOfTheSpecifiedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        std::cerr << "Lambert Solver did not converge within the maximum number of iterations ("
                  << maximumNumberOfIterations << ")." << std::endl;
    }

    // Revert to x parameter.
    const double xParameter = std::exp( xNew ) - 1.0;

    // Determine semi-major axis of the conic.
    const double semiMajorAxis = semiMajorAxisOfTheMinimumEnergyEllipse
            / ( 1.0 - xParameter * xParameter );

    // Declare variables.
    double etaParameter, etaParameterSquared, psiParameter;

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * std::asin( std::sqrt( ( semiPerimeter - chord )
                                                           / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter.
        psiParameter = ( alphaParameter - betaParameter ) / 2.0;

        // Eta parameter.
        etaParameterSquared = 2.0 * semiMajorAxis * std::sin( psiParameter )
                * std::sin( psiParameter ) / semiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * boost::math::asinh (
                    std::sqrt( ( semiPerimeter - chord ) / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter.
        psiParameter = (alphaParameter - betaParameter ) / 2.0;

        // Eta parameter.
        etaParameterSquared = -2.0 * semiMajorAxis * std::sinh( psiParameter )
                * std::sinh( psiParameter ) / semiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Determine semi-latus rectum, p.
    const double semiLatusRectum = ( normalizedRadiusAtArrival
                                     / ( semiMajorAxisOfTheMinimumEnergyEllipse
                                         * etaParameterSquared ) )
            * std::sin( transferAngle / 2.0 )
            * std::sin( transferAngle / 2.0 );

    // Determine sigma.
    const double sigmaParameter =
            ( 1.0 / ( etaParameter * std::sqrt( semiMajorAxisOfTheMinimumEnergyEllipse ) ) )
            * ( 2.0 * lambdaParameter * semiMajorAxisOfTheMinimumEnergyEllipse
                - ( lambdaParameter + xParameter * etaParameter ) );

    // Velocity components at departure.
    const double radialVelocityAtDeparture = sigmaParameter;
    const double transverseVelocityAtDeparture = std::sqrt( semiLatusRectum );

    // Velocity components at arrival.
    const double transverseVelocityAtArrival = transverseVelocityAtDeparture
            / normalizedRadiusAtArrival;
    const double radialVelocityAtArrival = ( transverseVelocityAtDeparture
                                             - transverseVelocityAtArrival )
                                           / std::tan( transferAngle / 2.0 )
                                           - radialVelocityAtDeparture;

    // Determine radial unit vectors.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Determine plane of motion.
    Eigen::Vector3d angularMomentumVector;

    if ( isLongway )
    {
        angularMomentumVector = radialUnitVectorAtArrival.cross( radialUnitVectorAtDeparture );
    }
    else
    {
        angularMomentumVector = radialUnitVectorAtDeparture.cross( radialUnitVectorAtArrival );
    }

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Compute transverse unit vectors.
    const Eigen::Vector3d transverseUnitVectorAtDeparture =
            radialUnitVectorAtDeparture.cross( angularMomentumUnitVector );
    const Eigen::Vector3d transverseUnitVectorAtArrival =
            radialUnitVectorAtArrival.cross( angularMomentumUnitVector );

    // Reconstruct non-dimensional velocity vectors.
    cartesianVelocityAtDeparture
            << radialVelocityAtDeparture * radialUnitVectorAtDeparture.x( )
               - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.x( ),
            radialVelocityAtDeparture * radialUnitVectorAtDeparture.y( )
            - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.y( ),
            radialVelocityAtDeparture * radialUnitVectorAtDeparture.z( )
            - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.z( );

    cartesianVelocityAtArrival
            << radialVelocityAtArrival * radialUnitVectorAtArrival.x( )
               - transverseVelocityAtArrival * transverseUnitVectorAtArrival.x( ),
            radialVelocityAtArrival * radialUnitVectorAtArrival.y( )
            - transverseVelocityAtArrival * transverseUnitVectorAtArrival.y( ),
            radialVelocityAtArrival * radialUnitVectorAtArrival.z( )
            - transverseVelocityAtArrival * transverseUnitVectorAtArrival.z( );

    // Return dimensions.
    cartesianVelocityAtDeparture *= velocityNormalizingValue;
    cartesianVelocityAtArrival *= velocityNormalizingValue;

}

//! Compute time-of-flight using Lagrange's equation.
double computeTimeOfFlightIzzo( const double xParameter, const double semiPerimeter,
                                const double chord, const bool isLongway,
                                const double semiMajorAxisOfTheMinimumEnergyEllipse )
{
    // Determine semi-major axis.
    const double semiMajorAxis = semiMajorAxisOfTheMinimumEnergyEllipse
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * std::asin ( std::sqrt( ( semiPerimeter - chord )
                                                            / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) ) );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * boost::math::asinh(
                    std::sqrt( ( semiPerimeter - chord ) / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlight = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlight;
    }

}

//! Solve Lambert Problem using Gooding's algorithm.
void solveLambertProblemGooding( const Eigen::Vector3d& cartesianPositionAtDeparture,
                                 const Eigen::Vector3d& cartesianPositionAtArrival,
                                 const double timeOfFlight,
                                 const double gravitationalParameter,
                                 Eigen::Vector3d& cartesianVelocityAtDeparture,
                                 Eigen::Vector3d& cartesianVelocityAtArrival,
                                 RootFinderPointer rootFinder )
{
    if ( !rootFinder.get( ) )
    {
        rootFinder = boost::make_shared< root_finders::NewtonRaphson >( 1.0e-12, 1000 );
    }

    // Normalize positions.
    const double radiusAtDeparture = cartesianPositionAtDeparture.norm( );
    const double radiusAtArrival = cartesianPositionAtArrival.norm( );

    // Compute angle between positions.
    Eigen::Vector3d planeNormalPosition =
            cartesianPositionAtDeparture.cross( cartesianPositionAtArrival ).normalized( );

    double reducedLambertAngle = linear_algebra::computeAngleBetweenVectors(
                cartesianPositionAtDeparture, cartesianPositionAtArrival );

    if ( planeNormalPosition.z( ) < 0.0 )
    {
        reducedLambertAngle = 2.0 * mathematical_constants::PI
                - reducedLambertAngle;
    }

    // Compute chord.
    const double chord = std::sqrt( radiusAtDeparture * radiusAtDeparture +
                                    radiusAtArrival * radiusAtArrival -
                                    2.0 * radiusAtDeparture * radiusAtArrival *
                                    std::cos( reducedLambertAngle ) );

    // Compute semi-perimeter.
    const double semiPerimeter = ( radiusAtDeparture + radiusAtArrival + chord ) / 2.0;

    // Compute normalized time of flight.
    // Formula (7) [3].
    const double normalizedTimeOfFlight = std::sqrt( 8.0 * gravitationalParameter / semiPerimeter
                                                     / semiPerimeter / semiPerimeter )
                                          * timeOfFlight;

    // Compute q-parameter.
    // Formula (5) [3].
    const double qParameter = ( std::sqrt( radiusAtDeparture * radiusAtArrival ) /
                                semiPerimeter ) * cos( reducedLambertAngle / 2.0 );

    // Compute values of parameters needed to compute the initial guess of x-parameter.
    const double zParameterInitial = std::sqrt( 1.0 - qParameter * qParameter );

    // Formula (6.11) [2].
    const double lambertEccentricAnomalyInitial = -1.0;

    // Formula (6.12) [2].
    const double yParameterInitial = std::sqrt( std::fabs( lambertEccentricAnomalyInitial ) );

    // Formula (6.14) [2].
    const double fParameterInitial = yParameterInitial * zParameterInitial;

    // Formula (6.15) [2].
    const double gParameterInitial = -1.0 * qParameter * lambertEccentricAnomalyInitial;

    // Page 47 [2].
    const double dParameterInitial = atan( fParameterInitial / gParameterInitial );

    // Value of T(x) for x=0.
    // Formula (6.9) [2].
    const double tFunctionInitial = -2.0 * ( qParameter * zParameterInitial +
            dParameterInitial / yParameterInitial ) / lambertEccentricAnomalyInitial;

    // Declare initial guess of x-parameter.
    double initialLambertGuess;

    // Determine initial Lambert guess.
    if ( tFunctionInitial > normalizedTimeOfFlight )
    {
        // Formula (11) [3].
        initialLambertGuess = tFunctionInitial *
                ( tFunctionInitial - normalizedTimeOfFlight ) /
                ( 4.0 * normalizedTimeOfFlight );
    }
    else
    {
        // Formula (13) [3].
        const double x01 = -1.0 * ( normalizedTimeOfFlight - tFunctionInitial ) /
                     ( normalizedTimeOfFlight - tFunctionInitial + 4.0 );

        // Formula (15) [3].
        const double x02 = -1.0 * std::sqrt( ( normalizedTimeOfFlight -
                     tFunctionInitial ) / ( normalizedTimeOfFlight +
                     0.5 * tFunctionInitial ) );

        // Formula (20) [3].
        const double phi = 2.0 * atan2( ( 1.0 - qParameter * qParameter ), 2.0 * qParameter );

        // Formula (16) [3].
        const double W = x01 + 1.7 * std::sqrt(
                    2.0 - phi / mathematical_constants::PI );

        // Formula (17) [3].
        double x03;
        if ( W >= 0.0 )
        {
           x03 = x01;
        }
        else
        {
           x03 = x01 + std::pow( -W , 1.0 / 16.0 ) * ( x02 - x01 );
        }

        // Formula (19) [3].
        const double lambdax = 1.0 + 0.5 * x03 * ( 1.0 + x01 ) - 0.03 * x03 * x03
                * std::sqrt( 1.0 + x01 );

        // Formula (18) [3].
        initialLambertGuess = lambdax * x03;
    }

    // Newton-Raphson method implementation.
    // Set the class that contains the functions needed for Newton-Raphson.
    LambertFunctionsGooding lambertFunctionsGooding( qParameter, normalizedTimeOfFlight );

    // Create an object containing the function of which we whish to obtain the root from.
    using basic_mathematics::UnivariateProxyPointer;
    using basic_mathematics::UnivariateProxy;
    UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                boost::bind( &LambertFunctionsGooding::computeLambertFunctionGooding,
                             lambertFunctionsGooding, _1 ) );

    // Add the first derivative of the root function.
    rootFunction->addBinding( -1, boost::bind( &LambertFunctionsGooding::
                                               computeFirstDerivativeLambertFunctionGooding,
                                               lambertFunctionsGooding, _1 ) );

    // Initialize the xParameter.
    double xParameter = TUDAT_NAN;

    // Set initial guess of the variable computed in Newton-Rapshon method. A patch for negative
    // initialLambertGuess is applied. This has not been stated in the paper by Gooding, but this
    // patch has been found by trial and error.
    if ( initialLambertGuess * initialLambertGuess - 1.0 < 0.0 )
    {
        // Set xParameter based on result of Newton-Raphson root-finding algorithm.
        xParameter = rootFinder->execute( rootFunction, std::fabs( initialLambertGuess ) );
    }
    else
    {
        // Set xParameter based on result of Newton-Raphson root-finding algorithm.
        xParameter = rootFinder->execute( rootFunction, initialLambertGuess );
    }

    // Compute velocities at departure and at arrival.

    // Compute gamma, rho and sigma parameters, needed to compute the velocities.
    // Formula (8) [3].
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
            qParameter * qParameter * xParameter * xParameter );

    // Formula (6.23) [2].
    const double lambertGamma = std::sqrt( gravitationalParameter * semiPerimeter / 2.0 );

    // Formula (6.24) [2].
    const double lambertRho = ( radiusAtDeparture - radiusAtArrival ) / chord;

    // Formula (6.25) [2].
    const double lambertSigma = 2.0 * ( std::sqrt(
                                            radiusAtDeparture * radiusAtArrival / chord / chord ) )
            * std::sin( reducedLambertAngle / 2.0 );

    // Compute radial speeds at departure and at arrival.
    // Formula (23) [3].
    const double radialSpeedAtDeparture =
            lambertGamma * ( qParameter * zParameter - xParameter -
                             lambertRho * ( qParameter * zParameter + xParameter ) ) /
            radiusAtDeparture;

    // Formula (24) [3].
    const double radialSpeedAtArrival =
            -lambertGamma * ( qParameter * zParameter - xParameter +
                              lambertRho * ( qParameter * zParameter + xParameter ) ) /
            radiusAtArrival;

    // Compute transverse speeds at departure and at arrival.
    // Compute large part of formulas (25) and (26).
    const double transverseSpeedHelper = lambertGamma * lambertSigma *
            ( zParameter + qParameter * xParameter );

    // Formula (25) [3].
    const double transverseSpeedAtDeparture = transverseSpeedHelper / radiusAtDeparture;

    // Formula (26) [3].
    const double transverseSpeedAtArrival = transverseSpeedHelper / radiusAtArrival;

    // Compute inertial velocities (those velocity are computed with respect to the central body,
    // so they can be either heliocentric or planetocentric).

    // Compute radial unit vectors at departure and at arrival.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    Eigen::Vector3d unitZVector;
    unitZVector.x( ) = 0.0;
    unitZVector.y( ) = 0.0;
    unitZVector.z( ) = 1.0;

    Eigen::Vector3d planeNormal =
            ( ( cartesianPositionAtDeparture.cross( unitZVector ) ).cross(
                    cartesianPositionAtDeparture ) ).normalized( );

    // Compute unit vector that is normal to the plane in which the trajectory takes place, and
    // points in the positive z-direction.
    if ( planeNormal.z( ) < 0.0 )
    {
        planeNormal = -planeNormal;
    }

    // Compute transverse unit vectors at departure and at arrival.
    const Eigen::Vector3d transverseUnitVectorAtDeparture = planeNormal.cross(
            cartesianPositionAtDeparture.normalized( ) );
    const Eigen::Vector3d transverseUnitVectorAtArrival = planeNormal.cross(
            cartesianPositionAtArrival.normalized( ) );

    // Compute radial inertial velocities at departure and at arrival.
    const Eigen::Vector3d radialInertialVelocityAtDeparture
            = radialSpeedAtDeparture * radialUnitVectorAtDeparture;
    const Eigen::Vector3d radialInertialVelocityAtArrival
            = radialSpeedAtArrival * radialUnitVectorAtArrival;

    // Compute transverse heliocentric velocities at departure and
    // at arrival.
    const Eigen::Vector3d transverseInertialVelocityAtDeparture
            = transverseSpeedAtDeparture * transverseUnitVectorAtDeparture;
    const Eigen::Vector3d transverseInertialVelocityAtArrival
            = transverseSpeedAtArrival * transverseUnitVectorAtArrival;

    // Compute heliocentric velocities at departure and at arrival.
    // Define output velocities.
    cartesianVelocityAtDeparture
            = radialInertialVelocityAtDeparture + transverseInertialVelocityAtDeparture;
    cartesianVelocityAtArrival
            = radialInertialVelocityAtArrival + transverseInertialVelocityAtArrival;
}

//! Define general Lambert function.
double LambertFunctionsGooding::computeLambertFunctionGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;

    if ( lambertEccentricAnomaly > 0.0 )
    {
        return lambertFunctionPositiveGooding( xParameter );
    }

    else
    {
        return lambertFunctionNegativeGooding( xParameter );
    }
}

//! Define first derivative of general Lambert function.
double LambertFunctionsGooding::computeFirstDerivativeLambertFunctionGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;

    if ( lambertEccentricAnomaly > 0.0 )
    {
        return lambertFirstDerivativeFunctionPositiveGooding( xParameter );
    }

    else
    {
        return lambertFirstDerivativeFunctionNegativeGooding( xParameter );
    }
}

//! Define Lambert function for positive lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFunctionPositiveGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter+
                                         qParameter * qParameter * xParameter  * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = log( fParameter + gParameter );

    return normalizedTimeOfFlight - 2.0 * ( xParameter - qParameter * zParameter - dParameter /
                                            yParameter ) / lambertEccentricAnomaly;
}

//! Define Lambert function for negative lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFunctionNegativeGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::atan( fParameter / gParameter );

    return normalizedTimeOfFlight - 2.0 * ( xParameter - qParameter * zParameter - dParameter
                                            / yParameter ) / lambertEccentricAnomaly;
}

//! Define first derivative of Lambert function for positive lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFirstDerivativeFunctionPositiveGooding(
        const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::log( fParameter + gParameter );
    const double lambertEccentricAnomalyDerivative = 2.0 * xParameter;
    const double yParameterDerivative = xParameter / std::sqrt( xParameter * xParameter - 1.0 );
    const double zParameterDerivative = qParameter * qParameter * xParameter / zParameter;
    const double fParameterDerivative = yParameterDerivative  *
            ( zParameter - xParameter * qParameter ) + yParameter *
            ( zParameterDerivative - qParameter );
    const double gParameterDerivative = zParameter + xParameter *
            zParameterDerivative - qParameter * lambertEccentricAnomalyDerivative;
    const double dParameterDerivative = ( fParameterDerivative + gParameterDerivative ) /
            ( fParameter + gParameter );

    return - 2.0 * ( lambertEccentricAnomaly
                     * ( 1.0 - qParameter * zParameterDerivative
                         - ( ( dParameterDerivative * yParameter
                               - yParameterDerivative * dParameter )
                             / ( yParameter * yParameter ) ) )
                     - ( ( xParameter - qParameter * zParameter - ( dParameter / yParameter ) )
                         * lambertEccentricAnomalyDerivative ) )
            / ( lambertEccentricAnomaly * lambertEccentricAnomaly );
}

//! Define first derivative of Lambert function for negative lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFirstDerivativeFunctionNegativeGooding(
        const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::atan( fParameter / gParameter );
    const double lambertEccentricAnomalyDerivative = 2.0 * xParameter;
    const double yParameterDerivative = -1.0 * xParameter
            / std::sqrt( 1.0 - xParameter * xParameter);
    const double zParameterDerivative = qParameter * qParameter * xParameter / zParameter;
    const double fParameterDerivative = yParameterDerivative *
            ( zParameter - xParameter * qParameter ) + yParameter *
            ( zParameterDerivative - qParameter );
    const double gParameterDerivative = zParameter + xParameter *
            zParameterDerivative - qParameter *  lambertEccentricAnomalyDerivative;
    const double dParameterDerivative = ( fParameterDerivative * gParameter -
                                          fParameter * gParameterDerivative ) /
            ( fParameter * fParameter + gParameter * gParameter );

    return - 2.0 * ( lambertEccentricAnomaly * ( 1.0 - qParameter * zParameterDerivative -
                                                 ( ( dParameterDerivative * yParameter -
                                                     yParameterDerivative * dParameter ) /
                                                   yParameter / yParameter ) ) -
                     ( ( xParameter - qParameter * zParameter - ( dParameter / yParameter ) ) *
                       lambertEccentricAnomalyDerivative ) ) /
            ( lambertEccentricAnomaly * lambertEccentricAnomaly );
}

} // namespace mission_segments
} // namespace tudat


#include <b0/node.h>
#include <b0/publisher.h>
#include <b0/subscriber.h>

#include <boost/lexical_cast.hpp>

/*! \example remapping/operation.cpp
 * This node performs a mathematical operation on the data received on its two topics
 */

//! \cond HIDDEN_SYMBOLS

class Operation : public b0::Node
{
public:
    Operation(char op)
        : b0::Node("operation"),
          op_(op),
          sub_a_(this, "a", &Operation::callback_a, this),
          sub_b_(this, "b", &Operation::callback_b, this),
          pub_(this, "out"),
          value_a_(0),
          value_b_(0)
    {
        if(op != '+' && op != '-' && op != '*' && op != '/')
            throw std::runtime_error("invalid operator");
    }

    void callback_a(const std::string &msg)
    {
        value_a_ = boost::lexical_cast<int>(msg);
        compute();
    }

    void callback_b(const std::string &msg)
    {
        value_b_ = boost::lexical_cast<int>(msg);
        compute();
    }

    void compute()
    {
        int result = 0;
        switch(op_)
        {
        case '+': result = value_a_ + value_b_; break;
        case '-': result = value_a_ - value_b_; break;
        case '*': result = value_a_ * value_b_; break;
        case '/': result = value_a_ / value_b_; break;
        }
        pub_.publish(boost::lexical_cast<std::string>(result));
    }

private:
    char op_;
    b0::Subscriber sub_a_;
    b0::Subscriber sub_b_;
    b0::Publisher pub_;
    int value_a_;
    int value_b_;
};

int main(int argc, char **argv)
{
    b0::addOptionString("operator,o", "the mathematical operator", nullptr, true);
    b0::init(argc, argv);
    Operation node(b0::getOptionString("operator")[0]);
    node.init();
    node.spin();
    node.cleanup();
    return 0;
}

//! \endcond


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

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[register_ring_templated
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_RING_TEMPLATED

#include <iostream>
#include <deque>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

// Adapt any deque to Boost.Geometry Ring Concept
BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(std::deque)

int main()
{
    std::deque<boost::geometry::model::d2::point_xy<double> > ring(3);
    boost::geometry::assign_values(ring[0], 0, 0);
    boost::geometry::assign_values(ring[2], 4, 1);
    boost::geometry::assign_values(ring[1], 1, 4);

    // Boost.Geometry algorithms work on any deque now
    boost::geometry::correct(ring);
    std::cout << "Area: "  << boost::geometry::area(ring) << std::endl;
    std::cout << "Contents: "  << boost::geometry::wkt(ring) << std::endl;

    return 0;
}

//]

//[register_ring_templated_output
/*`
Output:
[pre
Area: 7.5
Line: ((0, 0), (1, 4), (4, 1), (0, 0))
]
*/
//]