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

/*
Copyright (C) 2003, 2004, 2007 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 swap.cpp file from test-suite.

#ifndef cl_adjoint_swap_impl_hpp
#define cl_adjoint_swap_impl_hpp
#pragma once

#include <ql/quantlib.hpp>
#include "utilities.hpp"
#include "adjointswaptest.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 "AdjointSwap"

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

            return columns;
        }

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

        Real param_;
        Real swapNpv_;
    };

    struct SwapData
    {
        SwapData()
        : type_(VanillaSwap::Payer)
        , settlementDays_(2)
        , nominal_(100.0)
        , fixedConvention_(Unadjusted)
        , floatingConvention_(ModifiedFollowing)
        , fixedFrequency_(Annual)
        , floatingFrequency_(Semiannual)
        , fixedDayCount_(Thirty360())
        , lengths_({ 1, 2, 5, 10, 20 })
        , rates_({ 0.04, 0.05, 0.06, 0.07 })
        , spreads_({ -0.01, -0.001, 0.001, 0.01 })
        , backup_()
        {
            index_ = boost::make_shared<Euribor>(Period(floatingFrequency_), termStructure_);
            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()));
        }

        boost::shared_ptr<VanillaSwap> makeSwap(Integer length
                                                , Rate fixedRate
                                                , Spread floatingSpread)
        {
            Date maturity = calendar_.advance(settlement_
                                              , length
                                              , Years
                                              , floatingConvention_);

            Schedule fixedSchedule(settlement_
                                   , maturity
                                   , Period(fixedFrequency_)
                                   , calendar_
                                   , fixedConvention_
                                   , fixedConvention_
                                   , DateGeneration::Forward
                                   , false);

            Schedule floatSchedule(settlement_
                                   , maturity
                                   , Period(floatingFrequency_)
                                   , calendar_
                                   , floatingConvention_
                                   , floatingConvention_
                                   , DateGeneration::Forward
                                   , false);

            boost::shared_ptr<VanillaSwap> swap =
                boost::make_shared<VanillaSwap>(type_
                                , nominal_
                                , fixedSchedule
                                , fixedRate
                                , fixedDayCount_
                                , floatSchedule
                                , index_
                                , floatingSpread
                                , index_->dayCounter());

            swap->setPricingEngine(boost::make_shared<DiscountingSwapEngine>(termStructure_));

            return swap;
        }

        Real calculateRateDependencyNPV(Real rate)
        {
            Real Y = 0;
            for (Size i = 0; i < lengths_.size(); i++)
            {
                for (Size j = 0; j < spreads_.size(); j++)
                {
                    boost::shared_ptr<VanillaSwap> swap =
                        makeSwap(lengths_[i], rate, spreads_[j]);
                    Y += swap->NPV();
                }
            }
            return Y;
        }

        Real calculateSpreadDependencyNPV(Real spread)
        {
            Real Y = 0;

            for (Size i = 0; i < lengths_.size(); i++)
            {
                for (Size j = 0; j < rates_.size(); j++)
                {
                    boost::shared_ptr<VanillaSwap> swap =
                        makeSwap(lengths_[i], rates_[j], spread);
                    Y += swap->NPV();
                }
            }
            return Y;
        }

        Real calculateInArrearsNPV(Real capletVolatility)
        {
            Date maturity = today_ + 5 * Years;
            Calendar calendar = NullCalendar();

            Schedule schedule(today_
                              , maturity
                              , Period(Annual)
                              , calendar
                              , Following
                              , Following
                              , DateGeneration::Forward
                              , false);

            DayCounter dayCounter = SimpleDayCounter();
            std::vector<Real> nominals(1, 100000000.0);

            boost::shared_ptr<IborIndex> index_
                = boost::make_shared<IborIndex>("dummy"
                , 1 * Years
                , 0
                , EURCurrency()
                , calendar
                , Following
                , false
                , dayCounter
                , termStructure_);
            Rate oneYear = 0.05;
            Rate r = std::log(1.0 + oneYear);
            termStructure_.linkTo(flatRate(today_, r, dayCounter));

            std::vector<Rate> coupons(1, oneYear);
            Leg fixedLeg = FixedRateLeg(schedule)
                .withNotionals(nominals)
                .withCouponRates(coupons, dayCounter);

            std::vector<Real> gearings;
            std::vector<Rate> spreads;
            Natural fixingDays = 0;

            Real Y = 0;

            Handle<OptionletVolatilityStructure> vol(
                boost::make_shared<ConstantOptionletVolatility>(today_
                , NullCalendar()
                , Following
                , capletVolatility
                , dayCounter));

            boost::shared_ptr<IborCouponPricer> pricer
                = boost::make_shared<BlackIborCouponPricer>(vol);

            Leg floatingLeg = IborLeg(schedule, index_)
                .withNotionals(nominals)
                .withPaymentDayCounter(dayCounter)
                .withFixingDays(fixingDays)
                .withGearings(gearings)
                .withSpreads(spreads)
                .inArrears();
            setCouponPricer(floatingLeg, pricer);

            Swap swap(floatingLeg, fixedLeg);
            swap.setPricingEngine(boost::make_shared<DiscountingSwapEngine>(termStructure_));

            return swap.NPV();
        }

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

            std::vector<Real> swapPortfolioNpv(sizeX);
            for (int i = 0; i < sizeX; i++)
            {
                swapPortfolioNpv[i] = swapNpv(X[i]);
            }

            sf_Finite.resize(sizeX);

            for (int i = 0; i < sizeX; i++)
            {
                sf_Finite[i] = (swapNpv(X[i] + h) - swapPortfolioNpv[i]) / h;
            }
        }

        // Global data.
        Calendar calendar_;
        Date today_, settlement_;
        VanillaSwap::Type type_;
        Real nominal_;
        BusinessDayConvention fixedConvention_, floatingConvention_;
        Frequency fixedFrequency_, floatingFrequency_;
        DayCounter fixedDayCount_;
        Natural settlementDays_;
        RelinkableHandle<YieldTermStructure> termStructure_;
        boost::shared_ptr<IborIndex> index_;

        std::vector<Integer> lengths_;
        std::vector<Rate> rates_;
        std::vector<Spread> spreads_;

        // Cleanup.
        SavedSettings backup_;
    };

    struct RateDependencyTestData
        : public SwapData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            explicit Test(Size size, RateDependencyTestData* data)
            : size_(size)
            , data_(data)
            , rate_(size)
            , iterNumFactor_(1000)
            {
                setLogger(&data_->outPerform_);

                Rate startRate = 0.01;
                Rate maxRate = 0.06;
                Real step = (maxRate - startRate) / size_;

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

            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

            void recordTape()
            {
                cl::Independent(rate_);
                calculateTotalSwapNpv();
                f_ = std::make_unique<cl::tape_function<double>>(rate_, swapNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwapNpv()
            {
                swapNpv_.resize(depVarNumber());
                for (Size i = 0; i < size_; i++)
                    swapNpv_[0] += data_->calculateRateDependencyNPV(rate_[i]);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-12;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFinDiff(rate_
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> Real
                                          {
                                              return data_->calculateRateDependencyNPV(v);
                                          });

            }

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

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

            Size size_;
            Size iterNumFactor_;
            RateDependencyTestData* data_;
            std::vector<cl::tape_double> rate_;
            std::vector<cl::tape_double> swapNpv_;
        };

        RateDependencyTestData()
            : SwapData()

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

            , outAdjoint_(OUTPUT_FOLDER_NAME "//RateDependency"
            , { { "filename", "Adjoint" }
              , { "not_clear", "Not" }
              , { "smooth", "default" }
              , { "title", "Swap NPV adjoint differentiation performance with respect to rate" }
              , { "cleanlog", "false" }
              , { "ylabel", "Time (s)" }
              , { "xlabel", "Number of rates" } })

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

            , out_(OUTPUT_FOLDER_NAME "//RateDependency//output"
            , { { "filename", "SwapNPVonRates" }
              , { "ylabel", "Swap NPV" }
              , { "not_clear", "Not" }
              , { "title", "Swap NPV dependence on rate" }
              , { "cleanlog", "false" }
              , { "xlabel", "Rate" } })

#if defined CL_GRAPH_GEN
            , pointNo_(10)
            , iterNo_(10)
            , 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 rate sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<Variation> outData(pointNo_);
            auto test = getTest(pointNo_);
            for (Size i = 0; i < pointNo_; i++)
            {
                outData[i].param_ = test->rate_[i];
                outData[i].swapNpv_ = test->data_->calculateRateDependencyNPV(test->rate_[i]);
            }
            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 RateDependencyTestData::Test RateDependencyTest;

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

                Spread startSpread = -0.01;
                Spread 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_);
                calculateTotalSwapNpv();
                f_ = std::make_unique<cl::tape_function<double>>(spread_, swapNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwapNpv()
            {
                swapNpv_.resize(depVarNumber());
                for (Size i = 0; i < size_; i++)
                    swapNpv_[0] += data_->calculateSpreadDependencyNPV(spread_[i]);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-12;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFinDiff(spread_
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> Real
                                          {
                                              return data_->calculateSpreadDependencyNPV(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> swapNpv_;
        };

        SpreadDependencyTestData()
            : SwapData()

            , outPerform_(OUTPUT_FOLDER_NAME "//SpreadDependency"
            , { { "filename", "AdjointPerformance" }
              , { "not_clear", "Not" }
              , { "line_box_width", "-5" }
              , { "title", "Swap 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", "Swap 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", "SwapNPVonSpreads" }
              , { "ylabel", "Swap NPV" }
              , { "not_clear", "Not" }
              , { "title", "Swap NPV dependence on spread" }
              , { "cleanlog", "false" }
              , { "xlabel", "Spread" } })

#if defined CL_GRAPH_GEN
            , pointNo_(10)
            , iterNo_(10)
            , 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 spread 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].swapNpv_ = test->data_->calculateSpreadDependencyNPV(test->spread_[i]);
            }
            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 InArrearsTestData
        : public SwapData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            explicit Test(Size size, InArrearsTestData* data)
            : size_(size)
            , data_(data)
            , volatility_(size)
            , iterNumFactor_(1000)
            {
                setLogger(&data_->outPerform_);

                Volatility startVol = 0.18;
                Volatility maxVol = 0.22;
                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_);
                calculateTotalSwapNpv();
                f_ = std::make_unique<cl::tape_function<double>>(volatility_, swapNpv_);
            }

            // Calculates total calibration error.
            void calculateTotalSwapNpv()
            {
                swapNpv_.resize(depVarNumber());
                for (Size i = 0; i < size_; i++)
                    swapNpv_[0] += data_->calculateInArrearsNPV(volatility_[i]);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-12;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFinDiff(volatility_
                                        , h
                                        , analyticalResults_
                                        , [this] (Real v) -> Real
                                          {
                                              return data_->calculateInArrearsNPV(v);
                                          });

            }

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

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

            Size size_;
            Size iterNumFactor_;
            InArrearsTestData* data_;
            std::vector<cl::tape_double> volatility_;
            std::vector<cl::tape_double> swapNpv_;
        };

        InArrearsTestData()
            : SwapData()

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

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

            , outSize_(OUTPUT_FOLDER_NAME "//InArrears"
            , { { "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 "//InArrears//output"
            , { { "filename", "SwapNPVonVolatilities" }
              , { "ylabel", "Swap NPV" }
              , { "not_clear", "Not" }
              , { "title", "Swap NPV dependence on volatility" }
              , { "cleanlog", "false" }
              , { "xlabel", "Spread" } })

#if defined CL_GRAPH_GEN
            , pointNo_(20)
            , iterNo_(10)
            , step_(20)
#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 volatility 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].swapNpv_ = test->data_->calculateInArrearsNPV(test->volatility_[i]);
            }
            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 InArrearsTestData::Test InArrearsTest;
}

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VEC_TRIGONOMETRIC_INCLUDE
#define MTL_VEC_TRIGONOMETRIC_INCLUDE

#include <boost/numeric/mtl/vector/map_view.hpp>

// vector functions

namespace mtl { namespace vec {

    /// Element-wise acos of \a v
    template <typename Vector>
    acos_view<Vector> acos(const Vector& v)
    {
        return acos_view<Vector>(v);
    }

    /// Element-wise acosh of \a v
    template <typename Vector>
    acosh_view<Vector> acosh(const Vector& v)
    {
        return acosh_view<Vector>(v);
    }

    /// Element-wise asin of \a v
    template <typename Vector>
    asin_view<Vector> asin(const Vector& v)
    {
        return asin_view<Vector>(v);
    }

    /// Element-wise asinh of \a v
    template <typename Vector>
    asinh_view<Vector> asinh(const Vector& v)
    {
        return asinh_view<Vector>(v);
    }

    /// Element-wise atan of \a v
    template <typename Vector>
    atan_view<Vector> atan(const Vector& v)
    {
        return atan_view<Vector>(v);
    }

    /// Element-wise atanh of \a v
    template <typename Vector>
    atanh_view<Vector> atanh(const Vector& v)
    {
        return atanh_view<Vector>(v);
    }

    // non-inverse

    /// Element-wise cos of \a v
    template <typename Vector>
    cos_view<Vector> cos(const Vector& v)
    {
        return cos_view<Vector>(v);
    }

    /// Element-wise cosh of \a v
    template <typename Vector>
    cosh_view<Vector> cosh(const Vector& v)
    {
        return cosh_view<Vector>(v);
    }

    /// Element-wise sin of \a v
    template <typename Vector>
    sin_view<Vector> sin(const Vector& v)
    {
        return sin_view<Vector>(v);
    }

    /// Element-wise sinh of \a v
    template <typename Vector>
    sinh_view<Vector> sinh(const Vector& v)
    {
        return sinh_view<Vector>(v);
    }

    /// Element-wise tan of \a v
    template <typename Vector>
    tan_view<Vector> tan(const Vector& v)
    {
        return tan_view<Vector>(v);
    }

    /// Element-wise tanh of \a v
    template <typename Vector>
    tanh_view<Vector> tanh(const Vector& v)
    {
        return tanh_view<Vector>(v);
    }

}} // namespace mtl::vec

#endif // MTL_VEC_TRIGONOMETRIC_INCLUDE


#ifndef GAUSSIAN_PROCESS_HPP
#define GAISSIAN_PROCESS_HPP

#include "gplib/kernel.hpp"
#include <Eigen/Cholesky>
#include <vector>

namespace librav{

    class GaussianProcess{
        public:
            ~GaussianProcess() {};
            // The keyword explicit avoid the implicit type conversion constructor
            explicit GaussianProcess(const Kernel::Ptr& kernel, double noise, const std::shared_ptr<std::vector<Eigen::VectorXd>> points, const Eigen::VectorXd& targets, size_t max_points = 100);

            // Evaluate mean and variance at a point
            void Evaluate(const Eigen::VectorXd& x, double& mean, double& variance) const;
            void EvaluateTrainingPoint(size_t ii, double& mean, double& variance) const;

            // Add new point(s). Return whether or not points were added (points will
            // only be added until 'max_points' is reached).
            bool Add(const Eigen::VectorXd& x, double target);
            bool Add(const std::vector<Eigen::VectorXd>& points, const Eigen::VectorXd& targets);

            // Update the training targets in the direction of the gradient of the
            // mean squared error at the given points. Returns the mean squared error.
            // If 'finalize' is set, computes regressed targets - only set to false if
            // you are doing repeated updates, and be sure to set true on final update.
            double UpdateTargets(const std::vector<Eigen::VectorXd>& points,
                                 const std::vector<double>& targets,
                                 double step_size, bool finalize = true);

            // Learn kernel hyperparameters by maximizing log-likelihood of the
            // training data.
            bool LearnHyperparams();

            // Immutable accessors
            const Eigen::MatrixXd& ImmutableCovariance() const { return covariance_; };
            const Eigen::VectorXd& ImmutableRegressedTargets() const { return regressed_; };
            const Eigen::VectorXd& ImmutableTargerts() const { return targets_; };
            const std::shared_ptr<const std::vector<Eigen::VectorXd>> ImmutablePoints() const { return points_; };
            const Eigen::LLT<Eigen::MatrixXd>& ImmutableCholesky() const { return llt_; };
            size_t Dimension() const { return dimension_; };

        private:
            // Compute the covariance and cross covariance against the trainning points.
            void Covariance();
            void CrossCovariance(const Eigen::VectorXd& x, Eigen::VectorXd& cross) const;

            // Kernel.
            const Kernel::Ptr kernel_;

            // Noise variance
            double noise_;

            // Trainning points, targets, and regressed targets (inv(cov) * targets).
            const std::shared_ptr<std::vector<Eigen::VectorXd>> points_;
            size_t dimension_;
            Eigen::VectorXd targets_;
            Eigen::VectorXd regressed_;

            // Maximum number of points.
            const size_t max_points_;

            // Covariance matrix, with Cholesky decomposition.
            Eigen::MatrixXd covariance_;
            Eigen::LLT<Eigen::MatrixXd> llt_;
    };
}

#endif /* GAUSSIAN_PROCESS_HPP */


/**
 * @file
 * @author Alexander Sherikov
 * @date 02.12.2011 15:34:06 MSK
 */

/****************************************
 * INCLUDES
 ****************************************/
#include <stdio.h>
#include <Eigen/Core>
#include <Eigen/Cholesky>

/****************************************
 * FUNCTIONS
 ****************************************/

/** \brief Print a [m by n] matrix on the screne

    \param[in] m Number of rows
    \param[in] n Number of columns
    \param[in] A Matrix of dimension [m by n]
    \param[in] description A short description of the array (it is printed above the array)

    \note This is the same function as void Matrix_print(unsigned int m, unsigned int n, double * A, const char * description)
    but I changed the name, because if I link to mbs_simulator, I get duplicate symbols

    \return void
*/
void MatrixPrint(unsigned int m, unsigned int n, double * A, const char * description)
{
    unsigned int  i, j;

    printf(" %s", description);
    for (i=0; i<m; i++)
    {
        printf("\n");
        for (j=0; j<n; j++)
            printf("% f ", A[ j*m + i ]);
    }
    printf("\n");
}


// Use, modification and distribution are 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)

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This program computes fixed-point negatable limits for various
// precisions and stores them in partial format for a Quickbook table.

#include <iomanip>
#include <iostream>
#include <limits>

#include <boost/fixed_point/fixed_point.hpp>

template<typename fixed_point_type>
void print_limits()
{
  std::cout << std::setprecision(4)
            << "["
            << "[<" << fixed_point_type::range << ", " << fixed_point_type::resolution << ">] "
            << "["  << fixed_point_type::range << "] "
            << "["  << fixed_point_type::resolution << "] "
            << "["  << std::numeric_limits<fixed_point_type>::digits << "] "
            << "["  << std::numeric_limits<fixed_point_type>::digits +1 << "] "
            << "["  << std::numeric_limits<fixed_point_type>::epsilon() << "] "
            << "["  << std::numeric_limits<fixed_point_type>::lowest() << "] "
            << "["  << (std::numeric_limits<fixed_point_type>::min)() << "] "
            << "["  << (std::numeric_limits<fixed_point_type>::max)() << "]"
            << "]"
            << std::endl
            ;
}

int main()
{
  print_limits<boost::fixed_point::negatable< 15,  -16>>();
  print_limits<boost::fixed_point::negatable< 11,  -20>>();
  print_limits<boost::fixed_point::negatable<  0,  -31>>();
  print_limits<boost::fixed_point::negatable< 30,   -1>>();
  print_limits<boost::fixed_point::negatable< 15, -240>>();
  print_limits<boost::fixed_point::negatable<  0, -255>>();
  print_limits<boost::fixed_point::negatable<200,  -55>>();
  print_limits<boost::fixed_point::negatable<  7,   -8>>();
  print_limits<boost::fixed_point::negatable<  0,   -7>>();
  print_limits<boost::fixed_point::negatable<  2,   -5>>();
  print_limits<boost::fixed_point::negatable<  4,  -11>>();
  print_limits<boost::fixed_point::negatable<  7,  -24>>();
  print_limits<boost::fixed_point::negatable< 10,  -53>>();
  print_limits<boost::fixed_point::negatable< 14, -113>>();
}


#define CERES_FOUND
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/core/cvstd.hpp>
#include <opencv2/opencv.hpp>
#include <unistd.h>
#include <algorithm>

//#include <opencv2/xfeatures2d.hpp>
//#include <opencv2/xfeatures2d/nonfree.hpp>
//#include <opencv2/sfm.hpp>

#include <Eigen/Geometry>

//https://docs.opencv.org/3.1.0/d0/d13/classcv_1_1Feature2D.html
namespace featuresdetectors {enum FEATURES_DETECTORS
{
AgastFeatureDetector,
AKAZE,
BRISK,
FastFeatureDetector,
GFTTDetector,
KAZE,
MSER,
ORB,
SimpleBlobDetector,
MSDDetector,
SIFT,
StarDetector,
SURF
};
}

namespace featuresdescriptor {enum FEATURES_DESCRIPTOR
{
AKAZE,
BRISK,
KAZE,
ORB,
SimpleBlobDetector,
BriefDescriptorExtractor,
DAISY,
FREAK,
LATCH,
LUCID,
MSDDetector,
SIFT,
StarDetector,
SURF
};
}

//void test()
//{
//    double max_dist = 0; double min_dist = 100;

//    //-- Quick calculation of max and min distances between keypoints
//    for( int i = 0; i < descriptors_object.rows; i++ )
//    {   double dist = matches[i].distance;
//        if( dist < min_dist ) min_dist = dist;
//        if( dist > max_dist ) max_dist = dist;
//    }

//    printf("-- Max dist : %f \n", max_dist );
//    printf("-- Min dist : %f \n", min_dist );

//    //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
//    std::vector< cv::DMatch > good_matches;

//    for( int i = 0; i < descriptors_object.rows; i++ )
//    { if( matches[i].distance < 1.5*min_dist )
//        {
//            good_matches.push_back( matches[i]);
//        }
//    }
//}

int dumb(int argc, char ** argv)
{

    /**/

        unsigned int microseconds=1000000;
        cv::VideoCapture webCam(0); // open the default camera
        webCam.set(cv::CAP_PROP_FRAME_WIDTH,640);
        webCam.set(cv::CAP_PROP_FRAME_HEIGHT,480);
        webCam.set(cv::CAP_PROP_FPS, 1);
        //webCam.set(cv::CAP_PROP_MODE, CV_CAP_MODE_YUYV);

        if(!webCam.isOpened())  // check if we succeeded
    //    return;

        cv::namedWindow("camera",1);
        for(;;)
        {
    //        Mat frame;
    //        webCam >> frame; // get a new frame from camera
    //        imshow("camera", frame);
            if(cv::waitKey(200) >= 0) break;

    //        Mat img_1 = imread( argv[1], IMREAD_GRAYSCALE );
    //        Mat img_2 = imread( argv[2], IMREAD_GRAYSCALE );

            cv::Mat img_1, img_2;
            webCam >> img_1;

            usleep(microseconds);

            webCam >> img_2;

            std::vector<cv::Mat> points2d;

            if( !img_1.data || !img_2.data )
            { std::cout<< " --(!) Error reading images " << std::endl; return -1; }
            //-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
            int minHessian = 400;
            cv::Ptr<cv::xfeatures2d::SURF> detector = cv::xfeatures2d::SURF::create();

    //        cv::xfeatures2d::DAISY;

    //SURF
    //SIFT
    //ORB
    //BRISK
    //MSER
    //GFTT
    //Harris
    //Dense
    //SimpleBlob

    //FAST
    //STAR

    //        cv::xfeatures2d::

            cv::Ptr<cv::xfeatures2d::SIFT> SIFT_detector = cv::xfeatures2d::SIFT::create();
            SIFT_detector->defaultNorm();

//            Ptr<ORB> ORB_detector = ORB::create();
//            //ORB_detector->set

//            Ptr<BRISK> BRISK_detector = BRISK::create();

    //        SIFT::create();
            detector->setHessianThreshold(minHessian);
            std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
            cv::Mat descriptors_1, descriptors_2;
//            detector->detectAndCompute( img_1, cv::Mat(), keypoints_1, descriptors_1 );
//            detector->detectAndCompute( img_2, cv::Mat(), keypoints_2, descriptors_2 );

            SIFT_detector->detectAndCompute( img_1, cv::Mat(), keypoints_1, descriptors_1 );
            SIFT_detector->detectAndCompute( img_2, cv::Mat(), keypoints_2, descriptors_2 );

            //-- Step 2: Matching descriptor vectors using FLANN matcher
            cv::FlannBasedMatcher matcher;
            std::vector< cv::DMatch > matches;
            matcher.match( descriptors_1, descriptors_2, matches );
            double max_dist = 0; double min_dist = 100;
            //-- Quick calculation of max and min distances between keypoints
            for( int i = 0; i < descriptors_1.rows; i++ )
            { double dist = matches[i].distance;
              if( dist < min_dist ) min_dist = dist;
              if( dist > max_dist ) max_dist = dist;
            }
            printf("-- Max dist : %f \n", max_dist );
            printf("-- Min dist : %f \n", min_dist );
            //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist,
            //-- or a small arbitary value ( 0.02 ) in the event that min_dist is very
            //-- small)
            //-- PS.- radiusMatch can also be used here.
            std::vector< cv::DMatch > good_matches;
            for( int i = 0; i < descriptors_1.rows; i++ )
            { if( matches[i].distance <= std::max(2*min_dist, 0.02) )
              {
                    good_matches.push_back( matches[i]);
                }
            }

            int number_of_frames=2;
            int number_of_points=good_matches.size();
            int number_of_rows=2;
    //        std::cout <<"good_matches:"<<good_matches.size() <<std::endl;

            cv::Mat_<double> frame1(number_of_rows, number_of_points),frame2(number_of_rows, number_of_points);
            double  p_f1_x,
                    p_f1_y,
                    p_f2_x,
                    p_f2_y;
            for (std::size_t i=0;i<good_matches.size();i++)
            {
                p_f1_x=keypoints_1[good_matches.at(i).queryIdx].pt.x;
                p_f1_y=keypoints_1[good_matches.at(i).queryIdx].pt.y;

                frame1(0,i)=p_f1_x;
                frame1(1,i)=p_f1_y;
            }

            for (std::size_t i=0;i<good_matches.size();i++)
            {
                p_f2_x=keypoints_2[good_matches.at(i).trainIdx].pt.x;
                p_f2_y=keypoints_2[good_matches.at(i).trainIdx].pt.y;
                frame2(0,i)=p_f2_x;
                frame2(1,i)=p_f2_y;
            }
            points2d.clear();

            points2d.push_back(cv::Mat(frame1));
            points2d.push_back(cv::Mat(frame2));

            const double f  = std::atof(argv[1]),
                         cx = std::atof(argv[2]), cy = std::atof(argv[3]);
            cv::Matx33d K = cv::Matx33d( f, 0, cx,
                                 0, f, cy,
                                 0, 0,  1);
            bool is_projective = true;

            std::cout <<"points2d.size():"<<        points2d.size()  <<std::endl;

            std::vector<cv::Mat> Rs_est, ts_est, points3d_estimated;
            //cv::::reconstruct(points2d, Rs_est, ts_est, K, points3d_estimated, is_projective);
            std::cout<<"Rotation:" <<std::endl;
            std::cout<<Rs_est.size() <<std::endl;

            std::cout<<"Translation:" <<std::endl;
            std::cout<<ts_est.size() <<std::endl;

            //-- Draw only "good" matches
            cv::Mat img_matches;
            cv::drawMatches( img_1, keypoints_1, img_2, keypoints_2,
                         good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),
                         std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
            //-- Show detected matches
            cv::imshow( "Good Matches", img_matches );
            cv::imshow( "Diff", img_1-img_2 );
    //        for( int i = 0; i < (int)good_matches.size(); i++ )
    //        {
    //            printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx );
    //        }
            cv::waitKey(10);
    //        return 0;

        }

}

void featureDetection( cv::Mat &image_in, std::vector<cv::KeyPoint> &keypoints,cv::Mat &descriptors,featuresdetectors::FEATURES_DETECTORS featureDetector, featuresdescriptor::FEATURES_DESCRIPTOR featureDescriptor)
{
    cv::Ptr<cv::Feature2D> features_detector;
    cv::Ptr<cv::Feature2D> features_descriptor;

     switch(featureDetector)
    {
        case featuresdetectors::AgastFeatureDetector :
             features_detector = cv::AgastFeatureDetector::create();
            break;
        case featuresdetectors::AKAZE :
            features_detector = cv::AKAZE::create();
            break;
        case featuresdetectors::BRISK:
            features_detector = cv::BRISK::create();
            break;
        case featuresdetectors::FastFeatureDetector:
            features_detector = cv::FastFeatureDetector::create();
            break;
        case featuresdetectors::GFTTDetector:
            features_detector = cv::GFTTDetector::create();
            break;
        case featuresdetectors::KAZE:
            features_detector = cv::KAZE::create();
            break;
        case featuresdetectors::MSER:
            features_detector = cv::MSER::create();
            break;
        case featuresdetectors::ORB:
            features_detector = cv::ORB::create();
            break;
        case featuresdetectors::SimpleBlobDetector:
            features_detector = cv::SimpleBlobDetector::create();
            break;
        case featuresdetectors::MSDDetector:
            features_detector = cv::xfeatures2d::MSDDetector::create();
            break;
        case featuresdetectors::SIFT:
            features_detector = cv::xfeatures2d::SIFT::create();
            break;
        case featuresdetectors::StarDetector:
            features_detector = cv::xfeatures2d::StarDetector::create();
            break;
        case featuresdetectors::SURF:
            features_detector = cv::xfeatures2d::SURF::create();
            break;
    }

    switch(featureDescriptor)
    {
        case featuresdescriptor::AKAZE :
            features_descriptor = cv::AKAZE::create();
            break;
        case featuresdescriptor::BRISK:
            features_descriptor = cv::BRISK::create();
            break;
        case featuresdescriptor::KAZE:
            features_descriptor = cv::KAZE::create();
            break;
        case featuresdescriptor::ORB:
            features_descriptor = cv::ORB::create();
            break;
        case featuresdescriptor::BriefDescriptorExtractor:
            features_descriptor = cv::xfeatures2d::BriefDescriptorExtractor::create();
            break;
        case featuresdescriptor::DAISY:
            features_descriptor = cv::xfeatures2d::DAISY::create();
            break;
        case featuresdescriptor::FREAK:
            features_descriptor = cv::xfeatures2d::FREAK::create();
            break;
        case featuresdescriptor::LATCH:
            features_descriptor = cv::xfeatures2d::LATCH::create();
            break;
        case featuresdescriptor::LUCID:
            features_descriptor = cv::xfeatures2d::LUCID::create();
            break;
        case featuresdescriptor::MSDDetector:
            features_descriptor = cv::xfeatures2d::MSDDetector::create();
            break;
        case featuresdescriptor::SIFT:
            features_descriptor = cv::xfeatures2d::SIFT::create();
            break;
        case featuresdescriptor::StarDetector:
            features_descriptor = cv::xfeatures2d::StarDetector::create();
            break;
        case featuresdescriptor::SURF:
            features_descriptor = cv::xfeatures2d::SURF::create();
            break;
    }

    features_detector->detect(image_in,keypoints,cv::Mat());
    features_descriptor->compute(image_in,keypoints,descriptors);
}

void featureDetection_test(int argc , char ** argv )
{
    cv::Mat image_in;
    cv::Mat image_out;
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;
    std::string file_path=argv[1];
    image_in=cv::imread( file_path,cv::IMREAD_COLOR);
    featureDetection(image_in,keypoints,descriptors,featuresdetectors::AgastFeatureDetector,featuresdescriptor::DAISY);
    cv::drawKeypoints(image_in,keypoints,image_out);
    std::string window_title="Feature";
    cv::imshow(window_title, image_out);
    cv::waitKey(0);
}

void featureMatching( cv::Mat &descriptors_object, cv::Mat descriptors_scene, std::vector< cv::DMatch >& matches)
{

    cv::BFMatcher bruteForceMatching;
    //bruteForceMatching.match();
    //bruteForceMatching.knnMatch();
    cv::FlannBasedMatcher matcher;

    //matcher.radiusMatch();

    matcher.match( descriptors_object, descriptors_scene, matches );

}

void refineMathes(std::vector< cv::DMatch > &matches,cv::Mat &object_descriptors ,std::vector< cv::DMatch > &good_matches)
{

    double max_dist = 0; double min_dist = 100;

    std::vector<double> vec;

    //-- Quick calculation of max and min distances between keypoints
    for( int i = 0; i < object_descriptors.rows; i++ )
    {   double dist = matches[i].distance;
        if( dist < min_dist ) min_dist = dist;
        if( dist > max_dist ) max_dist = dist;
        vec.push_back(dist);
    }

//    size_t midIndex = vec.size()/9;
    size_t midIndex = 40;
    std::nth_element(vec.begin(), vec.begin() + midIndex, vec.end());

//    printf("-- Max dist : %f \n", max_dist );
//    printf("-- Min dist : %f \n", min_dist );
//    std::cout<<"the 5the element is: "<<vec[midIndex] <<std::endl;

    //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )

    for( int i = 0; i < object_descriptors.rows; i++ )
    {
        if( matches[i].distance < vec[midIndex] )
        {
            good_matches.push_back( matches[i]);
        }
    }
}

void featureMatching_Test(int argc, char ** argv)
{
    cv::Mat image_object,image_scene;
    std::vector<cv::KeyPoint> object_keypoints,scene_keypoints;
    cv::Mat object_descriptors, scene_descriptors;

    image_scene=cv::imread( argv[1],cv::IMREAD_COLOR);
    image_object=cv::imread( argv[2],cv::IMREAD_COLOR);

    featureDetection(image_object,object_keypoints,object_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);
    featureDetection(image_scene,scene_keypoints,scene_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);

    std::vector< cv::DMatch > matches;
    featureMatching(object_descriptors, scene_descriptors, matches);

//    std::cout<<"matches"<<matches.size() <<std::endl;
    std::vector< cv::DMatch > good_matches;

    refineMathes(matches,object_descriptors ,good_matches);

    cv::Mat img_matches;
    cv::drawMatches( image_object, object_keypoints, image_scene, scene_keypoints,
    good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),
    std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

    std::string window_title="matches";
    cv::imshow(window_title, img_matches);
    cv::waitKey(0);
    cv::imwrite("mathes.jpg",img_matches);

    std::vector<cv::Point2f> points1Raw; //Raw points from Keypoints
    std::vector<cv::Point2f> points1; //Undistorted points
    std::vector<cv::Point2f> points2Raw;
    std::vector<cv::Point2f> points2;
    for(int k=0; k<good_matches.size(); k++)
    {
        points1Raw.push_back(object_keypoints[good_matches[k].queryIdx].pt);
        points2Raw.push_back(scene_keypoints[good_matches[k].trainIdx].pt);
    }

//    cv::undistortPoints(points1Raw, points1, cameraMatrixm, distCoeffsm);
//    cv::undistortPoints(points2Raw, points2, cameraMatrixm, distCoeffsm);

    points1=points1Raw;
    points2=points2Raw;
    std::vector<uchar> states;

//https://stackoverflow.com/questions/25251676/opencv-findfundamentalmat-very-unstable-and-sensitive

//    cv::Mat f = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, states);

    cv::Mat f = cv::findFundamentalMat(points1, points2, cv::FM_LMEDS, 3, 0.99, states);

//    cv::Mat f = cv::findFundamentalMat(points1, points2, cv::FM_7POINT, 3, 0.99, states);

//    cv::Mat f = cv::findFundamentalMat(points1, points2, cv::FM_8POINT, 3, 0.99, states);

    std::cout<<"Fundamental Mat is: " <<f <<std::endl;

    double err=0;
    for(int k=0; k<good_matches.size(); k++)
    {
        cv::Mat p1(3, 1, CV_64F);
        p1.at<double>(0, 0) = points1[k].x;
        p1.at<double>(1, 0) = points1[k].y;
        p1.at<double>(2, 0) = 1;
        cv::Mat p2(1, 3, CV_64F);
        p2.at<double>(0, 0) = points2[k].x;
        p2.at<double>(0, 1) = points2[k].y;
        p2.at<double>(0, 2) = 1;

        cv::Mat res = cv::abs(p2 * f * p1); // f computed matrix

        if((bool)states[k]) //if match considered inlier (in my strange case all)
            err = err + res.at<double>(0, 0); //accumulate errors

    }

    std::cout<<"Total error is: " <<err <<std::endl;

    double focal=1.0;
    cv::Point2d pp=cv::Point2d(10, 10);
    double threshold=1;
    double prob=0.999;
    cv::LMEDS;
    cv::Mat E,R,t,mask;

    E= cv::findEssentialMat(points1, points2,focal,pp, cv::RANSAC,prob,threshold,mask);
    std::cout<<"E: " <<E<<std::endl;
    cv::recoverPose(E, points1, points2, R, t, focal, pp, mask);
    std::cout<<"R: " <<R<<std::endl;
    std::cout<<"t: " <<t<<std::endl;

}

void findFundamentalMatrix(cv::Mat &image1,cv::Mat &image2,cv::Mat &fundamentalMatrix,double &error)
{
    //1)featureDetection
    std::vector<cv::KeyPoint> image1_keypoints,image2_keypoints;
    cv::Mat image1_descriptors,image2_descriptors;

    featureDetection(  image1, image1_keypoints,image1_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);
    featureDetection(  image2, image2_keypoints,image2_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);

    //2)featureMatching
    std::vector< cv::DMatch > matches;

    featureMatching( image1_descriptors, image2_descriptors,  matches);

    //)refineMathes
    std::vector< cv::DMatch > good_matches;
    refineMathes(matches,image2_descriptors, good_matches);

    std::vector<cv::Point2f> points1Raw; //Raw points from Keypoints
    std::vector<cv::Point2f> points1; //Undistorted points
    std::vector<cv::Point2f> points2Raw;
    std::vector<cv::Point2f> points2;
    for(int k=0; k<good_matches.size(); k++)
    {
        points1Raw.push_back(image1_keypoints[good_matches[k].queryIdx].pt);
        points2Raw.push_back(image2_keypoints[good_matches[k].trainIdx].pt);
    }

    //4)undistortion
//    cv::undistortPoints(points1Raw, points1, cameraMatrixm, distCoeffsm);
//    cv::undistortPoints(points2Raw, points2, cameraMatrixm, distCoeffsm);

    points1=points1Raw;
    points2=points2Raw;
    std::vector<uchar> states;

    //5)findFundamentalMat
//https://stackoverflow.com/questions/25251676/opencv-findfundamentalmat-very-unstable-and-sensitive
//  cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, states);
//  cv::findFundamentalMat(points1, points2, cv::FM_7POINT, 3, 0.99, states);
//  cv::findFundamentalMat(points1, points2, cv::FM_8POINT, 3, 0.99, states);
    fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_LMEDS, 3, 0.99, states);
    for(int k=0; k<good_matches.size(); k++)
    {
        cv::Mat p1(3, 1, CV_64F);
        p1.at<double>(0, 0) = points1[k].x;
        p1.at<double>(1, 0) = points1[k].y;
        p1.at<double>(2, 0) = 1;
        cv::Mat p2(1, 3, CV_64F);
        p2.at<double>(0, 0) = points2[k].x;
        p2.at<double>(0, 1) = points2[k].y;
        p2.at<double>(0, 2) = 1;
        cv::Mat res = cv::abs(p2 * fundamentalMatrix * p1); // f computed matrix
        if((bool)states[k]) //if match considered inlier (in my strange case all)
            error = error + res.at<double>(0, 0); //accumulate errors
    }

}

void findFundamentalMatrix_test(int argc, char ** argv)
{
    cv::Mat image1=cv::imread( argv[1],cv::IMREAD_COLOR);
    cv::Mat image2=cv::imread( argv[2],cv::IMREAD_COLOR);
    cv::Mat fundamentalMatrix;
    double error;
    findFundamentalMatrix(image1,image2,fundamentalMatrix,error);
    std::cout<<"Fundamental Matrix is: " <<fundamentalMatrix <<std::endl;
    std::cout<<"error is: " <<error <<std::endl;
}

void findEssentialMatrix(cv::Mat &image1,cv::Mat &image2, cv::Mat &cameraMatrix, cv::Mat &EssentialMatrix,cv::Mat &rotation,cv::Mat &translation)
{
    //1)featureDetection
    std::vector<cv::KeyPoint> image1_keypoints,image2_keypoints;
    cv::Mat image1_descriptors,image2_descriptors;

    featureDetection(  image1, image1_keypoints,image1_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);
    featureDetection(  image2, image2_keypoints,image2_descriptors,featuresdetectors::SIFT,featuresdescriptor::SURF);

    //2)featureMatching
    std::vector< cv::DMatch > matches;

    featureMatching( image1_descriptors, image2_descriptors,  matches);

    //)refineMathes
    std::vector< cv::DMatch > good_matches;
    refineMathes(matches,image2_descriptors, good_matches);

    std::vector<cv::Point2f> points1Raw; //Raw points from Keypoints
    std::vector<cv::Point2f> points1; //Undistorted points
    std::vector<cv::Point2f> points2Raw;
    std::vector<cv::Point2f> points2;
    for(int k=0; k<good_matches.size(); k++)
    {
        points1Raw.push_back(image1_keypoints[good_matches[k].queryIdx].pt);
        points2Raw.push_back(image2_keypoints[good_matches[k].trainIdx].pt);
    }

    //4)undistortion
//    cv::undistortPoints(points1Raw, points1, cameraMatrixm, distCoeffsm);
//    cv::undistortPoints(points2Raw, points2, cameraMatrixm, distCoeffsm);

    points1=points1Raw;
    points2=points2Raw;
    std::vector<uchar> states;

    double focal=1.0;
    cv::Point2d pp=cv::Point2d(10, 10);
    double threshold=1;
    double prob=0.999;
    cv::LMEDS;
    cv::Mat mask;

    EssentialMatrix= cv::findEssentialMat(points1, points2,focal,pp, cv::RANSAC,prob,threshold,mask);
    cv::recoverPose(EssentialMatrix, points1, points2, rotation, translation, focal, pp, mask);
}

void findEssentialMatrix_Test(int argc, char** argv)
{
    std::string camera_calibration_path="front_webcam.yml";
    cv::FileStorage fs(camera_calibration_path,cv::FileStorage::READ);
    cv::Mat camera_matrix, distortion_coefficient;
    fs["camera_matrix"]>>camera_matrix;
    fs["distortion_coefficients"]>>distortion_coefficient;

//    std::cout<<"R: " <<R<<std::endl;
//    std::cout<<"t: " <<t<<std::endl;
}

void test()
{
//    unsigned int microseconds=1000000;
    unsigned int microseconds=0;
    cv::VideoCapture webCam(1); // open the default camera
    webCam.set(CV_CAP_PROP_FRAME_WIDTH,640);
    webCam.set(CV_CAP_PROP_FRAME_HEIGHT,480);
    webCam.set(CV_CAP_PROP_FPS, 30);

    if(!webCam.isOpened())  // check if we succeeded
        return;
//    cv::namedWindow("camera",1);
    for(;;)
    {

        try
        {

            if(cv::waitKey(200) >= 0) break;
            cv::Mat image1, image2;
            webCam >> image1;
            usleep(microseconds);
            webCam >> image2;

            std::vector<cv::KeyPoint> image1_keypoints,image2_keypoints;
            cv::Mat image1_descriptors, image2_descriptors;

            featureDetection(image1,image1_keypoints,image1_descriptors,featuresdetectors::SIFT,featuresdescriptor::DAISY);
            featureDetection(image2,image2_keypoints,image2_descriptors,featuresdetectors::SIFT,featuresdescriptor::DAISY);

//            std::cout<<"image1_keypoints.size()"<<image1_keypoints.size() <<std::endl;
//            std::cout<<"image2_keypoints.size()"<<image2_keypoints.size() <<std::endl;

            std::vector< cv::DMatch > matches;
            featureMatching(image1_descriptors, image2_descriptors, matches);

            if(matches.size()<20)
                continue;

        //    std::cout<<"matches"<<matches.size() <<std::endl;
            std::vector< cv::DMatch > good_matches;

    //        std::cout<<"matches.size()"<<matches.size() <<std::endl;

            refineMathes(matches,image1_descriptors ,good_matches);

            if(good_matches.size()<9)
                continue;
    //        std::cout<<"good_matches.size()"<<good_matches.size() <<std::endl;
    /**/
            cv::Mat img_matches;
            cv::drawMatches( image1, image1_keypoints, image2, image2_keypoints,
            good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),
            std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

            std::vector<cv::Point2f> points1Raw; //Raw points from Keypoints
            std::vector<cv::Point2f> points1; //Undistorted points
            std::vector<cv::Point2f> points2Raw;
            std::vector<cv::Point2f> points2;
            for(int k=0; k<good_matches.size(); k++)
            {
                points1Raw.push_back(image1_keypoints[good_matches[k].queryIdx].pt);
                points2Raw.push_back(image2_keypoints[good_matches[k].trainIdx].pt);
            }

            //4)undistortion
        //    cv::undistortPoints(points1Raw, points1, cameraMatrixm, distCoeffsm);
        //    cv::undistortPoints(points2Raw, points2, cameraMatrixm, distCoeffsm);

            points1=points1Raw;
            points2=points2Raw;
            std::vector<uchar> states;

            cv::Mat f = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 1, 0.99, states);

            double err=0;
            for(int k=0; k<good_matches.size(); k++)
            {
                cv::Mat p1(3, 1, CV_64F);
                p1.at<double>(0, 0) = points1[k].x;
                p1.at<double>(1, 0) = points1[k].y;
                p1.at<double>(2, 0) = 1;
                cv::Mat p2(1, 3, CV_64F);
                p2.at<double>(0, 0) = points2[k].x;
                p2.at<double>(0, 1) = points2[k].y;
                p2.at<double>(0, 2) = 1;

                cv::Mat res = cv::abs(p2 * f * p1); // f computed matrix

                if((bool)states[k]) //if match considered inlier (in my strange case all)
                    err = err + res.at<double>(0, 0); //accumulate errors

            }

//            std::cout<<"Total error is: " <<err <<std::endl;

    ////        std::string camera_calibration_path="/home/behnam/workspace/OpenCVProjects/build/front_webcam.yml";
    ////        cv::FileStorage fs(camera_calibration_path,cv::FileStorage::READ);
    ////        cv::Mat camera_matrix, distortion_coefficient;
    ////        fs["camera_matrix"]>>camera_matrix;
    ////        fs["distortion_coefficients"]>>distortion_coefficient;

    ///*
    // camera_matrix:
    //    |fx   0   cx|
    //    |0   fy   cy|
    //    |0    0    1|

    //*/

            double cx,cy,focal;
            cv::Point2d pp;

    //        focal=camera_matrix.at<double>(1,1);

    //        cx=camera_matrix.at<double>(0,2);
    //        cy=camera_matrix.at<double>(1,2);
//            cv::Point2d pp=cv::Point2d(cx, cy);

            //normalize coordinates

            focal=1036.169926;
            cx=308.412977;
            cy=270.068003;
            pp=cv::Point2d(cx,cy);

            //(do not normalize coordinates)
//            cx=320;
//            cy=240;
//            focal=1;
            //http://answers.opencv.org/question/65788/undistortpoints-findessentialmat-recoverpose-what-is-the-relation-between-their-arguments/
            //http://answers.opencv.org/question/179981/does-recoverpose-return-up-to-scale-or-correct-translation/
            //https://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#decomposeessentialmat
            //https://stackoverflow.com/questions/23114047/opencv-camera-relative-pose-estimation
            //https://github.com/PacktPublishing/OpenCV3-Computer-Vision-Application-Programming-Cookbook-Third-Edition

            //By decomposing E, you can only get the direction of the translation, so the function returns unit

            double threshold=1;
            double prob=0.999;
            cv::LMEDS;
            cv::Mat mask,rotation,translation;

            cv::Mat EssentialMatrix= cv::findEssentialMat(points1, points2,focal,pp, cv::LMEDS,prob,threshold,mask);

//            cv::correctMatches(E, imgpts1, imgpts2, imgpts1, imgpts2)

//  The R and t are the rotation and translation (in camera 2's coordinates) to get to camera 1.

            cv::recoverPose(EssentialMatrix, points1, points2, rotation, translation, focal, pp, mask);

    //        Eigen::Quaternion<double> q =  yawAngle*pitchAngle *rollAngle;

            Eigen::Matrix3d rotationMatrix;
            rotationMatrix(0,0)=rotation.at<double>(0,0);
            rotationMatrix(0,1)=rotation.at<double>(0,1);
            rotationMatrix(0,2)=rotation.at<double>(0,2);
            rotationMatrix(1,0)=rotation.at<double>(1,0);
            rotationMatrix(1,1)=rotation.at<double>(1,1);
            rotationMatrix(1,2)=rotation.at<double>(1,2);
            rotationMatrix(2,0)=rotation.at<double>(2,0);
            rotationMatrix(2,1)=rotation.at<double>(2,1);
            rotationMatrix(2,2)=rotation.at<double>(2,2);

            std::cout.precision(3);
            std::cout.setf(std::ios_base::fixed);

//            std::cout<<rotationMatrix <<std::endl;

//            Eigen::Quaterniond quaternion_mat(rotationMatrix);
//            std::cout<<"quaternion_mat.x(): " <<quaternion_mat.x()<<std::endl;
//            std::cout<<"quaternion_mat.y(): " <<quaternion_mat.y()<<std::endl;
//            std::cout<<"quaternion_mat.z(): " <<quaternion_mat.z()<<std::endl;
//            std::cout<<"quaternion_mat.w(): " <<quaternion_mat.w()<<std::endl;
//            std::cout<<"--------------------------" <<std::endl;
//            std::cout<<"translation: " <<std::endl;
//            //cv::norm(translation, cv::NORM_L2, cv::Mat());

            double minVal;
            double maxVal=0;
            cv::Point minLoc;
            cv::Point maxLoc;
            double scale=100;

            cv::minMaxLoc(translation, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
            translation=translation/(std::abs(maxVal)*scale);

            std::cout.precision(3);
            std::cout.setf(std::ios_base::fixed);

            std::cout<<translation.at<double>(0,0)<<std::endl;
//            std::cout<<translation.at<double>(1,0)<<std::endl;
//            std::cout<<translation.at<double>(2,0)<<std::endl;

//            std::cout<<"roll is: " <<atan2( rotationMatrix(2,1),rotationMatrix(2,2) ) <<std::endl;
//            std::cout<<"pitch: " <<atan2( -rotationMatrix(2,0), std::pow( rotationMatrix(2,1)*rotationMatrix(2,1) +rotationMatrix(2,2)*rotationMatrix(2,2) ,0.5  )  ) <<std::endl;
//            std::cout<<"yaw is: " <<atan2( rotationMatrix(1,0),rotationMatrix(0,0) ) <<std::endl;

//            std::cout<<"EssentialMatrix: "<<std::endl;
//            std::cout<<EssentialMatrix<<std::endl;
////            std::cout<<"f: "<<std::endl;
////            std::cout<<f<<std::endl;

            std::string window_title="matches";
            cv::imshow(window_title, img_matches);

        }
               catch (const std::exception& e)
               { /* */ }

////        cv::waitKey(0);

    }
}

int main(int argc , char ** argv)
{
    //dumb(argc, argv);
//    featureDetection_test(argc, argv);
//    featureMatching_Test(argc, argv);

    test();
}


/*
 * <one line to give the library's name and an idea of what it does.>
 * Copyright (C) 2016  <copyright holder> <email>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "impact.h"

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

using namespace mia;

bool mia::impact(const Segment2 & s1, const Segment2 & s2)
{
  std::list<Float2> X;
  return ( boost::geometry::intersection(s1, s2, X) && !X.empty() );
}

bool mia::impact(const Segment2 & s1, const Segment2 & s2, Float2 & point)
{
  std::list<Float2> X;
  if( boost::geometry::intersection(s1, s2, X) && !X.empty() )
  {
    point= *(X.begin());
    return true;
  }
  return false;
}

bool mia::impact(const Circle2 & c1, const Circle2 & c2)
{
    Float2 between= c2.center - c1.center;
    float distMin= c1.radius + c2.radius;

    return between.length2() < (distMin*distMin);
}

bool mia::impact(const Circle2 & c1, const Circle2 & c2, Float2 & point)
{
    Float2 between= c2.center - c1.center;
    float distMin= c1.radius + c2.radius;
    bool out(false);

    if( between.length2() < (distMin*distMin) )
    {
      point= c2.center + (between*0.5f);
      out= true;
    }
    return out;
}

bool mia::impact(const Float2 & v, const Circle2 & c){
  Float2 d(v - c.center);
  return d.length2() < (c.radius*c.radius);
}

bool mia::impact(const Segment2 & s, const Circle2 & c){
  Float2 out;
  return impact(s, c, out);
}

bool mia::impact(const Segment2 & s, const Circle2 & c, Float2 & point){

    Float2 radius= s.b - s.a;
    radius.orthonormalize(c.radius);
    Segment2 diametre( c.center - radius, c.center + radius );

    if ( impact( s, diametre, point ) )
    {
        return true;
    }

    if( impact( s.a, c ) )
    {
        point= s.a;
        return true;
    }

    if( impact( s.b, c ) )
    {
        point= s.b;
        return true;
    }

    return false;
}

bool mia::impact(const Polygon2 & p1, const Polygon2 & p2)
{
  std::list<Polygon2> output;
  return (boost::geometry::intersection(p1, p2, output) && !output.empty() );
}

bool mia::impact(const Polygon2 & p1, const Polygon2 & p2, std::list<Polygon2> & output)
{
  return (boost::geometry::intersection(p1, p2, output) && !output.empty() );
}

// bool mia::impact(const Polygon2 & p1, Polygon2 & p2, Float2 & point)
// {
//   std::list<Polygon2> output;
//   boost::geometry::intersection(p1, p2, output);
//   compute mean point of output;
//   return output.empty();
// }


// Copyright (c) 2020 by Ignacio Alzugaray <alzugaray dot ign at gmail dot com>
// ETH Zurich, Vision for Robotics Lab.

#pragma once

#include <Eigen/Core>
#include <x/haste/types/event.hpp>

namespace haste {
template<typename Event_, size_t Size_>
class FixedSizeLocationEventWindowType {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static constexpr size_t kSize = Size_;
  static_assert(kSize % 2 == 1, "EventWindow kSize parameter must be an odd");

  using Event = Event_;
  using Time = typename Event::Time;
  using Location = typename Event::Location;
  using EventTuple = std::tuple<Time, Location, Location>;

  template<typename T>
  using EventWindowVector = Eigen::Array<T, kSize, 1>;
  using TimeVector = EventWindowVector<Time>;
  using LocationVector = EventWindowVector<Location>;

  static constexpr size_t kOldestEventIdx = 0;
  static constexpr size_t kNewestEventIdx = kSize - 1;
  static constexpr size_t kMiddleEventIdx = (kSize - 1) / 2;

  FixedSizeLocationEventWindowType();

  // TODO(ialzugaray): template to direct access without getEvent
  auto getEvent(const size_t &idx) const -> EventTuple;
  auto oldestEvent() const -> EventTuple;
  auto newestEvent() const -> EventTuple;
  auto middleEvent() const -> EventTuple;

  auto setEvent(const Time &et, const Location &ex, const Location &ey, const size_t &idx) -> void;
  auto setEvent(const EventTuple &event, const size_t &idx) -> void;
  auto appendEvent(const Time &et, const Location &ex, const Location &ey) -> EventTuple;
  auto appendEvent(const EventTuple &new_event) -> EventTuple;

  // TODO(ialzugaray): convert to const ref getters. Maybe transpose the underlying data depending on use
  auto et_vec() const -> TimeVector { return et_vec_; };
  auto ex_vec() const -> LocationVector { return ex_vec_; };
  auto ey_vec() const -> LocationVector { return ey_vec_; };

 protected:
  TimeVector et_vec_;
  LocationVector ex_vec_;
  LocationVector ey_vec_;
};

}// namespace haste

#include <x/haste/core/event_window_impl.hpp>


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html [^]
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE. See the above copyright notices for more information.
//
//------------------------------------------------------------------------

// this file defines the rttbCoreTests for the test driver
// and all it expects is that you have a function called RegisterTests

#include "litCheckMacros.h"
#include "rttbDoseIndex.h"
#include "rttbDVHSet.h"
#include "rttbBaseType.h"
#include "rttbNullPointerException.h"
#include "rttbCoverageIndex.h"
#include "rttbException.h"
#include "rttbInvalidParameterException.h"
#include "rttbDVHXMLFileReader.h"

#include <math.h>

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

namespace rttb
{
	namespace testing
	{
		/*! @brief Test of CoverageIndex.
		test dvh: deltaV 0.125, deltaD 0.5
		1. dvh TV: number of voxels 2900,  maximum dose bin 133, dose bin 127~133
		2. dvh HT1: number of voxels 5410, maximum dose bin 40, dose bin 0~2,40
		3. dvh HT2: number of voxels 10210, maximum dose bin 50, dose bin 0~2,50
		4. dvh HT3: number of voxels 1210, maximum dose bin 70, dose bin 0~2,70

		Test if calculation in new architecture returns similar results to the
		original implementation.

		WARNING: The values for comparison need to be adjusted if the input files are changed!
		*/
		int CoverageIndexTest(int argc, char* argv[])
		{
			PREPARE_DEFAULT_TEST_REPORTING;

			//ARGUMENTS: 1: ptv dvh file name
			//           2: normal tissue 1 dvh file name
			//           3: normal tissue 2 dvh file name
			//           4: normal tissue 3 dvh file name

			std::string DVH_FILENAME_PTV;
			std::string DVH_FILENAME_NT1;
			std::string DVH_FILENAME_NT2;
			std::string DVH_FILENAME_NT3;

			if (argc > 1)
			{
				DVH_FILENAME_PTV = argv[1];
			}

			if (argc > 2)
			{
				DVH_FILENAME_NT1 = argv[2];
			}

			if (argc > 3)
			{
				DVH_FILENAME_NT2 = argv[3];
			}

			if (argc > 4)
			{
				DVH_FILENAME_NT3 = argv[4];
			}

			/*test dvh: deltaV 0.125, deltaD 0.5*/
			/*dvh TV: number of voxels 2900,  maximum dose bin 133, dose bin 127~133*/
			rttb::io::other::DVHXMLFileReader dvhReader = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_PTV);
			rttb::core::DVH dvhPTV = *(dvhReader.generateDVH());
			/*dvh HT1: number of voxels 5410, maximum dose bin 40, dose bin 0~2,40*/
			rttb::io::other::DVHXMLFileReader dvhReader1 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT1);
			core::DVH dvhNT1 = *(dvhReader1.generateDVH());
			/*dvh HT2: number of voxels 10210, maximum dose bin 50, dose bin 0~2,50*/
			rttb::io::other::DVHXMLFileReader dvhReader2 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT2);
			core::DVH dvhNT2 = *(dvhReader2.generateDVH());
			/*dvh HT3: number of voxels 1210, maximum dose bin 70, dose bin 0~2,70*/
			rttb::io::other::DVHXMLFileReader dvhReader3 = rttb::io::other::DVHXMLFileReader(DVH_FILENAME_NT3);
			core::DVH dvhNT3 = *(dvhReader3.generateDVH());

			std::vector<core::DVH> dvhTVSet;
			std::vector<core::DVH> dvhHTSet;
			dvhTVSet.push_back(dvhPTV);
			dvhHTSet.push_back(dvhNT1);
			dvhHTSet.push_back(dvhNT2);
			dvhHTSet.push_back(dvhNT3);

			::boost::shared_ptr<core::DVHSet> dvhSetPtr = ::boost::make_shared<core::DVHSet>(dvhTVSet, dvhHTSet,
			        "testStrSet", dvhPTV.getDoseID());

			/*test exception*/
			::boost::shared_ptr<core::DVHSet> dvhSetNullPtr;
			CHECK_THROW_EXPLICIT(indices::CoverageIndex(dvhSetNullPtr, 0), core::InvalidParameterException);

			/*test index calculation*/

			/*CoverageIndex*/
			indices::CoverageIndex coverageI = indices::CoverageIndex(dvhSetPtr, 30);
			CHECK_CLOSE(362.5 / 362.5, coverageI.getValue(), errorConstant); //ref dose: 30 -> coverage index=1

			coverageI.setDoseReference(65);
			CHECK_CLOSE(2300 / 2900.0, coverageI.getValue(),
			            errorConstant); //ref dose: 65->coverage index=2300/2900

			CHECK_EQUAL(coverageI.getValue(), coverageI.getValueAt(0));
			CHECK_THROW_EXPLICIT(coverageI.getValueAt(1), core::InvalidParameterException);

			RETURN_AND_REPORT_TEST_SUCCESS;

		}

	}//testing
}//rttb


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

#include <gtest/gtest.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/Lie/SE.h>
#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

// List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
// degrees)
static const std::vector<mrpt::poses::CPose3D> ptc = {
	{.0, .0, .0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, 10.0_deg, .0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, 1.0_deg, .0_deg},
	{1.0, 2.0, 3.0, .0_deg, .0_deg, 1.0_deg},
	{-6.0, 2.0, 3.0, 40.0_deg, 20.0_deg, 15.0_deg},
	{1.0, 8.0, 0.0, -45.0_deg, 10.0_deg, 70.0_deg},
	{1.0, 2.0, 3.0, 89.0_deg, .5_deg, 1.0_deg},
	{1.0, -2.0, 0.4, -89.0_deg, .5_deg, 1.0_deg},
	{1.0, 2.0, 3.0, .1_deg, 89.0_deg, 1.0_deg},
	{1.0, -2.0, 0.4, .1_deg, -88.0_deg, 1.0_deg},
	{1.0, 2.0, 3.0, .1_deg, .9_deg, 178.0_deg},
	{1.0, -2.0, 0.4, .1_deg, .9_deg, -179.1_deg},
};

template <class POSE_TYPE>
class SE_traits_tests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	using SE_TYPE = mrpt::poses::Lie::SE<POSE_TYPE::rotation_dimensions>;

	struct TParams
	{
		typename SE_TYPE::type P1, D, P2;
	};

	template <unsigned int MAT_LEN>
	struct TParamsMat
	{
		CVectorFixedDouble<MAT_LEN> Avec, Bvec;
	};

	static void func_numeric_DinvP1InvP2(
		const CVectorFixedDouble<2 * SE_TYPE::DOFs>& x, const TParams& params,
		CVectorFixedDouble<SE_TYPE::DOFs>& Y)
	{
		typename SE_TYPE::tangent_vector eps1, eps2;
		for (size_t i = 0; i < SE_TYPE::DOFs; i++)
		{
			eps1[i] = x[0 + i];
			eps2[i] = x[SE_TYPE::DOFs + i];
		}

		const POSE_TYPE incr1 = SE_TYPE::exp(eps1);
		const POSE_TYPE incr2 = SE_TYPE::exp(eps2);

		const POSE_TYPE P1 = params.P1 + incr1;
		const POSE_TYPE P2 = params.P2 + incr2;
		const POSE_TYPE& Pd = params.D;

		CMatrixDouble44 P1_inv_hm, P2hm, Pd_inv_hm;
		P1.getInverseHomogeneousMatrix(P1_inv_hm);
		P2.getHomogeneousMatrix(P2hm);
		Pd.getInverseHomogeneousMatrix(Pd_inv_hm);

		const CPose3D DinvP1invP2_(CMatrixDouble44(
			Pd_inv_hm.asEigen() * P1_inv_hm.asEigen() * P2hm.asEigen()));
		const POSE_TYPE DinvP1invP2(DinvP1invP2_);

		// Pseudo-logarithm:
		Y = SE_TYPE::log(DinvP1invP2);
	}

	void test_jacobs_DinvP1InvP2(
		const CPose3D& P1_, const CPose3D& Pd_, const CPose3D& P2_)
	{
		const POSE_TYPE P1(P1_);
		const POSE_TYPE Pd(Pd_);
		const POSE_TYPE P2(P2_);
		const POSE_TYPE Pdinv = -Pd;

		static const int DIMS = SE_TYPE::DOFs;

		// Theoretical results:
		CMatrixFixed<double, DIMS, DIMS> J1, J2;
		SE_TYPE::jacob_dDinvP1invP2_de1e2(Pdinv, P1, P2, J1, J2);

		// Numerical approx:
		CMatrixFixed<double, DIMS, DIMS> num_J1, num_J2;
		{
			CVectorFixedDouble<2 * DIMS> x_mean;
			for (int i = 0; i < DIMS + DIMS; i++) x_mean[i] = 0;

			TParams params;
			params.P1 = P1;
			params.D = Pd;
			params.P2 = P2;

			CVectorFixedDouble<DIMS + DIMS> x_incrs;
			x_incrs.fill(1e-4);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<2 * SE_TYPE::DOFs>& x,
					const TParams& params,
					CVectorFixedDouble<SE_TYPE::DOFs>& Y)>(
					&func_numeric_DinvP1InvP2),
				x_incrs, params, numJacobs);

			num_J1 = numJacobs.asEigen().block<DIMS, DIMS>(0, 0);
			num_J2 = numJacobs.asEigen().block<DIMS, DIMS>(0, DIMS);
		}

		const double max_eror = 1e-3;

		EXPECT_NEAR(0, (num_J1 - J1).sum_abs(), max_eror)
			<< std::setprecision(3) << "p1: " << P1 << endl
			<< "d: " << Pd << endl
			<< "p2: " << P2 << endl
			<< "Numeric J1:\n"
			<< num_J1 << endl
			<< "Implemented J1:\n"
			<< J1 << endl
			<< "Error:\n"
			<< J1 - num_J1 << endl;

		EXPECT_NEAR(0, (num_J2 - J2).sum_abs(), max_eror)
			<< "p1: " << P1 << endl
			<< "d: " << Pd << endl
			<< "p2: " << P2 << endl
			<< "Numeric J2:\n"
			<< num_J2 << endl
			<< "Implemented J2:\n"
			<< J2 << endl
			<< "Error:\n"
			<< J2 - num_J2 << endl;
	}

	static void func_numeric_dAB_dA(
		const CVectorFixedDouble<SE_TYPE::MANIFOLD_DIM>& x,
		const TParamsMat<SE_TYPE::MANIFOLD_DIM>& params,
		CVectorFixedDouble<SE_TYPE::MANIFOLD_DIM>& Y)
	{
		const POSE_TYPE A = SE_TYPE::fromManifoldVector(params.Avec + x);
		const POSE_TYPE B = SE_TYPE::fromManifoldVector(params.Bvec);
		Y = SE_TYPE::asManifoldVector(A + B);
	}

	static void func_numeric_dAB_dB(
		const CVectorFixedDouble<SE_TYPE::MANIFOLD_DIM>& x,
		const TParamsMat<SE_TYPE::MANIFOLD_DIM>& params,
		CVectorFixedDouble<SE_TYPE::MANIFOLD_DIM>& Y)
	{
		const POSE_TYPE A = SE_TYPE::fromManifoldVector(params.Avec);
		const POSE_TYPE B = SE_TYPE::fromManifoldVector(params.Bvec + x);
		Y = SE_TYPE::asManifoldVector(A + B);
	}

	void test_jacobs_AB(const CPose3D& A_, const CPose3D& B_)
	{
		const POSE_TYPE A(A_);
		const POSE_TYPE B(B_);

		constexpr auto N = SE_TYPE::MANIFOLD_DIM;

		// Theoretical results:
		const auto dAB_A = SE_TYPE::jacob_dAB_dA(A, B);
		const auto dAB_B = SE_TYPE::jacob_dAB_dB(A, B);

		// Numerical approx: dAB_A
		CMatrixFixed<double, N, N> num_dAB_A;
		{
			CVectorFixedDouble<N> x_mean;
			x_mean.fill(0);

			TParamsMat<N> params;
			params.Avec = SE_TYPE::asManifoldVector(A);
			params.Bvec = SE_TYPE::asManifoldVector(B);

			CVectorFixedDouble<N> x_incrs;
			x_incrs.fill(1e-4);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<N>& x, const TParamsMat<N>& params,
					CVectorFixedDouble<N>& Y)>(&func_numeric_dAB_dA),
				x_incrs, params, numJacobs);

			num_dAB_A = numJacobs;
		}

		// Numerical approx: dAB_B
		CMatrixFixed<double, N, N> num_dAB_B;
		{
			CVectorFixedDouble<N> x_mean;
			x_mean.fill(0);

			TParamsMat<N> params;
			params.Avec = SE_TYPE::asManifoldVector(A);
			params.Bvec = SE_TYPE::asManifoldVector(B);

			CVectorFixedDouble<N> x_incrs;
			x_incrs.fill(1e-4);
			CMatrixDouble numJacobs;
			mrpt::math::estimateJacobian(
				x_mean,
				std::function<void(
					const CVectorFixedDouble<N>& x, const TParamsMat<N>& params,
					CVectorFixedDouble<N>& Y)>(&func_numeric_dAB_dB),
				x_incrs, params, numJacobs);

			num_dAB_B = numJacobs;
		}

		const double max_eror = 1e-3;

		EXPECT_NEAR(0, (num_dAB_A - dAB_A).sum_abs(), max_eror)
			<< std::setprecision(3) << "A: " << A << endl
			<< "B: " << B << endl
			<< "Numeric dAB_A:\n"
			<< num_dAB_A << endl
			<< "Implemented dAB_A:\n"
			<< dAB_A << endl
			<< "Error:\n"
			<< dAB_A - num_dAB_A << endl;

		EXPECT_NEAR(0, (num_dAB_B - dAB_B).sum_abs(), max_eror)
			<< std::setprecision(3) << "A: " << A << endl
			<< "B: " << B << endl
			<< "Numeric dAB_B:\n"
			<< num_dAB_B << endl
			<< "Implemented dAB_B:\n"
			<< dAB_B << endl
			<< "Error:\n"
			<< dAB_B - num_dAB_B << endl;
	}

	void tests_jacobs_DinvP1InvP2()
	{
		for (const auto& p1 : ptc)
			for (const auto& p2 : ptc)
				for (const auto& pd : ptc) test_jacobs_DinvP1InvP2(p1, pd, p2);
	}

	void tests_jacobs_dAB_dAB()
	{
		for (const auto& p1 : ptc)
			for (const auto& p2 : ptc) test_jacobs_AB(p1, p2);
	}
};

using SE3_traits_tests = SE_traits_tests<mrpt::poses::CPose3D>;
using SE2_traits_tests = SE_traits_tests<mrpt::poses::CPose2D>;

TEST_F(SE3_traits_tests, SE3_jacobs_DinvP1InvP2) { tests_jacobs_DinvP1InvP2(); }
TEST_F(SE3_traits_tests, SE3_jacobs_dAB_dAB) { tests_jacobs_dAB_dAB(); }

TEST_F(SE2_traits_tests, SE2_jacobs_DinvP1InvP2) { tests_jacobs_DinvP1InvP2(); }
TEST_F(SE2_traits_tests, SE2_jacobs_dAB_dAB) { tests_jacobs_dAB_dAB(); }


#pragma once
#include <Eigen/Dense>

#include "./bit_operations.hpp"
namespace yavque
{
namespace detail
{
	template<typename T>
	void swap_qubits(const uint32_t num_qubits, const T* idata, T* odata, uint32_t i,
	                 uint32_t j)
	{
		for(uint32_t n = 0; n < (1u << num_qubits); ++n)
		{
			odata[bitswap(n, i, j)] = idata[n];
		}
	}
}
}


#include <blitzml/dataset/dense_dataset.h>

#include <blitzml/base/math_util.h>

using std::vector;

namespace BlitzML {

size_t coordinates2dense_array_index(index_t row, index_t col, index_t height) {
  size_t offset = static_cast<size_t>(height) * static_cast<size_t>(col);
  return offset + static_cast<size_t>(row);
}

template <typename data_t>
DenseColumn<data_t>::DenseColumn(const data_t *values, index_t length)
    : Column(length, length), values(values) { }

template <typename data_t>
value_t DenseColumn<data_t>::inner_product(const vector<value_t> &vec) const {
  value_t result = 0.0;
  for (index_t i = 0; i < length(); ++i)
    result += values[i] * vec[i];
  return result;
}

template <typename data_t>
value_t DenseColumn<data_t>::weighted_inner_product(
    const vector<value_t> &vec, const vector<value_t> &weights) const {
  value_t result = 0.0;
  for (index_t i = 0; i < length(); ++i)
    result += values[i] * vec[i] * weights[i];
  return result;
}

template <typename data_t>
value_t DenseColumn<data_t>::weighted_norm_sq(
    const vector<value_t> &weights) const {

  value_t result = 0.0;
  for (index_t i = 0; i < length(); ++i)
    result += values[i] * values[i] * weights[i];
  return result;
}

template <typename data_t>
void DenseColumn<data_t>::add_multiple(
    vector<value_t> &target, value_t scalar) const {
  for (index_t i = 0; i < length(); ++i) {
    target[i] += values[i] * scalar;
  }
}

template <typename data_t>
void DenseColumn<data_t>::weighted_add_multiple(
    std::vector<value_t> &target, std::vector<value_t> &weights, value_t scalar) const {
  for (index_t i = 0; i < length(); ++i) {
    target[i] += values[i] * weights[i] * scalar;
  }
}

template <typename data_t>
value_t DenseColumn<data_t>::sum() const {
  return sum_array(values, length());
}

template <typename data_t>
value_t DenseColumn<data_t>::mean() const {
  return this->sum() / length();
}

template <typename data_t>
value_t DenseColumn<data_t>::l2_norm_sq() const {
  return BlitzML::l2_norm_sq(values, length());
}

template class DenseColumn<double>;
template class DenseColumn<float>;
template class DenseColumn<int>;
template class DenseColumn<bool>;

template<typename data_t>
DenseDataset<data_t>::DenseDataset(const data_t *data,
    index_t height, index_t width, const value_t *b, index_t length_b)
    : Dataset(height, width,
              coordinates2dense_array_index(0, width, height),
              b, length_b) {
  A_cols.clear();
  A_cols.reserve(width);
  for (index_t col_ind = 0; col_ind < width; ++col_ind) {
    size_t array_index = coordinates2dense_array_index(0, col_ind, height);
    const data_t *col_data = data + array_index;
    DenseColumn<data_t>* col = new DenseColumn<data_t>(col_data, height);
    A_cols.push_back(col);
  }
}

template <typename data_t>
DenseDataset<data_t>::~DenseDataset() {
  for (size_t ind = 0; ind < A_cols.size(); ++ind) {
    delete A_cols[ind];
  }
}

template<typename data_t>
bool DenseDataset<data_t>::any_columns_overlapping_in_submatrix(
    index_t first_column_submatrix, index_t end_column_submatrix) const {
  if (end_column_submatrix - first_column_submatrix <= 1) {
    return false;
  } else {
    return true;
  }
}

template class DenseDataset<float>;
template class DenseDataset<double>;
template class DenseDataset<int>;
template class DenseDataset<bool>;

} // namespace BlitzML


#ifndef DRAWER_HPP
#define DRAWER_HPP
#include <pangolin/pangolin.h>
#include <Eigen/Dense>
#include "holder.hpp"

class Drawer{
    public:
        Drawer(std::map<std::string,std::string> commandlineArgs, Holder &holder);
        void drawAttention();
        void drawDetectCone();
        void drawSurfaces(bool,bool);
        void drawAimPoint();
        void drawGraph();

    private:
        Holder& holder;
        std::vector<opendlv::logic::perception::GroundSurfaceArea> m_surfaces = {};
        Eigen::MatrixXd m_attentionCones = {};
        Eigen::MatrixXd m_colorCones = {};
        opendlv::logic::action::AimPoint m_aimPoint = {};
        void drawPath(opendlv::logic::perception::GroundSurfaceArea surface);
        void drawFinalCones(opendlv::logic::perception::GroundSurfaceArea surface);
        Eigen::MatrixXd Spherical2Cartesian(double, double, double);
        const double DEG2RAD = 0.017453292522222; // PI/180.0
        const double RAD2DEG = 1.0/0.017453292522222; // PI/180.0

};
#endif