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

/*
Copyright (C) 2003 RiskMap 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.
*/

#ifndef cl_adjoint_term_structure_impl_hpp
#define cl_adjoint_term_structure_impl_hpp
#pragma once

#include <ql/quantlib.hpp>
#include "utilities.hpp"
#include "adjointtermstructuretest.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointTermStructure"

namespace
{
    struct RateDiscount
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Swap Rate", "Base Discount", "Discount", "Implied Discount"
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, RateDiscount& v)
        {
                stm << v.inputRate_
                    << ";" << v.baseDiscount_
                    << ";" << v.discount_
                    << ";" << v.impliedDiscount_ << std::endl;

                return stm;
            }

        Real inputRate_;
        Real baseDiscount_;
        Real discount_;
        Real impliedDiscount_;
    };

    struct RateConsistency
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Swap Rate", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, RateConsistency& v)
        {
                stm << v.inputRate_
                    << ";" << v.functionValue_ << std::endl;

                return stm;
            }

        Real inputRate_;
        Real functionValue_;
    };

    struct Datum
    {
        Integer n;
        TimeUnit units;
        Rate rate;
    };

    struct TermStructureData
    {
        TermStructureData()
        : calendar_(TARGET())
        , settlementDays_(2)
        {
        }

        std::vector<Datum> createSwapData(std::vector<Real> rateValues)
        {
            Size rateSize = rateValues.size();
            std::vector<Datum> swapData(rateSize);
            for (Integer i = 0; i < rateSize; i++)
            {
                swapData[i] = { i * 3 + 10, Months, rateValues[i] };
            }
            return swapData;
        }

        void setTermStructure(std::vector<Datum> swapData)
        {
            std::vector<Datum> depositData =
            {
                { 1, Months, 4.581 },
                { 2, Months, 4.573 },
                { 3, Months, 4.557 },
                { 6, Months, 4.496 },
                { 9, Months, 4.490 }
            };

            Date today = calendar_.adjust(Date::todaysDate());
            Settings::instance().evaluationDate() = today;
            Date settlement = calendar_.advance(today, settlementDays_, Days);
            Size deposits = depositData.size(),
                swaps = swapData.size();

            std::vector<boost::shared_ptr<RateHelper> > instruments(deposits + swaps);
            for (Size i = 0; i < deposits; i++)
            {
                instruments[i] = boost::make_shared<DepositRateHelper>(
                    depositData[i].rate / 100
                    , depositData[i].n*depositData[i].units
                    , settlementDays_
                    , calendar_
                    , ModifiedFollowing
                    , true
                    , Actual360());
            }
            boost::shared_ptr<IborIndex> index(new IborIndex("dummy"
                , 6 * Months
                , settlementDays_
                , Currency()
                , calendar_
                , ModifiedFollowing
                , false
                , Actual360()));
            for (Size i = 0; i < swaps; ++i)
            {
                instruments[i + deposits] = boost::make_shared<SwapRateHelper>(
                    swapData[i].rate / 100
                    , swapData[i].n*swapData[i].units
                    , calendar_
                    , Annual
                    , Unadjusted
                    , Thirty360()
                    , index);
            }
            termStructure_ = boost::make_shared<PiecewiseYieldCurve<Discount, LogLinear>>(settlement,
                                                                                instruments, Actual360());

        }

        std::vector<double> getDoubleVector(Size size)
        {
            std::vector<double> vec =
            {
                0.50, 0.69, 0.57, 0.27, 0.48, 0.85, 0.07, 0.74, 0.47, 0.78, 0.07, 0.68
                , 0.92, 0.10, 0.93, 0.33, 0.20, 0.40, 0.04, 0.73, 0.91, 0.19, 0.50, 0.87
                , 0.81, 0.34, 0.10, 0.52, 0.43, 0.54, 0.34, 0.31, 0.75, 0.51, 0.30, 0.69
                , 0.36, 0.16, 0.90, 0.32, 0.40, 0.20, 0.64, 0.64, 0.72, 0.90, 0.78, 0.74
                , 0.27, 0.39, 0.24, 0.60, 0.88, 0.20, 0.97, 0.33, 0.59, 0.52, 0.91, 0.77
                , 0.73, 0.63, 0.89, 0.88, 0.54, 0.72, 0.92, 0.32, 0.09, 0.56, 0.68, 0.91
                , 0.82, 0.72, 0.35, 0.41, 0.42, 0.23, 0.92, 0.25, 0.67, 0.35, 0.58, 0.52
                , 0.65, 0.74, 0.19, 0.75, 0.06, 0.08, 0.51, 0.97, 0.10, 0.99, 0.32, 0.40
                , 0.69, 0.71, 0.83, 0.64, 0.08, 0.61, 0.56, 0.27, 0.15, 0.69, 0.70, 0.89
                , 0.52, 0.15
            };
            return std::vector<double>(vec.begin(), vec.begin() + size);
        }

        std::vector<Real> calculateZeroValue(std::vector<Datum> swapData)
        {
            setTermStructure(swapData);

            Handle<Quote> spread(boost::make_shared<SimpleQuote>(0.01));

            boost::shared_ptr<YieldTermStructure> spreaded(
                new ZeroSpreadedTermStructure(Handle<YieldTermStructure>(termStructure_), spread));

            Date testDate = termStructure_->referenceDate() + 5 * Years;
            DayCounter resultDayCounter = termStructure_->dayCounter();

            Rate zero = termStructure_->zeroRate(testDate, resultDayCounter, Continuous, NoFrequency);

            return std::vector<Real>(1, zero);
        }

        std::vector<Real> calculateForwardValue(std::vector<Datum> swapData)
        {
            setTermStructure(swapData);

            Handle<Quote> spread(boost::shared_ptr<Quote>(new SimpleQuote(0.01)));

            boost::shared_ptr<YieldTermStructure> spreaded(
                new ForwardSpreadedTermStructure(Handle<YieldTermStructure>(termStructure_), spread));

            Date testDate = termStructure_->referenceDate() + 5 * Years;
            DayCounter resultDayCounter = termStructure_->dayCounter();

            Rate forward = termStructure_->forwardRate(testDate, testDate, resultDayCounter, Continuous, NoFrequency);

            return std::vector<Real>(1, forward);
        }

        std::vector<Real> calculateDiscountValues(std::vector<Datum> swapData)
        {
            setTermStructure(swapData);

            Date today = Settings::instance().evaluationDate();
            Date newToday = today + 3 * Years;
            Date newSettlement = calendar_.advance(newToday, settlementDays_, Days);
            Date testDate = newSettlement + 5 * Years;

            boost::shared_ptr<YieldTermStructure> implied(
                new ImpliedTermStructure(Handle<YieldTermStructure>(termStructure_), newSettlement));

            DiscountFactor baseDiscount = termStructure_->discount(newSettlement);
            DiscountFactor discount = termStructure_->discount(testDate);
            DiscountFactor impliedDiscount = implied->discount(testDate);

            return std::vector<Real>{ baseDiscount, discount, impliedDiscount };
        }

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

            std::vector<Real> value = getValue(createSwapData(rateValues));

            Size sizeY = value.size();

            sf_Finite.resize(sizeX * sizeY);

            for (Size i = 0; i < sizeX; i++)
            {
                rateValues[i] += h;
                std::vector<Real> finiteValue = getValue(createSwapData(rateValues));
                rateValues[i] -= h;

                for (Size j = 0; j < sizeY; j++)
                {
                    sf_Finite[sizeX*j + i] = (finiteValue[j] - value[j]) / h;
                }
            }
        }

        // Global data.
        Calendar calendar_;
        Natural settlementDays_;
        boost::shared_ptr<YieldTermStructure> termStructure_;

        // Cleanup.
        SavedSettings backup_;

    };

    struct ZSpreadedTestData
        : public TermStructureData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, ZSpreadedTestData* data)
            : size_(size)
            , data_(data)
            , rate_(size)
            , iterNumFactor_(10)
            {
                setLogger(&data_->outPerform_);

                std::vector<double> doubleRate = data_->getDoubleVector(size);
                for (Size i = 0; i < size; i++)
                    rate_[i] = doubleRate[i];
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

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

            // Calculates total calibration error.
            void calculateZeroValue()
            {
                zeroValue_ = data_->calculateZeroValue(data_->createSwapData(rate_));
            }

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

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

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

            Size size_;
            Size iterNumFactor_;
            ZSpreadedTestData* data_;
            std::vector<cl::tape_double> rate_;
            std::vector<cl::tape_double> zeroValue_;
        };

        ZSpreadedTestData()
            : TermStructureData()

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

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

            , outSize_(OUTPUT_FOLDER_NAME "//ConsistencyZeroSpreadedTermStructure"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Tape size dependence on number of swap rates" }
        , { "xlabel", "Number of swap rates" }
        , { "ylabel", "Memory(MB)" } })

            , out_(OUTPUT_FOLDER_NAME "//ConsistencyZeroSpreadedTermStructure//output"
            , { { "filename", "zeroRateOnSwapRate" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Zero Rate dependence on Swap Rate" }
        , { "ylabel", "Zero Rate" } })

#if defined CL_GRAPH_GEN
            , pointNo_(80)
            , iterNo_(80)
            , 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 + 30, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<RateConsistency> outData(pointNo_);
            auto test = getTest(pointNo_);
            Rate rate = 0.0;
            for (Size i = 0; i < pointNo_; rate += 1.0, i++)
            {
                outData[i] = { rate / 100, test->data_->calculateZeroValue({ { 10, Years, rate } })[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 ZSpreadedTestData::Test ZSpreadedTest;

    struct FSpreadedTestData
        : public TermStructureData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, FSpreadedTestData* data)
            : size_(size)
            , data_(data)
            , rate_(size)
            , iterNumFactor_(10)
            {
                setLogger(&data_->outPerform_);

                std::vector<double> doubleRate = data_->getDoubleVector(size);
                for (Size i = 0; i < size; i++)
                    rate_[i] = doubleRate[i];
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor_; }

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

            // Calculates total calibration error.
            void calculateForwardValue()
            {
                forwardValue_ = data_->calculateForwardValue(data_->createSwapData(rate_));
            }

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

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

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

            Size size_;
            Size iterNumFactor_;
            FSpreadedTestData* data_;
            std::vector<cl::tape_double> rate_;
            std::vector<cl::tape_double> forwardValue_;
        };

        FSpreadedTestData()
            : TermStructureData()

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

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

            , outSize_(OUTPUT_FOLDER_NAME "//ConsistencyForwardSpreadedTermStructure"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Tape size dependence on number of swap rates" }
        , { "xlabel", "Number of swap rates" }
        , { "ylabel", "Memory(MB)" } })

            , out_(OUTPUT_FOLDER_NAME "//ConsistencyForwardSpreadedTermStructure//output"
            , { { "filename", "forwardRateOnSwapRate" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Forward Rate dependence on Swap Rate" }
        , { "ylabel", "Forward Rate" } })

#if defined CL_GRAPH_GEN
            , pointNo_(80)
            , iterNo_(80)
            , 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 + 30, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<RateConsistency> outData(pointNo_);
            auto test = getTest(pointNo_);
            Rate rate = 0.0;
            for (Size i = 0; i < pointNo_; rate += 1.0, i++)
            {
                outData[i] = { rate / 100, test->data_->calculateForwardValue({ { 10, Years, rate } })[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 FSpreadedTestData::Test FSpreadedTest;

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

            Test(Size size, ImpliedTestData* data)
                : size_(size)
                , data_(data)
                , rate_(size)
                , iterNumFactor_(10)
            {
                setLogger(&data_->outPerform_);

                doubleRate_ = data_->getDoubleVector(size);

                std::copy(doubleRate_.begin(), doubleRate_.end(), rate_.begin());
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 3; }

            Size minPerfIteration() { return iterNumFactor_; }

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

            // Calculates total calibration error.
            void calculateDiscountValues()
            {
                discountValues_ = data_->calculateDiscountValues(data_->createSwapData(rate_));
            }

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

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

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

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

            Size size_;
            Size iterNumFactor_;
            ImpliedTestData* data_;
            std::vector<double> doubleRate_;
            std::vector<cl::tape_double> rate_;
            std::vector<cl::tape_double> discountValues_;
        };

        ImpliedTestData()
            : TermStructureData()

            , outPerform_(OUTPUT_FOLDER_NAME "//ConsistencyYieldTermStructure"
            , { { "filename", "AdjointPerformance" }
        , { "not_clear", "Not" }
        , { "cleanlog", "true" }
        , { "title", "Base discount, discount, implied discount\\ndifferentiation performance with respect to swap rate" }
        , { "xlabel", "Number of swap rates" }
        , { "ylabel", "Time (s)" }
        , { "smooth", "default" }
        , { "line_box_width", "-5" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME "//ConsistencyYieldTermStructure"
            , { { "filename", "Adjoint" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Base discount, discount, implied discount\\nadjoint differentiation performance with respect to swap rate" }
        , { "xlabel", "Number of swap rates" }
        , { "smooth", "default" }
        , { "ylabel", "Time (s)" } })

            , outSize_(OUTPUT_FOLDER_NAME "//ConsistencyYieldTermStructure"
            , { { "filename", "TapeSize" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Tape size dependence on number of swap rates" }
        , { "xlabel", "Number of swap rates" }
        , { "ylabel", "Memory(MB)" } })

            , out_(OUTPUT_FOLDER_NAME "//ConsistencyYieldTermStructure//output"
            , { { "filename", "discountOnSwapRate" }
        , { "not_clear", "Not" }
        , { "cleanlog", "false" }
        , { "title", "Discount values dependence on Swap Rate" }
        , { "ylabel", "Forward Rate" } })

#if defined CL_GRAPH_GEN
            , pointNo_(80)
            , iterNo_(80)
            , 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 + 30, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<RateDiscount> outData(pointNo_);
            auto test = getTest(pointNo_);
            Rate rate = 0.0;
            for (Size i = 0; i < pointNo_; rate += 1.0, i++)
            {
                std::vector<Real> out = test->data_->calculateDiscountValues({ { 10, Years, rate } });
                outData[i] = { rate / 100
                    , out[0]
                    , out[1]
                    , out[2] };
            }
            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 ImpliedTestData::Test ImpliedTest;

}

#endif


#include "fractile.h"

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

#include "logger.h"
#include "plugin_factory.h"

#include "lagged_ensemble.h"
#include "time_ensemble.h"

#include "fetcher.h"
#include "radon.h"

#include "numerical_functions.h"
#include "util.h"

namespace himan
{
namespace plugin
{
fractile::fractile() : itsFractiles({0., 10., 25., 50., 75., 90., 100.})
{
	itsCudaEnabledCalculation = false;
	itsLogger = logger("fractile");
}

std::unique_ptr<ensemble> CreateEnsemble(const std::shared_ptr<const plugin_configuration> conf)
{
	logger log("fractile");

	std::string paramName = conf->GetValue("param");

	auto ensTypestr = conf->GetValue("ensemble_type");
	HPEnsembleType ensType = kPerturbedEnsemble;

	if (!ensTypestr.empty())
	{
		ensType = HPStringToEnsembleType.at(ensTypestr);
	}

	size_t ensSize = 0;

	if (conf->Exists("ensemble_size"))
	{
		ensSize = std::stoi(conf->GetValue("ensemble_size"));
	}
	else if (ensType == kPerturbedEnsemble || ensType == kLaggedEnsemble)
	{
		// Regular ensemble size is static, get it from database if user
		// hasn't specified any size

		auto r = GET_PLUGIN(radon);

		std::string ensembleSizeStr = r->RadonDB().GetProducerMetaData(conf->SourceProducer(0).Id(), "ensemble size");

		if (ensembleSizeStr.empty())
		{
			log.Error("Unable to find ensemble size from database");
			himan::Abort();
		}

		ensSize = std::stoi(ensembleSizeStr);
	}

	std::unique_ptr<ensemble> ens = nullptr;

	switch (ensType)
	{
		case kPerturbedEnsemble:
			ens = std::unique_ptr<ensemble>(new ensemble(param(paramName), ensSize));
			break;
		case kTimeEnsemble:
		{
			int secondaryLen = 0, secondaryStep = 1;
			HPTimeResolution secondarySpan = kHourResolution;

			if (conf->Exists(("secondary_time_len")))
			{
				secondaryLen = std::stoi(conf->GetValue("secondary_time_len"));
			}
			if (conf->Exists(("secondary_time_step")))
			{
				secondaryStep = std::stoi(conf->GetValue("secondary_time_step"));
			}
			if (conf->Exists(("secondary_time_span")))
			{
				secondarySpan = HPStringToTimeResolution.at(conf->GetValue("secondary_time_span"));
			}

			ens = std::unique_ptr<time_ensemble>(new time_ensemble(param(paramName), ensSize, kYearResolution,
			                                                       secondaryLen, secondaryStep, secondarySpan));
		}
		break;
		case kLaggedEnsemble:
		{
			auto name = conf->GetValue("named_ensemble");

			if (name.empty() == false)
			{
				ens = std::unique_ptr<lagged_ensemble>(new lagged_ensemble(param(paramName), name));
			}

			else
			{
				auto lagstr = conf->GetValue("lag");
				if (lagstr.empty())
				{
					log.Fatal("specify lag value for lagged_ensemble");
					himan::Abort();
				}

				int lag = std::stoi(conf->GetValue("lag"));

				if (lag == 0)
				{
					log.Fatal("lag value needs to be negative integer");
					himan::Abort();
				}
				else if (lag > 0)
				{
					log.Warning("negating lag value " + std::to_string(-lag));
					lag = -lag;
				}

				auto stepsstr = conf->GetValue("lagged_steps");

				if (stepsstr.empty())
				{
					log.Fatal("specify lagged_steps value for lagged_ensemble");
					himan::Abort();
				}

				int steps = std::stoi(conf->GetValue("lagged_steps"));

				if (steps <= 0)
				{
					log.Fatal("invalid lagged_steps value. Allowed range >= 0");
					himan::Abort();
				}

				steps++;

				ens = std::unique_ptr<lagged_ensemble>(
				    new lagged_ensemble(param(paramName), ensSize, time_duration(kHourResolution, lag), steps));
			}
		}
		break;
		default:
			log.Fatal("Unknown ensemble type: " + ensType);
			himan::Abort();
	}

	if (conf->Exists("max_missing_forecasts"))
	{
		ens->MaximumMissingForecasts(std::stoi(conf->GetValue("max_missing_forecasts")));
	}

	return std::move(ens);
}

void fractile::SetParams()
{
	auto fractiles = itsConfiguration->GetValue("fractiles");

	if (!fractiles.empty())
	{
		itsFractiles.clear();

		auto list = util::Split(fractiles, ",");

		for (std::string& val : list)
		{
			boost::trim(val);
			try
			{
				itsFractiles.push_back(std::stof(val));
			}
			catch (const std::invalid_argument& e)
			{
				itsLogger.Fatal("Invalid fractile value: '" + val + "'");
				himan::Abort();
			}
		}
	}

	params calculatedParams;
	std::string paramName = itsConfiguration->GetValue("param");

	if (paramName.empty())
	{
		itsLogger.Fatal("param not specified");
		himan::Abort();
	}

	for (float frac : itsFractiles)
	{
		auto name = fmt::format("F{}-{}", frac, paramName);
		param p(name);
		p.ProcessingType(processing_type(kFractile, frac, kHPMissingValue, kHPMissingInt));
		calculatedParams.push_back(p);
	}

	auto name = util::Split(paramName, "-");

	param mean(name[0] + "-MEAN-" + name[1]);
	mean.ProcessingType(processing_type(kEnsembleMean, kHPMissingInt, kHPMissingInt, kHPMissingInt));

	calculatedParams.push_back(mean);

	param stdev(name[0] + "-STDDEV-" + name[1]);
	stdev.ProcessingType(processing_type(kStandardDeviation, kHPMissingInt, kHPMissingInt, kHPMissingInt));

	calculatedParams.push_back(stdev);

	compiled_plugin_base::SetParams(calculatedParams);
}

void fractile::SetForecastType()
{
	if (itsForecastTypeIterator.Size() > 1)
	{
		itsLogger.Warning("More than one forecast type defined - fractile can only produce 'statistical processing'");
	}

	const std::vector<forecast_type> type({forecast_type(kStatisticalProcessing)});
	itsForecastTypeIterator = forecast_type_iter(type);
	std::const_pointer_cast<himan::plugin_configuration>(itsConfiguration)->ForecastTypes(type);
}

void fractile::Process(const std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	SetParams();
	SetForecastType();
	Start<float>();
}

void fractile::Calculate(std::shared_ptr<info<float>> myTargetInfo, uint16_t threadIndex)
{
	const std::string deviceType = "CPU";
	auto ens = CreateEnsemble(itsConfiguration);

	auto threadedLogger = logger("fractileThread # " + std::to_string(threadIndex));

	forecast_time forecastTime = myTargetInfo->Time();
	level forecastLevel = myTargetInfo->Level();

	threadedLogger.Info("Calculating time " + static_cast<std::string>(forecastTime.ValidDateTime()) + " level " +
	                    static_cast<std::string>(forecastLevel));

	try
	{
		ens->Fetch(itsConfiguration, forecastTime, forecastLevel);
	}
	catch (const HPExceptionType& e)
	{
		if (e == kFileDataNotFound)
		{
			itsLogger.Error("Failed to find ensemble data");
			return;
		}
	}

	myTargetInfo->ResetLocation();
	ens->ResetLocation();

	while (myTargetInfo->NextLocation() && ens->NextLocation())
	{
		auto sortedValues = ens->SortedValues();
		const size_t ensembleSize = sortedValues.size();

		// Skip this step if we didn't get any valid fields
		if (ensembleSize == 0)
		{
			continue;
		}

		// process mean&var before hanging size of sortedValues
		float mean = numerical_functions::Mean<float>(sortedValues);
		if (!std::isfinite(mean))
		{
			mean = MissingFloat();
		}

		float var = std::sqrt(numerical_functions::Variance<float>(sortedValues));
		if (!std::isfinite(var))
		{
			var = MissingFloat();
		}

		// sortedValues needs to have one element at the back for correct array indexing
		// NOTE: `ensembleSize` stays the same

		sortedValues.push_back(0.0);

		ASSERT(!itsFractiles.empty());

		size_t targetInfoIndex = 0;
		for (auto P : itsFractiles)
		{
			// use the linear interpolation between closest ranks method recommended by NIST
			// http://www.itl.nist.gov/div898/handbook/prc/section2/prc262.htm
			float x;

			// check lower corner case p E [0,1/(N+1)]
			if (P / 100.0 <= 1.0 / static_cast<float>(ensembleSize + 1))
			{
				x = 1;
			}
			// check upper corner case p E [N/(N+1),1]
			else if (P / 100.0f >= static_cast<float>(ensembleSize) / static_cast<float>(ensembleSize + 1))
			{
				x = static_cast<float>(ensembleSize);
			}
			// everything that happens on the interval between
			else
			{
				x = P / 100.0f * static_cast<float>(ensembleSize + 1);
			}
			// floor x explicitly before casting to int
			int i = static_cast<int>(std::floor(x));

			myTargetInfo->Index<param>(targetInfoIndex);

			myTargetInfo->Value(sortedValues[i - 1] + std::fmod(x, 1.0f) * (sortedValues[i] - sortedValues[i - 1]));
			++targetInfoIndex;
		}

		myTargetInfo->Index<param>(targetInfoIndex);
		myTargetInfo->Value(mean);
		++targetInfoIndex;
		myTargetInfo->Index<param>(targetInfoIndex);
		myTargetInfo->Value(var);
	}

	threadedLogger.Info("[" + deviceType + "] Missing values: " + std::to_string(myTargetInfo->Data().MissingCount()) +
	                    "/" + std::to_string(myTargetInfo->Data().Size()));
}

}  // namespace plugin
}  // namespace himan


// --------------------------------------------------------------------------
// Copyright (C) Karlsruhe Institute of Technology, 2018
//
// 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 "src/bid_set.h"
#include <boost/numeric/ublas/matrix_proxy.hpp>

BidSet::BidSet(const std::vector<double> &v_v,
               const boost::numeric::ublas::matrix<int> &m_q)
    : values(v_v), quantities(m_q) {}

BidSet::BidSet(const BidSet &copy)
    : values(copy.values), quantities(copy.quantities) {}

BidSet BidSet::fromYAML(YAML::Node bidset) {
  auto values = bidset["values"].as<std::vector<double>>();

  std::vector<unsigned int> v_q;
  for (auto row : bidset["quantities"]) {
    auto v_row = row.as<std::vector<unsigned int>>();
    v_q.insert(v_q.end(), v_row.begin(), v_row.end());
  }

  auto nrows = bidset["quantities"].size();
  auto ncols = v_q.size() / nrows;

  boost::numeric::ublas::matrix<unsigned int> quantities(nrows, ncols);
  std::copy(v_q.begin(), v_q.end(), quantities.data().begin());

  return BidSet(values, quantities);
}

BidSet BidSet::sample(double sampling_ratio) {
  unsigned int sample_n = (int)(N() * sampling_ratio);

  std::vector<double> sample_values;
  sample_values.resize(sample_n);
  std::copy_n(values.begin(), sample_n, sample_values.begin());

  auto sample_quantities =
      boost::numeric::ublas::subslice(quantities, 0, 1, sample_n, 0, 1, L());

  return BidSet(sample_values, sample_quantities);
}

boost::unordered_map<unsigned int, double> BidSet::computeAvgPrices() const {
  boost::unordered_map<unsigned int, double> avg_price;
  for (unsigned int i = 0; i < N(); ++i) {
    unsigned int q_i = 0;
    for (unsigned int k = 0; k < L(); ++k) {
      q_i += quantities(i, k);
    }
    avg_price[i] = values[i] / q_i;
  }
  return avg_price;
}

boost::unordered_map<unsigned int, double> BidSet::computeDensities() const {
  return computeDensities(std::vector<double>(L(), 1.));
}

boost::unordered_map<unsigned int, double> BidSet::computeDensities(
    std::vector<double> f) const {
  boost::unordered_map<unsigned int, double> density;
  for (unsigned int i = 0; i < N(); ++i) {
    double m_i = 0;
    for (unsigned int k = 0; k < L(); ++k) {
      m_i += quantities(i, k) * f[k];
    }
    density[i] = values[i] / std::sqrt(m_i);
  }
  return density;
}

std::vector<unsigned int> BidSet::computeQPerResource() const {
  std::vector<unsigned int> qpr(L(), 0);
  for (unsigned int i = 0; i < N(); ++i) {
    for (unsigned int k = 0; k < L(); ++k) {
      qpr[k] += quantities(i, k);
    }
  }
  return qpr;
}


#include <iostream>
//#include "../inc/ParkingPlannerThread.h"
#include <ros/ros.h>    //ROS湲곕낯 ?ㅻ뜑 ?뚯씪
#include <cmath>
#include "../inc/quadtree.h"
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <GeometricUtils.h>
#include <MathParam.h>
#include <AngleUtils.h>
#include <ompl/base/StateSpaceTypes.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ClothoidStateSpace.h>
#include <ompl/base/ScopedState.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/cforest/CForest.h>
#include <boost/program_options.hpp>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/timer/timer.hpp>
#include <boost/thread/thread.hpp>
#include<ompl/tools/benchmark/Benchmark.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <std_msgs/Float32MultiArray.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <fstream>
#define DRAW
//#define DRAWVEHICLE
//#define CONFIG_ANALYSIS

//#define ODEDEMO
using namespace std;
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace oc = ompl::control;
namespace po = boost::program_options;
using namespace Eigen;

//bool CLOTHOID = true;
//ob::StateSpacePtr space(new ob::ClothoidStateSpace(0.17, true)); // false: forward
//typedef ob::ClothoidStateSpace::StateType STATETYPE;
//typedef ob::ClothoidStateSpace STATESPACE;

int MODEDORRTSTAR = 1;
og::SimpleSetup* ss_g;
og::SimpleSetup* ss_l;
vector<Vector3d> vPath_g;

vector<Vector3d> vTrajec_g;

ofstream outFile("output.txt");
bool CLOTHOID = false;
//ob::StateSpacePtr g_space(new ob::DubinsStateSpace(4.88, true)); // false: forward
//ob::StateSpacePtr g_space_local(new ob::DubinsStateSpace(4.88, true)); // false: forward
ob::StateSpacePtr g_space(new ob::DubinsStateSpace(4, true)); // false: forward
ob::StateSpacePtr g_space_local(new ob::DubinsStateSpace(4, true)); // false: forward
typedef ob::SE2StateSpace::StateType STATETYPE;
typedef ob::SE2StateSpace STATESPACE;
///////////////////////////////////////////////////////////////////////////////////////////////////////////
string LOGFILENAME="DEFAULT";
bool UPDATINGMAP = false;
int TMPMOVINGCNT=0;
int TMPMOVINGPOSCNT=0;
int ITER = 1;
int LOGFLAG=0;
double PLANNINGTIME = 5.0;
double PLANNINGTIME_L = 1.0;
double COSTTHRESHOLD=8.0;

vector<Vector2d> OBSTACLE;
double START_GORI[3]={0.0,0.0,RADIANS(0)};
double GOAL_GORI[3]={0.0,0.0,RADIANS(0)};
double START_G[3]={0.0,0.0,RADIANS(0)};
double GOAL_G[3]={0.0,0.0,RADIANS(0)};
//For local planner
double START_L[3]={0.0,0.0,RADIANS(0)};
double GOAL_L[3]={0.0,0.0,RADIANS(0)};
ob::RealVectorBounds BOUNDS(2);
double SAFEREGION = 0.6;

double RANGE_OBS = 4.0;
double K_REP_OBS = 1.00f;
double RANGE_REP=180.0;
double K_REP = 100.0;
double K_ATT = 0.002;

//double CAR_C2F = 3.2; //ori
//double CAR_C2R = 0.8; //ori
double CAR_C2F = 2.843;
double CAR_C2R = 1.435;

double CAR_WIDTH = 0.9;

vector<vector<VectorXd> > g_map; //unuse
ob::PlannerStatus g_solved;
bool g_replan = false;
bool g_solved_init = false;
double g_localization_map[4]={0.0,0.0,RADIANS(0),0.0};
double g_localization[4]={0.0,0.0,RADIANS(0),0.0};
double VEHICLEPOS_TMP[4]={0,0,0,0};
// KD TREE : Obstacle
pcl::PointCloud<pcl::PointXYZ>::Ptr g_pTree;
pcl::KdTreeFLANN<pcl::PointXYZ> g_kdTree;

// KD TREE : Path
pcl::PointCloud<pcl::PointXYZ>::Ptr g_pTree_Path;
pcl::KdTreeFLANN<pcl::PointXYZ> g_kdTree_Path;

// Publish Msg
ros::Publisher g_msgpub1;
ros::Publisher g_msgpub2;
ros::Publisher g_msgpub3;
ros::Publisher g_msgpub4;
ros::Publisher g_msgpub5;

ros::Subscriber g_msgsub1;
//ros::Subscriber g_msgsub2;

geometry_msgs::PoseArray g_posArray1;
geometry_msgs::PoseArray g_posArray2;
geometry_msgs::PoseArray g_posArray3;

ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
{
    auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
    obj->setCostThreshold(ob::Cost(COSTTHRESHOLD));
    return obj;
}

int SearchNearestNodeIdxByRadius(pcl::PointXYZ searchPoint, float radius)
{
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    std::vector<pcl::PointXYZ> pvNode;

    if( g_kdTree_Path.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance ) > 0 )
    {
        return pointIdxRadiusSearch[0];
    }
    return -1;
}

std::vector<pcl::PointXYZ> SearchNodeByRadius(pcl::PointXYZ searchPoint, float radius)
{
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    std::vector<pcl::PointXYZ> pvNode;

    if( g_kdTree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance ) > 0 )
    {
        for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
        {
            pvNode.push_back(g_pTree->points[pointIdxRadiusSearch[i]]);
        }
    }
    return pvNode;
}

bool isFreeSpace_(float x, float y)
{
    bool isFreeSpace = true;

    std::vector<pcl::PointXYZ> obs = SearchNodeByRadius(pcl::PointXYZ(x,y,0),SAFEREGION);

    if( obs.size() > 0 )
        isFreeSpace = false;

    return isFreeSpace;
}

bool isValid(double x, double y, double yaw)
{
    double _x = x + 0.2*cos(yaw);
    double _y = y + 0.2*sin(yaw);

    bool isFreeSpace1 = isFreeSpace_(_x,_y);

    _x = x + 1.2*cos(yaw);
    _y = y + 1.2*sin(yaw);

    bool isFreeSpace2 = isFreeSpace_(_x,_y);

    _x = x + 2.2*cos(yaw);
    _y = y + 2.2*sin(yaw);

    bool isFreeSpace3 = isFreeSpace_(_x,_y);

    return isFreeSpace1 && isFreeSpace2 && isFreeSpace3;
}

bool isStateValid(const ob::SpaceInformation *si, const vector<vector<VectorXd> >& map, const ob::State *state)
{
    const STATETYPE *s = state->as<STATETYPE>();
    return si->satisfiesBounds(s) && isValid(s->getX(),s->getY(),s->getYaw());
}

void UpdateGlobalPathData()
{
    if (g_solved && g_solved_init)
    {
        og::PathGeometric path= ss_g->getSolutionPath();
        path.interpolate(100);

        vPath_g.clear();
        if( g_pTree_Path != NULL )
            g_pTree_Path->clear();

        g_pTree_Path  = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

        for(int i=0;i<path.getStateCount(); i++)
        {
            const STATETYPE *s = path.getState(i)->as<STATETYPE>();
            double x=s->getX(), y=s->getY(), yaw=s->getYaw();// 경로에서 x,y,h을 뽑아와서 저 아래 벡터에 넣는다.

            g_pTree_Path->push_back(pcl::PointXYZ(x, y, 0.0));
            vPath_g.push_back(Vector3d(x,y,yaw));// 이게 글로벌 경로임
        }

        g_kdTree_Path.setInputCloud(g_pTree_Path);// 장애물 각각에서 충돌이 없는 경로를 하기보다는 만든 경로에서 일정 거리에 장애물이 있는지 없는지 알아보는 것임

    }
}

void MergeLocal2GlobalPathData(int startIdx, int goalIdx, og::PathGeometric* path_l)// 지역적으로 만든 경로를 기존에 전역 경로에 붙인다. 이전에 없앤 글로벌한 경로의 개수만큼만.
{
    if (g_solved && g_solved_init)
    {
        for(int i=0;i<goalIdx-startIdx+1; i++)
        {
            const STATETYPE *s = path_l->getState(i)->as<STATETYPE>();
            double x=s->getX(), y=s->getY(), yaw=s->getYaw();

            vPath_g[i+startIdx]=Vector3d(x,y,yaw);
            g_pTree_Path->points[i+startIdx] = pcl::PointXYZ(x, y, 0.0);
        }
        g_kdTree_Path.setInputCloud(g_pTree_Path);
    }
}

void plan_init(og::SimpleSetup* ss,double* start, double* goal, bool isLocal = false)
{
    if( isLocal )
    {
        ob::ScopedState<> ss_start(g_space_local), ss_goal(g_space_local);
        ////////////////////////////////
        // set the start and goal states
        ss_start[0] =start[0];
        ss_start[1] =start[1];
        ss_start[2] =start[2];
        ss_goal[0] = goal[0];
        ss_goal[1] = goal[1];
        ss_goal[2] = goal[2];

        ss->setStartAndGoalStates(ss_start, ss_goal,0.05);
    }
    else
    {
        ob::ScopedState<> ss_start(g_space), ss_goal(g_space);
        ////////////////////////////////
        // set the start and goal states
        ss_start[0] =start[0];
        ss_start[1] =start[1];
        ss_start[2] =start[2];
        ss_goal[0] = goal[0];
        ss_goal[1] = goal[1];
        ss_goal[2] = goal[2];

        ss->setStartAndGoalStates(ss_start, ss_goal,0.05);
    }
    // set state validity checking for this space
    ob::SpaceInformationPtr si(ss->getSpaceInformation());
    ss->setStateValidityChecker(std::bind(
                &isStateValid, si.get(),
                g_map, std::placeholders::_1));

    auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
    obj->setCostThreshold(ob::Cost(COSTTHRESHOLD));

    if( isLocal )
    {
        //ss->setPlanner(ob::PlannerPtr(new og::CForest(ss->getSpaceInformation())));
        ss->setPlanner(std::make_shared<ompl::geometric::RRTstar>(ss->getSpaceInformation()));
    }
    else
    {
        ss->setPlanner(std::make_shared<ompl::geometric::RRTstar>(ss->getSpaceInformation()));
    }

    // this call is optional, but we put it in to get more output information
    ss->getSpaceInformation()->setStateValidityCheckingResolution(0.05);
    ss->setup();
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool plan(og::SimpleSetup* ss, double time, bool isLocal = false)
{
    //    ss.print();
    // attempt to solve the problem within 30 seconds of planning time
    if( isLocal )
    {
        cout <<"### LOCAL ### "<<time<<endl;
        return ss->solve(time);
    }
    else
    {
        //cout <<"### GLOBAL ### "<<time<<endl;
        g_solved = ss->solve(time);
        g_solved_init = true;

        return g_solved;
    }
}

void planner_global()
{
    // Global Path Planning
    // Update Global Information
    //////////////////////////////////////////////////////
    START_G[0] = g_localization_map[0];//전역 맵에서의 차량의 현재 글로벌한 x,y,h를 받는다.
    START_G[1] = g_localization_map[1];
    START_G[2] = g_localization_map[2];

    g_space->as<STATESPACE>()->setBounds(BOUNDS);// 바운더리 값은 입력으로 미리 받는것이다.

    cout << START_G[0] << " " <<START_G[1] <<" "<<START_G[2]<<" "<<GOAL_G[0] <<" "<<GOAL_G[1] << " "<<GOAL_G[2]<<endl;

    plan_init(ss_g,START_G,GOAL_G, false);

    plan(ss_g,PLANNINGTIME);
    UpdateGlobalPathData();
}

// 0 : No Global Path Data
// -1: Fail to Find NN
// -2: Fail to Goal Index ( collision )
// -3: Fail to Local Path Generation ( Cannot solve )
// 1: Success to Local Path
int planner_local()
{
    if (g_solved && g_solved_init  )
    {
        if( g_replan == false )
        {
            return 2;
        }
        cout <<"PATH REPLANNING - LOCAL"<<endl;
        // Find Nearest Node Index from the global path w.r.t current pos (g_localization_map)

        double goal[3]={0,0,0};
        double start[3]={0,0,0};

        //START_G - global map
        // 기준좌표계: x, y, theta, 대상좌표: _x, _y
        //GeometricUtils::TransRelativeCoord(double ref_x, double ref_y, double ref_h, double _x, double _y, double _h, double& ret_x, double& ret_y, double& ret_h)
        //GeometricUtils::TransRelativeCoord(START_G[0], START_G[1], START_G[2], g_localization_map[0], g_localization_map[1], g_localization_map[2], start[0], start[1], start[2]);

        start[0] = g_localization_map[0];
        start[1] = g_localization_map[1];
        start[2] = g_localization_map[2];

        int startIndex  =  SearchNearestNodeIdxByRadius(pcl::PointXYZ(start[0], start[1], 0), 10.0);

        if( startIndex == -1 )
        {
            cout <<"local -1" <<endl;
            return -1;
        }

        int goalIndex = -1;
        int minGoalIndex = 40;
        int maxGoalIndex = 80;

        for(int i=startIndex+minGoalIndex; i<vPath_g.size();i++)
        {
            if( i > maxGoalIndex + startIndex )
            {
                break;
            }

            double x=vPath_g[i][0], y=vPath_g[i][1], yaw=vPath_g[i][2];

            std::vector<pcl::PointXYZ> obs = SearchNodeByRadius(pcl::PointXYZ(x,y,0),0.5);
            if( obs.size() == 0 )
            {
                goal[0] = x;
                goal[1] = y;
                goal[2] = yaw;
                goalIndex = i;
                break;

            }
            else
            {
            }

        }

        cout <<"local : startidx["<<startIndex<<"] goalidx["<<goalIndex<<"]" <<endl;

        if(goalIndex == -1)
        {
            cout <<"local -2 : startidx["<<startIndex<<"] goalidx["<<goalIndex<<"]" <<endl;
            return -2;
        }

        //cout <<"LOCAL "<<start[0] <<" "<< start[1]<<" " <<start[2] <<" "<<goal[0] <<" "<<goal[1] <<" "<< goal[2] <<endl;

        // For magnetic field, obstacle field
        START_L[0]=start[0];
        START_L[1]=start[1];
        START_L[2]=start[2];
        GOAL_L[0]=goal[0];
        GOAL_L[1]=goal[1];
        GOAL_L[2]=goal[2];

        vector<double> vX;
        vector<double> vY;
        double bound = 20.0;

        for(int i=startIndex;i<=goalIndex; i++)
        {
            double x=vPath_g[i][0], y=vPath_g[i][1], yaw=vPath_g[i][2];

            vX.push_back(x+bound);
            vX.push_back(x-bound);
            vY.push_back(y+bound);
            vY.push_back(y-bound);
        }

        ob::RealVectorBounds bounds(2);

        bounds.low[0] = *min_element(vX.begin(),vX.end());
        bounds.high[0] =*max_element(vX.begin(),vX.end());
        bounds.low[1] = *min_element(vY.begin(),vY.end());
        bounds.high[1] =*max_element(vY.begin(),vY.end());

        g_space_local->as<STATESPACE>()->setBounds(bounds);

        plan_init(ss_l,start, goal, true);
        ///////////////////////////////////////////////////
        bool retplan = plan(ss_l, PLANNINGTIME_L, true);
        int ret = -3;
        if( retplan )
        {
            og::PathGeometric path = ss_l->getSolutionPath();
            path.interpolate(goalIndex - startIndex + 1);

            MergeLocal2GlobalPathData(startIndex, goalIndex, &path);
            ret = 1;
        }

        cout <<"local :"<<ret <<endl;
        cout <<"HH"<<endl;
        return ret;
    }
    return 0;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// From HeightMap Module : Local Path Update : 5hz
void SubTopicProcess1(const std_msgs::Float32MultiArray::ConstPtr& msg)
// 하이트맵에서 이 것을 받는 순간의 차량의 전역 x,y,h,어쩌고, 정보(4개)와 장애물의 정보(x1,y1,x2,y2, ...)가 들어온다. 포인트 클라우드는 특정 라이다 데이터가 특정 그리드 위에 하나라도 있으면 그 그리드의 가운데 점을 주는 것이고 애초에 특정 높이로 자르고 난 뒤에 저런 정보는 받는 것이다.
{
    UPDATINGMAP=true;
    // Global Coordinate Obstacle Data and Position Data
    vector<Vector2d> vObstacle;
    vObstacle.push_back(Vector2d(-99999.0,-99999.0));

    for(int i=0; i<int((msg->data.size()-4)/2.0); i++)//4개는 하이트맵 받는 순간의 차량의 전역 정보 4개이고 저걸 빼고 2로 나누면 총 장애물이 있는 그리드의 개수이다.
    {
	vObstacle.push_back(Vector2d(msg->data.at(i*2+4),msg->data.at(i*2+5)));
    }
    cout <<"SUBDATA"<<endl;

    if( g_pTree != NULL )
        g_pTree->clear();

    g_pTree  = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

    for(int k=0; k<vObstacle.size(); k++)
    {
        g_pTree->push_back(pcl::PointXYZ(vObstacle[k](0), vObstacle[k](1), 0.0)); //우선, 장애물 정보를 충돌 검사를 하기 위해서 g_pTree에 넣어 놓는다.
    }
    // g_pTree->push_back(pcl::PointXYZ(100, 100, 0.0));  //  임의로 장애물 하나만 넣기. 아예 없으면 오류. 장애물 고려 안하려고 사용
    g_kdTree.setInputCloud(g_pTree);// 여기까지가 저 kdTree를 이용하기 위한 기본 세팅이다.

    /////////////////////////////////////////////////////////////////////////////
    // localization data (SLAM)
    g_localization_map[0] = msg->data.at(0);//하이트맵 받을 때의 차량 중심에서의 글로벌 x위치
    g_localization_map[1] = msg->data.at(1);
    g_localization_map[2] = msg->data.at(2);
    g_localization_map[2] = AngleUtils::toRange_PItoPI(g_localization_map[2]);
    g_localization_map[3] = msg->data.at(3);

    // 경로를 만드는 곳에서, 처음 로컬라이제이션 정보를 얻고 난 다음, 차량의 후륜 중심이 따라가는 경로를 만드는데, 이 시작점이 애초에
    // 차량 중심부터 시작한다. 즉, 경로의 시작점 위치가 차량 중심이면서, 이 순간 바로 차량이 후륜 중심이 있기를 바라는 아이러니에 빠지게 된다.
    // 따라서, 경로를 만들 때, 차량 후륜 중심부터 경로가 생길 수 있도록 경로를 생성하는 부분에서 처리를 해줘야 한다.
    double t_x = g_localization_map[0];
    double t_y = g_localization_map[1];
    double t_h = g_localization_map[2];

    // g_localization_map[0] = t_x;
    // g_localization_map[1] = t_y;

    //g_localization_map[0] = t_x - 0.8*cos( g_localization_map[2] ); //경로가 그려지는 지점과 저 0.8 수치를 같게 하면, Rviz에서 동일한 곳에서 경로를 만들 수 있네
    //g_localization_map[1] = t_y - 0.8*sin( g_localization_map[2] );
    // g_localization_map[0] = t_x - CAR_C2R*cos( g_localization_map[2] ); //경로가 그려지는 지점과 저 0.8 수치를 같게 하면, Rviz에서 동일한 곳에서 경로를 만들 수 있네
    // g_localization_map[1] = t_y - CAR_C2R*sin( g_localization_map[2] );

    g_replan = false;// 우선 처음 리플레닝이 바로 하진 않으니 우선은 초기 값으로 false
    for(int i=0; i<vPath_g.size(); i++) //우선, 전체 경로 사이즈 동안 전체 경로를 저 서치노드바이레디어스로 전역 경로 생성된 것이 있으면 그 경로 주변 0.2m안에 장애물이 있는지 확인
    {
        double x=vPath_g[i][0], y=vPath_g[i][1], yaw=vPath_g[i][2];

        std::vector<pcl::PointXYZ> obs = SearchNodeByRadius(pcl::PointXYZ(x,y,0),0.2);
        if( obs.size() > 0 )// 장애물이 하나라도 지금의 전역 경로에 있으면 바로 경로 재 생성
        {
            g_replan = true;
            break;//for문 나가지겠지.
        }
    }

    if ( g_solved_init == false )//맨 처음에는 당연히 false겠네
    {
        g_replan = true;
    }

    int iterLocal = 0;
    int retLocal = planner_local();
    if( retLocal == -3 ) // -3인 케이스가 있다. 아예 경로를 만들 수 없을 때.
    {
        while( iterLocal < 5 )
        {
            retLocal = planner_local();
            if( retLocal > 1 )//경로 못 만들었다가 만들었으면 와일문에서 나가자.
                break;
            iterLocal++;
        }
    }
    if( retLocal < 1 )
    {
        cout <<"PATH REPLANNING - GLOBAL"<<endl;
        planner_global();
    }
    else
    {
        //cout << "path size["<<vPath_g.size()<<"] "<<msg->data.at(0) << " "<<msg->data.at(1) <<endl;
        //cout <<"PATH Tracking ........"<<endl;
    }

    /////////////////////////////////////////////////////////////////////
    // Pub local planner path to controller ( Global Coordinate )
    {
        std_msgs::Header header;
        header.stamp = ros::Time::now();
#ifdef DRAWVEHICLE
        header.frame_id = "/camera_init_global";
#else
        header.frame_id = "/camera_init";
#endif
        nav_msgs::Path msg;
        msg.header = header;

        nav_msgs::Path msg2;
        msg2.header = header;

        g_posArray1.header = header;
        g_msgpub1.publish(g_posArray1);
        g_posArray1.poses.clear();

        g_posArray2.header = header;
        g_msgpub2.publish(g_posArray2);
        g_posArray2.poses.clear();

        g_posArray3.header = header;
        g_msgpub3.publish(g_posArray3);
        g_posArray3.poses.clear();

        for(int i=0;i<vPath_g.size(); i++)// vPath_g 에 전역 경로가 들어 있는 것이다.
        {

            double x=vPath_g[i][0], y=vPath_g[i][1], yaw=vPath_g[i][2];

            geometry_msgs::PoseStamped waypt;
            waypt.header = header;
#ifdef DRAWVEHICLE
            waypt.pose.position.x = y;
            waypt.pose.position.y = -x;
            waypt.pose.position.z = 0;
#else
            waypt.pose.position.x = x;
            waypt.pose.position.y = y;
            waypt.pose.position.z = 0;
#endif
            msg.poses.push_back(waypt);

            geometry_msgs::PoseStamped waypt2;
            waypt2.header = header;
            waypt2.pose.position.x = x;
            waypt2.pose.position.y = y;
            waypt2.pose.position.z = 0;
            msg2.poses.push_back(waypt2);
        }

        g_msgpub5.publish(msg2);
        g_msgpub4.publish(msg);// 최종으로 경로가 토픽으로 퍼블리쉬 되는 곳
    }

    UPDATINGMAP=false;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PLANNER_FRAMEWORK()
{
    int argc=0;
    char** argv;
    ros::init(argc, argv, "DecisionMaker");
    ros::NodeHandle node;
    ros::NodeHandle priv_nh("~");
    g_msgpub1 = node.advertise<geometry_msgs::PoseArray>("PoseArray_RRT_1", 1);//안쓴다.
    g_msgpub2 = node.advertise<geometry_msgs::PoseArray>("PoseArray_RRT_2", 1);
    g_msgpub3 = node.advertise<geometry_msgs::PoseArray>("PoseArray_RRT_3", 1);
    g_msgpub4 = node.advertise<nav_msgs::Path>("Path_RRT", 1);
    g_msgpub5 = node.advertise<nav_msgs::Path>("RNDFPathData", 1);

    g_msgsub1 = node.subscribe("velodyne_potential_array", 1, SubTopicProcess1);
    //g_msgsub2 = node.subscribe("velodyne_obstaclesGlobal", 1, SubTopicProcess2);

    cout << "START of DO-RRT*: ROS Version"<<endl;
    ros::Rate loop_rate(1);

    while(ros::ok())
    {
        ros::spinOnce();
        sleep(0.01);
    }
    cout << "END of DO-RRT*: ROS Version"<<endl;
}

int main(int argc, char* argv[])
{

    try
    {
        po::options_description desc("Options");
        desc.add_options()
            ("x", po::value<double>(),"goalX")
            ("y", po::value<double>(),"goalY")
            ("yaw", po::value<double>(),"goalYaw")
            ("globalT", po::value<double>(),"globalT")
            ("localT", po::value<double>(),"localT")
            ("bminx", po::value<double>(),"BminX")
            ("bminy", po::value<double>(),"BminY")
            ("bmaxx", po::value<double>(),"BmaxX")
            ("bmaxy", po::value<double>(),"BmaxY")
            ("rrt", po::value<double>(),"rrtmode")
            ;

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc,
                    po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);

        double x, y, yaw;
        if (vm.count("x"))
        {
            GOAL_G[0] = vm["x"].as<double>();
        }
        if (vm.count("y"))
        {
            GOAL_G[1] = vm["y"].as<double>();
        }
        if (vm.count("yaw"))
        {
            GOAL_G[2] = RADIANS(vm["yaw"].as<double>());
            GOAL_G[2] = AngleUtils::toRange_PItoPI(GOAL_G[2]);
        }
        if (vm.count("globalT"))
        {
            PLANNINGTIME = vm["globalT"].as<double>();
        }
        if (vm.count("localT"))
        {
            PLANNINGTIME_L = vm["localT"].as<double>();
        }
        if (vm.count("bminx"))
        {
            BOUNDS.low[0] = vm["bminx"].as<double>();
        }
        if (vm.count("bmaxx"))
        {
            BOUNDS.high[0] = vm["bmaxx"].as<double>();
        }
        if (vm.count("bminy"))
        {
            BOUNDS.low[1] = vm["bminy"].as<double>();
        }
        if (vm.count("bmaxy"))
        {
            BOUNDS.high[1] = vm["bmaxy"].as<double>();
        }
        if (vm.count("rrt"))
        {
            MODEDORRTSTAR = 0;
        }
        VEHICLEPOS_TMP[0]=85.0;
        VEHICLEPOS_TMP[1]=0.0;
        VEHICLEPOS_TMP[2]=RADIANS(179);
        cout <<"BOUND " <<BOUNDS.low[0]<<" "<<BOUNDS.high[0]<<" "<<BOUNDS.low[1]<<" "<<BOUNDS.high[1]<<endl;

        GOAL_GORI[0] = GOAL_G[0];// GOAL_GORI은 안쓰임
        GOAL_GORI[1] = GOAL_G[1];
        GOAL_GORI[2] = GOAL_G[2];
/*
        //GOAL_G[0] = -10.0;
        //GOAL_G[1] = 0.0;
        //GOAL_G[2] = RADIANS(179);

        BOUNDS.low[0] = bminx;
        BOUNDS.high[0] = bmaxy;

        BOUNDS.low[1] = bminy;
        BOUNDS.high[1] = bmaxy;
        */
        ss_g = new og::SimpleSetup(g_space);

        ss_l = new og::SimpleSetup(g_space_local);

        boost::thread t=boost::thread(boost::bind(&PLANNER_FRAMEWORK));//실제로 저 플래너 프레임워크가 쓰레드로 계속 도는 것이다.

        t.join();
    }
    catch(std::exception& e) {
        std::cerr << "error: " << e.what() << "\n";
        return 1;
    }
    catch(...) {
        std::cerr << "Exception of unknown type!\n";
    }

    return 0;
}


#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/join.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <map>
#include <vector>

using StringVect = std::vector<std::string>;

std::map<std::string, std::string> rules;

void show(const StringVect& v)
{
    for (const auto& r : v) {
        std::cout << r << "\n";
    }
    std::cout << "-----------------\n";
}

void mixRules2(const std::string& a, const std::string& b)
{
    StringVect rows;
    boost::algorithm::split(rows, a, boost::is_any_of("/"), boost::algorithm::token_compress_on);

    // orig
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // top-down
    std::swap(rows[0], rows[1]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // rotate
    std::swap(rows[0][0], rows[1][1]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // top-down
    std::swap(rows[0], rows[1]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // std::cout << "============\n";
}

void mixRules3(std::string a, const std::string& b)
{
    StringVect rows;
    boost::algorithm::split(rows, a, boost::is_any_of("/"), boost::algorithm::token_compress_on);

    // orig
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // top-down
    std::swap(rows[0], rows[2]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // rotate
    std::swap(rows[0][1], rows[1][0]);
    std::swap(rows[0][2], rows[2][0]);
    std::swap(rows[1][2], rows[2][1]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // top-down
    std::swap(rows[0], rows[2]);
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});

    // left-right
    for (auto& r : rows) {
        std::reverse(r.begin(), r.end());
    }
    // show(rows);
    rules.insert({boost::algorithm::join(rows, "/"), b});
}

int main()
{
    std::string line;
    while (getline(std::cin, line)) {
        StringVect words;
        boost::algorithm::split(
            words, line, boost::is_any_of("\t"), boost::algorithm::token_compress_on);
        if (words[0].size() == 5) {
            mixRules2(words[0], words[1]);
        } else {
            mixRules3(words[0], words[1]);
        }
    }

    StringVect map{{".#."}, {"..#"}, {"###"}};

    for (int ll = 0; ll < 5; ++ll) {
        if ((map.size() / 2) * 2 == map[0].size()) {
            std::cout << "dvema\n";
            StringVect newMap;
            int ny = 0;
            int x = 0;
            int y = 0;
            for (;;) {
                std::string k;
                const auto& row0 = map[y];
                const auto& row1 = map[y + 1];
                k.push_back(row0[x]);
                k.push_back(row0[x + 1]);
                k.push_back('/');
                k.push_back(row1[x]);
                k.push_back(row1[x + 1]);

                auto it = rules.find(k);
                if (it != rules.end()) {
                    StringVect rows;
                    boost::algorithm::split(
                        rows, it->second, boost::is_any_of("/"),
                        boost::algorithm::token_compress_on);
                    if (newMap.size() < ny + 3) {
                        newMap.resize(ny + 3);
                    }
                    newMap[ny].append(rows[0]);
                    newMap[ny + 1].append(rows[1]);
                    newMap[ny + 2].append(rows[2]);
                } else {
                    std::cerr << "NOT FOUND! r: " << y << ", c: " << x << "\n";
                }
                x += 2;
                if (x == row0.size()) {
                    x = 0;
                    y += 2;
                    ny += 3;
                }
                if (y == map.size()) {
                    break;
                }
            }
            std::swap(newMap, map);
        } else if ((map[0].size() / 3) * 3 == map[0].size()) {
            std::cout << "trema\n";
            StringVect newMap;
            int ny = 0;
            int x = 0;
            int y = 0;
            for (;;) {
                std::string k;
                const auto& row0 = map[y];
                const auto& row1 = map[y + 1];
                const auto& row2 = map[y + 2];
                k.push_back(row0[x]);
                k.push_back(row0[x + 1]);
                k.push_back(row0[x + 2]);
                k.push_back('/');
                k.push_back(row1[x]);
                k.push_back(row1[x + 1]);
                k.push_back(row1[x + 2]);
                k.push_back('/');
                k.push_back(row2[x]);
                k.push_back(row2[x + 1]);
                k.push_back(row2[x + 2]);

                auto it = rules.find(k);
                if (it != rules.end()) {
                    StringVect rows;
                    boost::algorithm::split(
                        rows, it->second, boost::is_any_of("/"),
                        boost::algorithm::token_compress_on);
                    if (newMap.size() < ny + 4) {
                        newMap.resize(ny + 4);
                    }
                    newMap[ny].append(rows[0]);
                    newMap[ny + 1].append(rows[1]);
                    newMap[ny + 2].append(rows[2]);
                    newMap[ny + 3].append(rows[3]);
                } else {
                    std::cerr << "NOT FOUND! r: " << y << ", c: " << x << "\n";
                }
                x += 3;
                if (x == row0.size()) {
                    x = 0;
                    y += 3;
                    ny += 4;
                }
                if (y == map.size()) {
                    break;
                }
            }
            std::swap(newMap, map);
        } else {
            std::cerr << "ERROR! " << map[0].size();
        }

        show(map);
    }

    int cnt = 0;
    for (const auto& r : map) {
        cnt += std::count_if(r.begin(), r.end(), [](const unsigned char c) { return c == '#'; });
    }
    std::cout << "cnt: " << cnt << "\n";

    return 0;
}


/****************************************************************************
 * Copyright (c) 2012-2019 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborX_BoostGeometryAdapters.hpp"
#include <ArborX_DetailsAlgorithms.hpp>

#include <boost/test/unit_test.hpp>

#define BOOST_TEST_MODULE BoostGeometryAdapters

namespace bg = boost::geometry;
namespace details = ArborX::Details;
// Conveniently importing Point and Box in ArborX::Details:: namespace
// and declaring type aliases within boost::geometry:: so that we are able to
// just use details:: and bg:: to specify what geometry or algorithm we mean.
namespace ArborX
{
namespace Details
{
using ArborX::Box;
using ArborX::Point;
} // namespace Details
} // namespace ArborX
namespace boost
{
namespace geometry
{
using Point = model::point<double, 3, cs::cartesian>;
using Box = model::box<Point>;
} // namespace geometry
} // namespace boost

BOOST_AUTO_TEST_CASE(equals)
{
  details::Point point = {{1., 2., 3.}};
  BOOST_TEST(details::equals(point, {{1., 2., 3.}}));
  BOOST_TEST(!details::equals(point, {{0., 0., 0.}}));
  BOOST_TEST(bg::equals(point, bg::make<details::Point>(1., 2., 3.)));
  BOOST_TEST(!bg::equals(point, bg::make<details::Point>(4., 5., 6.)));
  BOOST_TEST(bg::equals(point, bg::make<bg::Point>(1., 2., 3.)));
  BOOST_TEST(!bg::equals(point, bg::make<bg::Point>(0., 0., 0.)));

  details::Box box = {{{1., 2., 3.}}, {{4., 5., 6.}}};
  BOOST_TEST(details::equals(box, {{{1., 2., 3.}}, {{4., 5., 6.}}}));
  BOOST_TEST(!details::equals(box, {{{0., 0., 0.}}, {{1., 1., 1.}}}));
  BOOST_TEST(bg::equals(box, details::Box{{{1., 2., 3.}}, {{4., 5., 6.}}}));
  BOOST_TEST(!bg::equals(box, details::Box{{{0., 0., 0.}}, {{1., 1., 1.}}}));
  BOOST_TEST(bg::equals(box, bg::Box(bg::make<bg::Point>(1., 2., 3.),
                                     bg::make<bg::Point>(4., 5., 6.))));
  BOOST_TEST(!bg::equals(box, bg::Box(bg::make<bg::Point>(0., 0., 0.),
                                      bg::make<bg::Point>(1., 1., 1.))));
  // Now just for fun compare the ArborX box to a Boost.Geometry box composed
  // of ArborX points.
  BOOST_TEST(bg::equals(
      box, bg::model::box<details::Point>({{1., 2., 3.}}, {{4., 5., 6.}})));
  BOOST_TEST(!bg::equals(
      box, bg::model::box<details::Point>({{0., 0., 0.}}, {{0., 0., 0.}})));
}

BOOST_AUTO_TEST_CASE(distance)
{
  // NOTE unsure if should test for floating point equality here
  details::Point a = {{0., 0., 0.}};
  details::Point b = {{0., 1., 0.}};
  BOOST_TEST(details::distance(a, b) == 1.);
  BOOST_TEST(bg::distance(a, b) == 1.);

  std::tie(a, b) = std::make_pair<details::Point, details::Point>(
      {{0., 0., 0.}}, {{1., 1., 1.}});
  BOOST_TEST(details::distance(a, b) == std::sqrt(3.));
  BOOST_TEST(bg::distance(a, b) == std::sqrt(3.));

  BOOST_TEST(details::distance(a, a) == 0.);
  BOOST_TEST(bg::distance(a, a) == 0.);

  details::Box unit_box = {{{0., 0., 0.}}, {{1., 1., 1.}}};
  details::Point p = {{.5, .5, .5}};
  // NOTE ArborX has no overload distance( Box, Point )
  BOOST_TEST(details::distance(p, unit_box) == 0.);
  // BOOST_TEST( details::distance( unit_box, p ) == 0. );
  BOOST_TEST(bg::distance(p, unit_box) == 0.);
  BOOST_TEST(bg::distance(unit_box, p) == 0.);

  p = {{-1., -1., -1.}};
  BOOST_TEST(details::distance(p, unit_box) == std::sqrt(3.));
  BOOST_TEST(bg::distance(p, unit_box) == std::sqrt(3.));

  p = {{-1., .5, -1.}};
  BOOST_TEST(details::distance(p, unit_box) == std::sqrt(2.));
  BOOST_TEST(bg::distance(p, unit_box) == std::sqrt(2.));

  p = {{-1., .5, .5}};
  BOOST_TEST(details::distance(p, unit_box) == 1.);
  BOOST_TEST(bg::distance(p, unit_box) == 1.);
}

BOOST_AUTO_TEST_CASE(expand)
{
  using details::equals;
  details::Box box;
  // NOTE even though not considered valid, default constructed ArborX box can
  // be expanded using Boost.Geometry algorithm.
  BOOST_TEST(!bg::is_valid(box));
  bg::expand(box, details::Point{{0., 0., 0.}});
  details::expand(box, {{1., 1., 1.}});
  BOOST_TEST(equals(box, {{{0., 0., 0.}}, {{1., 1., 1.}}}));
  bg::expand(box, details::Box{{{1., 2., 3.}}, {{4., 5., 6.}}});
  details::expand(box, {{{-1., -2., -3.}}, {{0., 0., 0.}}});
  BOOST_TEST(equals(box, {{{-1., -2., -3.}}, {{4., 5., 6.}}}));
}

BOOST_AUTO_TEST_CASE(centroid)
{
  using details::equals;

  // Interestingly enough, even though for Boost.Geometry, the ArborX default
  // constructed "empty" box is not valid, it will still calculate its
  // centroid.  Admittedly, the result (centroid at origin) is garbage :)
  details::Box empty_box = {};
  BOOST_TEST(!bg::is_valid(empty_box));
  BOOST_TEST(
      equals(bg::return_centroid<details::Point>(empty_box), {{0., 0., 0.}}));
  BOOST_TEST(equals(details::returnCentroid(empty_box), {{0., 0., 0.}}));

  details::Box unit_box = {{{0., 0., 0.}}, {{1., 1., 1.}}};
  BOOST_TEST(
      equals(bg::return_centroid<details::Point>(unit_box), {{.5, .5, .5}}));
  BOOST_TEST(equals(details::returnCentroid(unit_box), {{.5, .5, .5}}));

  details::Point a_point = {{1., 2., 3.}};
  BOOST_TEST(equals(bg::return_centroid<details::Point>(a_point), a_point));
  BOOST_TEST(equals(details::returnCentroid(a_point), a_point));
}

BOOST_AUTO_TEST_CASE(is_valid)
{
  // NOTE "empty" box is considered as valid in ArborX but it is
  // not according to Boost.Geometry
  details::Box empty_box = {};
  BOOST_TEST(details::isValid(empty_box));
  std::string message;
  BOOST_TEST(!bg::is_valid(empty_box, message));
  BOOST_TEST(message == "Box has corners in wrong order");

  // Same issue with infinitesimal box around a point (here the origin)
  details::Box a_box = {{{0., 0., 0.}}, {{0., 0., 0.}}};
  BOOST_TEST(details::isValid(a_box));
  BOOST_TEST(!bg::is_valid(a_box, message));
  BOOST_TEST(message == "Geometry has wrong topological dimension");

  details::Box unit_box = {{{0., 0., 0.}}, {{1., 1., 1.}}};
  BOOST_TEST(details::isValid(unit_box));
  BOOST_TEST(bg::is_valid(unit_box));

  details::Box invalid_box = {{{1., 2., 3.}}, {{4., NAN, 6.}}};
  BOOST_TEST(!details::isValid(invalid_box));
  BOOST_TEST(!bg::is_valid(invalid_box, message));
  BOOST_TEST(message == "Geometry has point(s) with invalid coordinate(s)");

  details::Point a_point = {{1., 2., 3.}};
  BOOST_TEST(details::isValid(a_point));
  BOOST_TEST(bg::is_valid(a_point));

  auto const infty = std::numeric_limits<double>::infinity();
  details::Point invalid_point = {{infty, 1.41, 3.14}};
  BOOST_TEST(!details::isValid(invalid_point));
  BOOST_TEST(!bg::is_valid(invalid_point, message));
  BOOST_TEST(message == "Geometry has point(s) with invalid coordinate(s)");

  // Also Boost.Geometry has a is_empty() algorithm but it has a different
  // meaning, it checks whether a geometry is an empty set and always returns
  // false for a point or a box.
  BOOST_TEST(!bg::is_empty(empty_box));
  BOOST_TEST(!bg::is_empty(a_box));
  BOOST_TEST(!bg::is_empty(unit_box));
}

BOOST_AUTO_TEST_CASE(intersects)
{
  details::Box const unit_box = {{{0., 0., 0.}}, {{1., 1., 1.}}};

  // self-intersection
  BOOST_TEST(details::intersects(unit_box, unit_box));
  BOOST_TEST(bg::intersects(unit_box, unit_box));

  // share a corner
  details::Box other_box = {{{1., 1., 1.}}, {{2., 2., 2.}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // share an edge
  other_box = {{{1., 0., 1.}}, {{2., 1., 2.}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // share a face
  other_box = {{{0., -1., 0.}}, {{1., 0., 1.}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // contains the other box
  other_box = {{{.3, .3, .3}}, {{.6, .6, .6}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // within the other box
  other_box = {{{-1., -1., -1.}}, {{2., 2., 2.}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // overlapping
  other_box = {{{.5, .5, .5}}, {{2., 2., 2.}}};
  BOOST_TEST(details::intersects(unit_box, other_box));
  BOOST_TEST(bg::intersects(unit_box, other_box));

  // disjoint
  other_box = {{{1., 2., 3.}}, {{4., 5., 6.}}};
  BOOST_TEST(!details::intersects(unit_box, other_box));
  BOOST_TEST(!bg::intersects(unit_box, other_box));
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"

#include "graph_filtering.hh"

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

#include <cmath>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

            if (last)
                layers.pop_back();
        }

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

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

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

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

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

    typedef vprop_map_t<double>::type wmap_t;

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

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

#include <boost/python.hpp>

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


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/range/input_range.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/range.hpp>
#include <simd_test.hpp>
#include <vector>

template<typename Range, typename Out> inline void mycopy(Range const& r, Out dst)
{
  for( auto const& p : r)  *dst++ = p;
}

STF_CASE_TPL("distance", STF_NUMERIC_TYPES)
{
  using boost::simd::input_range;
  using boost::simd::pack;

  std::vector<T> data(pack<T>::static_size*3), data2(24);

  auto rng  = input_range(data);
  auto rng8 = input_range<8>(data2);

  STF_EQUAL( std::distance(std::begin(rng) , std::end(rng) ), 3 );
  STF_EQUAL( std::distance(std::begin(rng8), std::end(rng8)), 3 );
}

STF_CASE_TPL("iteration", STF_NUMERIC_TYPES)
{
  using boost::simd::input_range;
  using boost::simd::pack;

  std::vector<pack<T>,boost::simd::allocator<T>>  dst(3), ref(3);
  std::vector<T>          data(pack<T>::static_size*3);

  for(std::size_t i=0;i<data.size();i++)
    data[i] = i/pack<T>::static_size+1;

  for(std::size_t i=0;i<ref.size();i++)
    ref[i] = pack<T>(i+1);

  auto simd_range = input_range(data);
  mycopy(simd_range , dst.begin());

  STF_ALL_EQUAL( ref, dst );
}


#include "testsuite.h"
#include <blitz/tinyvec2.h>
#include <blitz/tinyvec2.cc>
#include <blitz/array.h>
#include <blitz/array.cc>

BZ_USING_NAMESPACE(blitz)

int main()
{
  TinyVector<int,5> d;
    d=1,-1,2,-2,0;
    BZTEST(d(0) == 1);
    BZTEST(d(1) == -1);
    BZTEST(d(2) == 2);
    BZTEST(d(3) == -2);
    BZTEST(d(4) == 0);

  TinyVector<double,5> c;
    c=1.0,-1.0,2.0,-2.0,0.0;
    BZTEST(c(0) == 1);
    BZTEST(c(1) == -1);
    BZTEST(c(2) == 2);
    BZTEST(c(3) == -2);
    BZTEST(c(4) == 0);

    // and finally test initializations where implicit conversions to
    // T_numtype are necessary

  TinyVector<double,5> b;
    b=1,-1,2,-2,0;
    BZTEST(b(0) == 1);
    BZTEST(b(1) == -1);
    BZTEST(b(2) == 2);
    BZTEST(b(3) == -2);
    BZTEST(b(4) == 0);

    return 0;
}