/**
 * @file rrts02-nearestNeighbors.cpp
 * @author Can Erdogan
 * @date Feb 04, 2013
 * @brief Checks if the nearest neighbor computation done by flann is correct.
 */

#include <iostream>
#include <gtest/gtest.h>
#include <Eigen/Core>
#include <dart/dart.hpp>
#if HAVE_FLANN
#include <flann/flann.hpp>
#endif // HAVE_FLANN
#include "TestHelpers.hpp"

/* ********************************************************************************************* */
#if HAVE_FLANN
TEST(NEAREST_NEIGHBOR, 2D) {

    // Build the index with the first node
    flann::Index<flann::L2<s_t> > index (flann::KDTreeSingleIndexParams(10, true));
    Eigen::VectorXs p1 (2);
    p1 << -3.04159, -3.04159;
    index.buildIndex(flann::Matrix<s_t>((s_t*)p1.data(), 1, p1.size()));

    // Add two more points
    Eigen::Vector2s p2 (-2.96751, -2.97443), p3 (-2.91946, -2.88672);
    index.addPoints(flann::Matrix<s_t>((s_t*)p2.data(), 1, p2.size()));
    index.addPoints(flann::Matrix<s_t>((s_t*)p3.data(), 1, p3.size()));

    // Check the size of the tree
    EXPECT_EQ(3, (int)index.size());

    // Get the nearest neighbor index for a sample point
    Eigen::Vector2s sample (-2.26654, 2.2874);
    int nearest;
    s_t distance;
    const flann::Matrix<s_t> queryMatrix((s_t*)sample.data(), 1, sample.size());
    flann::Matrix<int> nearestMatrix(&nearest, 1, 1);
    flann::Matrix<s_t> distanceMatrix(flann::Matrix<s_t>(&distance, 1, 1));
    index.knnSearch(queryMatrix, nearestMatrix, distanceMatrix, 1,
        flann::SearchParams(flann::FLANN_CHECKS_UNLIMITED));
    EXPECT_EQ(2, nearest);

    // Get the nearest neighbor
    s_t* point = index.getPoint(nearest);
    bool equality = equals(Vector2s(point[0], point[1]), p3, 1e-3);
    EXPECT_TRUE(equality);
}
#endif // HAVE_FLANN


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2022 Heal Research

#include <algorithm>
#include <doctest/doctest.h>
#include <Eigen/Core>

#include "operon/analyzers/diversity.hpp"
#include "operon/core/dataset.hpp"
#include "operon/core/pset.hpp"
#include "operon/core/tree.hpp"
#include "operon/operators/creator.hpp"
#include "operon/operators/initializer.hpp"

namespace Operon::Test {

TEST_CASE("diversity")
{
    PrimitiveSet grammar;
    grammar.SetConfig(PrimitiveSet::Full);
    Operon::RandomGenerator rd(std::random_device {}());

    Eigen::Matrix<Operon::Scalar, -1, -1> values(1, 2);
    values << 1, 1; // don't care

    Dataset ds(values);
    BalancedTreeCreator btc(grammar, ds.Variables());

    constexpr size_t minLength{1};
    constexpr size_t maxLength{100};
    constexpr size_t maxDepth{1000};

    UniformTreeInitializer treeInit(btc);
    treeInit.ParameterizeDistribution(minLength, maxLength);
    treeInit.SetMaxDepth(maxDepth);
    UniformCoefficientInitializer coeffInit;
    coeffInit.ParameterizeDistribution(Operon::Scalar{-1}, Operon::Scalar{+1});

    constexpr size_t nTrees = 1000;
    std::vector<Tree> trees;
    trees.reserve(nTrees);

    for(auto i = 0UL; i < nTrees; ++i) {
        auto tree = treeInit(rd);
        coeffInit(rd, tree);
        trees.push_back(std::move(tree));
    }

    PopulationDiversityAnalyzer<Tree> diversityAnalyzer;
    diversityAnalyzer.Prepare(trees);
}

} // namespace Operon::Test


//==================================================================================================
/*!

  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/function/scalar/majority.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/true.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/quarter.hpp>

STF_CASE_TPL (" majorityreal",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::majority;
  using r_t = decltype(majority(T(),T(),T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(majority(bs::Inf<T>(), bs::Inf<T>(), bs::Inf<T>()), r_t(true));
  STF_EQUAL(majority(bs::Minf<T>(), bs::Minf<T>(), bs::Minf<T>()), r_t(true));
  STF_EQUAL(majority(bs::Nan<T>(), bs::Nan<T>(), bs::Nan<T>()), r_t(true));
#endif
  STF_EQUAL(majority(-bs::Zero<T>(), -bs::Zero<T>(), -bs::Zero<T>()), r_t(false));
  STF_EQUAL(majority(bs::Half<T>(), bs::Half<T>(), bs::Half<T>()), r_t(true));
  STF_EQUAL(majority(bs::Mone<T>(), bs::Mone<T>(), bs::Mone<T>()), r_t(true));
  STF_EQUAL(majority(bs::One<T>(), bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(majority(bs::Quarter<T>(), bs::Quarter<T>(), bs::Quarter<T>()), r_t(true));
  STF_EQUAL(majority(bs::Two<T>(), bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(majority(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" majoritysigned_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::majority;
  using r_t = decltype(majority(T(),T(),T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(majority(bs::Mone<T>(), bs::Mone<T>(), bs::Mone<T>()), r_t(true));
  STF_EQUAL(majority(bs::One<T>(), bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(majority(bs::Two<T>(), bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(majority(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" majorityunsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::majority;
  using r_t = decltype(majority(T(),T(),T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(majority(bs::One<T>(), bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(majority(bs::Two<T>(), bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(majority(bs::Zero<T>(), bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}
STF_CASE ( "majority bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::majority;
  using r_t = decltype(majority(bool(),bool(),bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(majority(true, false, true), true);
  STF_EQUAL(majority(false, true, true), true);
  STF_EQUAL(majority(true, true, false), true);
  STF_EQUAL(majority(false, false, true), false);
}

STF_CASE_TPL ( "majority bool", STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::majority;
  using lT = bs::logical<T>;
  using r_t = decltype(majority(lT(),lT(),lT()));

  // return type conformity test
  STF_TYPE_IS(r_t, lT);

  // specific values tests
  STF_EQUAL(majority(bs::True<lT>(), bs::False<lT>(), bs::True<lT>()), bs::True<lT>());
  STF_EQUAL(majority(bs::False<lT>(), bs::True<lT>(), bs::True<lT>()), bs::True<lT>());
  STF_EQUAL(majority(bs::True<lT>(), bs::True<lT>(), bs::False<lT>()), bs::True<lT>());
  STF_EQUAL(majority(bs::False<lT>(), bs::False<lT>(), bs::True<lT>()), bs::False<lT>());
  STF_EQUAL(majority(bs::False<lT>(), bs::True<lT>(), bs::False<lT>()), bs::False<lT>());
  STF_EQUAL(majority(bs::True<lT>(), bs::False<lT>(), bs::False<lT>()), bs::False<lT>());
}


/*
 * File: svd.hpp
 * Project: include
 * Created Date: Saturday, November 30th 2019, 6:55:15 am
 * Author: Ramaseshan Ramachandran
 * -----
 * Last Modified: Saturday, 30th November 2019 9:18:02 am
 * Modified By: Ramaseshan Ramachandran
 * -----
 * Copyright (c) 2019 JCR Labz
 * blah
 * -----
 * HISTORY:
 * Date      	By	Comments
 * ----------	---	----------------------------------------------------------
 */

#ifndef __SVD_H__
#define __SVD_H__
#include <string>
#include <armadillo>

namespace nlp{
class svd {
protected:
std::string filename;
public:
    svd(std::string cooccur_filename);
    ~svd();
    void decompose(arma::mat matrix);
};
}
#endif //__SVD_H__


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file Values.h
 * @author Richard Roberts
 *
 * @brief A non-templated config holding any types of Manifold-group elements
 *
 *  Detailed story:
 *  A values structure is a map from keys to values. It is used to specify the value of a bunch
 *  of variables in a factor graph. A Values is a values structure which can hold variables that
 *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
 *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
 */

#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/iterator/transform_iterator.hpp>

#include <list>
#include <memory>
#include <sstream>

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  Values::Values(const Values& other) {
    this->insert(other);
  }

  /* ************************************************************************* */
  Values::Values(Values&& other) : values_(std::move(other.values_)) {
  }

  /* ************************************************************************* */
  Values::Values(const Values& other, const VectorValues& delta) {
    for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) {
      VectorValues::const_iterator it = delta.find(key_value->key);
      Key key = key_value->key;  // Non-const duplicate to deal with non-const insert argument
      if (it != delta.end()) {
        const Vector& v = it->second;

        Value* retractedValue(key_value->value.retract_(v));  // Retract
        values_.insert(key, retractedValue);  // Add retracted result directly to result values
      } else {
        values_.insert(key, key_value->value.clone_());  // Add original version to result values
      }
    }
  }
//======================= Values(is_mh) for mhRetract() =======================
  //[MH-A]: actually same as the original Values(other, delta), but can be separated for later functionalities...
  Values::Values(Values& other, const VectorValues& delta, const bool& is_mh) { //is_mh should always be true
    for (iterator key_value = other.begin(); key_value != other.end(); ++key_value) {
      VectorValues::const_iterator it = delta.find(key_value->key); //see if this Key required to be updated
      Key key = key_value->key;  // Non-const duplicate to deal with non-const insert argument
      if (it != delta.end()) { //found
        const Vector& v = it->second;

        //[MH-A]: MHGV::retract_()
        Value* retractedValue(key_value->value.retract_(v));  // Retract

        values_.insert(key, retractedValue);  // Add retracted result directly to result values

      } else {
        values_.insert(key, key_value->value.clone_());  // Add original version to result values
      }

    }
  }
//======================= END Values(is_mh) for mhRetract() =======================
  void Values::mhRetractInPlace(const VectorValues& delta) {
    for (iterator key_value = begin(); key_value != end(); ++key_value) {
      VectorValues::const_iterator it = delta.find(key_value->key); //see if this Key required to be updated
      if (it != delta.end()) { //found
        const Vector& v = it->second;

        //[MH-A]: MHGV::retract_()
        key_value->value.retractInPlace_(v);  // Retract

      } // END if
    } // END for
  } // END mhRetractInPlace()

  /* ************************************************************************* */
  void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
    cout << str << "Values with " << size() << " values:" << endl;
    for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
      cout << "Value " << keyFormatter(key_value->key) << ": ";
      key_value->value.print("");
      cout << "\n";
    }
  }

  /* ************************************************************************* */
  bool Values::equals(const Values& other, double tol) const {
    if (this->size() != other.size())
      return false;
    for (const_iterator it1 = this->begin(), it2 = other.begin();
        it1 != this->end(); ++it1, ++it2) {
      const Value& value1 = it1->value;
      const Value& value2 = it2->value;
      if (typeid(value1) != typeid(value2) || it1->key != it2->key
          || !value1.equals_(value2, tol)) {
        return false;
      }
    }
    return true; // We return false earlier if we find anything that does not match
}

  /* ************************************************************************* */
  bool Values::exists(Key j) const {
    return values_.find(j) != values_.end();
  }

  /* ************************************************************************* */
  Values Values::retract(const VectorValues& delta) const {
    return Values(*this, delta);
  }
//========================= mhRetract() ======================
  //[MH-A]:
  Values Values::mhRetract(const VectorValues& delta) {
    return Values(*this, delta, true);
  }
//========================= END mhRetract() ======================

  /* ************************************************************************* */
  VectorValues Values::localCoordinates(const Values& cp) const {
    if(this->size() != cp.size())
      throw DynamicValuesMismatched();
    VectorValues result;
    for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
      if(it1->key != it2->key)
        throw DynamicValuesMismatched(); // If keys do not match
      // Will throw a dynamic_cast exception if types do not match
      // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert
      result.insert(it1->key, it1->value.localCoordinates_(it2->value));
    }
    return result;
  }

  /* ************************************************************************* */
  const Value& Values::at(Key j) const {
    // Find the item
    KeyValueMap::const_iterator item = values_.find(j);

    // Throw exception if it does not exist
    if(item == values_.end())
      throw ValuesKeyDoesNotExist("retrieve", j);
    return *item->second;
  }

//====================== non-const at() ========================
  //[MH-A]: MH has to modify the values
  Value& Values::at(Key j) {
    // Find the item
    KeyValueMap::iterator item = values_.find(j);

    // Throw exception if it does not exist
    if(item == values_.end())
      throw ValuesKeyDoesNotExist("retrieve", j);
    return *item->second;
  }
//====================== END non-const at() ========================

  /* ************************************************************************* */
  void Values::insert(Key j, const Value& val) {
    std::pair<iterator,bool> insertResult = tryInsert(j, val);
    if(!insertResult.second)
      throw ValuesKeyAlreadyExists(j);
  }

  //=========================== MH ============================
  //[MH-A]:
  void Values::mhInsert(Key j, const Value& val) {
    std::pair<iterator,bool> insertResult = tryInsert(j, val);
    if(!insertResult.second)
      throw ValuesKeyAlreadyExists(j);

    //[MH-A]: link from HypoTree

    const HypoList& hypo_list = val.getHypoList();

    const GenericList& generic_list = val.getGenericList();

    if (generic_list.size() != hypo_list.size()) {
        std::cout << "ERROR: generic_list.size() != hypo_list.size()... Make sure to call mhCalculateBestEstimate() after pruning if the MHGVs are used later!!" << std::endl;
    }

    GenericListCstIter git = generic_list.begin();

    for (HypoListCstIter hit = hypo_list.begin(); hit != hypo_list.end(); ++hit) {

      // Setup Key-Value* map in each HypoNode
      (*hit)->addKeyValuePair(j, (*git));

      ++git;
    }
  }
  //=========================== END MH ============================

  /* ************************************************************************* */
  void Values::insert(const Values& values) {
    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
      Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
      insert(key, key_value->value); //NOT calling insert(Key, T)
    }
  }

//========================= MH ================================
  //[MH-A]:
  void Values::mhInsert(const Values& values) {
    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
      Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
      mhInsert(key, key_value->value); //NOT calling insert(Key, T)

      (key_value->value.getHypoLayer())->setKeyMap(key);

    }
  }

//========================= END MH ================================

  /* ************************************************************************* */
  std::pair<Values::iterator, bool> Values::tryInsert(Key j, const Value& value) {
    std::pair<KeyValueMap::iterator, bool> result = values_.insert(j, value.clone_());
    return std::make_pair(boost::make_transform_iterator(result.first, &make_deref_pair), result.second);
  }

  /* ************************************************************************* */
  void Values::update(Key j, const Value& val) {
    // Find the value to update
    KeyValueMap::iterator item = values_.find(j);
    if (item == values_.end())
      throw ValuesKeyDoesNotExist("update", j);

    // Cast to the derived type
    const Value& old_value = *item->second;
    if (typeid(old_value) != typeid(val))
      throw ValuesIncorrectType(j, typeid(old_value), typeid(val));

    values_.replace(item, val.clone_());
  }

  /* ************************************************************************* */
  void Values::update(const Values& values) {
    for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
      this->update(key_value->key, key_value->value);
    }
  }

  /* ************************************************************************* */
  void Values::erase(Key j) {
    KeyValueMap::iterator item = values_.find(j);
    if(item == values_.end())
      throw ValuesKeyDoesNotExist("erase", j);
    values_.erase(item);
  }

  /* ************************************************************************* */
  KeyVector Values::keys() const {
    KeyVector result;
    result.reserve(size());
    for(const_iterator key_value = begin(); key_value != end(); ++key_value)
      result.push_back(key_value->key);
    return result;
  }

  /* ************************************************************************* */
  Values& Values::operator=(const Values& rhs) {
    this->clear();
    this->insert(rhs);
    return *this;
  }

  /* ************************************************************************* */
  size_t Values::dim() const {
    size_t result = 0;
    for(const ConstKeyValuePair& key_value: *this) {
      result += key_value.value.dim();
    }
    return result;
  }

  /* ************************************************************************* */
  VectorValues Values::zeroVectors() const {
    VectorValues result;
    for(const ConstKeyValuePair& key_value: *this)
      result.insert(key_value.key, Vector::Zero(key_value.value.hypoNum()*key_value.value.dim())); //the hypoNum() for original Value is 1
    return result;
  }

  /* ************************************************************************* */
  const char* ValuesKeyAlreadyExists::what() const throw() {
    if(message_.empty())
      message_ =
          "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* ValuesKeyDoesNotExist::what() const throw() {
    if(message_.empty())
      message_ =
          "Attempting to " + std::string(operation_) + " the key \"" +
          DefaultKeyFormatter(key_) + "\", which does not exist in the Values.";
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* ValuesIncorrectType::what() const throw() {
    if(message_.empty())
      message_ =
          "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
          std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name());
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* NoMatchFoundForFixed::what() const throw() {
    if(message_.empty()) {
      ostringstream oss;
    oss
        << "Attempting to retrieve fixed-size matrix with dimensions " //
        << M1_ << "x" << N1_
        << ", but found dynamic Matrix with mismatched dimensions " //
        << M2_ << "x" << N2_;
      message_ = oss.str();
    }
    return message_.c_str();
  }

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 optimize toolbox - brent"

#include <iostream>
#include <nt2/include/functions/brent.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>

template<class T> inline T f0( T x ) { return x*x*x-3*x+4; }

float f1( float x ) { return x*x*x-3*x+4; }

struct f2
{
  template<class T> inline T operator()( T x ) const
  {
    return x*x*x-3*x+4;
  }
};

double f3( double x, double a, double b ) { return x*x*x-a*x+b; }

NT2_TEST_CASE_TPL( brent_function_ptr, (double)(float) )
{
  using nt2::brent;
  using nt2::optimization::output;

  output<T,T> res = brent<T>( &f0<T>, 0, 0.5, 2 );

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( brent_function, (double)(float) )
{
  using nt2::brent;
  using nt2::optimization::output;

  output<T,T> res = brent<T>( f1, 0, 0.5, 2 );

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - 1.f), nt2::Sqrteps<float>());
}

NT2_TEST_CASE_TPL( brent_functor, (double)(float) )
{
  using nt2::brent;
  using nt2::optimization::output;

  output<T,T> res = brent<T>( f2(), 0, 0.5, 2 );

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( brent_bind, (double)(float) )
{
  using nt2::brent;
  using nt2::optimization::output;

  output<T,T> res = brent<T>( boost::bind(f3, _1, 3., 4.), 0, 0.5, 2 );

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( brent_lambdda, (double)(float) )
{
  using nt2::brent;
  using nt2::optimization::output;
  namespace bl = boost::lambda;

  output<T,T> res = brent<T>( bl::_1*bl::_1*bl::_1 - 3*bl::_1 + 4
                            , 0, 0.5, 2
                            );

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - T(1)), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( brent_tie, (double)(float) )
{
  using nt2::brent;
  using boost::fusion::tie;
  using boost::fusion::ignore;

  std::size_t i;
  T x,y,fx;
  bool convergence_successful;

  tie(x,fx,i,convergence_successful) = brent<T>( &f0<T>, 0, 0.5, 2 );
  std::cout << "Minimum : f(" << x << ") = " << fx
            << " after " << i <<  " iterations\n";

  NT2_TEST(convergence_successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(x - T(1)), nt2::Sqrteps<T>());

  tie(y,ignore,ignore,ignore) = brent<T>( &f0<T>, 0, 0.5, 2 );
  std::cout << "Minimum is at y = " << y << "\n";

  NT2_TEST_LESSER_EQUAL(nt2::abs(y - T(1)), nt2::Sqrteps<T>());
}

NT2_TEST_CASE_TPL( brent_option, (double)(float) )
{
  using nt2::brent;
  using nt2::options;
  using nt2::optimization::output;

  output<T,T> res = brent<T>( f1, 0, 0.5, 2
                            , options [ nt2::iterations_ = 10
                                      , nt2::tolerance::absolute_ = T(1e-3)
                                      ]
                            );

  std::cout << "Minimum : f(" << res.minimum << ") = "  << res.value
            << " after "      << res.iterations_count   <<  " iterations\n";

  NT2_TEST(res.successful);
  NT2_TEST_LESSER_EQUAL(nt2::abs(res.minimum - 1.f), 1e-3 );
  NT2_TEST_LESSER_EQUAL(res.iterations_count       , 10u  );

  res = brent<T>( f1, 0,0.5,2, options [ nt2::iterations_ = 1 ] );
  NT2_TEST(!res.successful);
  NT2_TEST_GREATER(res.iterations_count       , 1u );
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>

#include <Eigen/Core>
#include <functional>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geodetic/coordinates.hpp"
#include "pyinterp/detail/geodetic/system.hpp"
#include "pyinterp/detail/geometry/rtree.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/eigen.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp {

/// Type of radial functions exposed in the Python module.
using RadialBasisFunction = detail::math::RadialBasisFunction;

/// Type of window functions exposed in the Python module.
using WindowFunction = detail::math::window::Function;

/// RTree spatial index for geodetic point
///
/// @note
/// The tree of the "boost" library allows to directly handle the geodetic
/// coordinates, but it is much less efficient than the use of the tree in a
/// Cartesian space.
/// @tparam CoordinateType The class of storage for a point's coordinates.
/// @tparam Type The type of data stored in the tree.
/// @tparam N Number of dimensions in the Cartesian space handled.
template <typename CoordinateType, typename Type, size_t N>
class RTree : public detail::geometry::RTree<CoordinateType, Type, N> {
 public:
  /// The tree must at least store the ECEF coordinates
  static_assert(N >= 3,
                "The RTree must at least store the ECEF coordinates: x, y, z");

  /// Type of points handled by this instance
  using point_t =
      typename detail::geometry::RTree<CoordinateType, Type, N>::point_t;

  /// Type of distances between two points
  using distance_t =
      typename detail::geometry::RTree<CoordinateType, Type, N>::distance_t;

  /// Type of distances between two points.
  using result_t =
      typename detail::geometry::RTree<CoordinateType, Type, N>::result_t;

  /// Type of the implicit conversion between the type of coordinates and values
  using promotion_t =
      typename detail::geometry::RTree<CoordinateType, Type, N>::promotion_t;

  /// Pointer on the method converting LLA coordinates to ECEF.
  using Converter = point_t (RTree<CoordinateType, Type, N>::*)(
      const Eigen::Map<const Vector<CoordinateType>> &) const;

  /// Pointer on the method to Search for the nearest K nearest neighbors
  using Requester = std::vector<result_t> (RTree<CoordinateType, Type, N>::*)(
      const point_t &, const uint32_t) const;

  /// Default constructor
  explicit RTree(const std::optional<detail::geodetic::System> &wgs)
      : detail::geometry::RTree<CoordinateType, Type, N>(),
        coordinates_(wgs.value_or(detail::geodetic::System())) {}

  /// Returns the box able to contain all values stored in the container.
  ///
  /// @returns A tuple that contains the coordinates of the minimum and
  /// maximum corners of the box able to contain all values stored in the
  /// container or an empty tuple if there are no values in the container.
  [[nodiscard]] auto equatorial_bounds() const -> pybind11::tuple {
    if (this->empty()) {
      return pybind11::make_tuple();
    }

    auto x0 = std::numeric_limits<CoordinateType>::max();
    auto x1 = std::numeric_limits<CoordinateType>::min();
    auto y0 = std::numeric_limits<CoordinateType>::max();
    auto y1 = std::numeric_limits<CoordinateType>::min();
    auto z0 = std::numeric_limits<CoordinateType>::max();
    auto z1 = std::numeric_limits<CoordinateType>::min();

    std::for_each(this->tree_->begin(), this->tree_->end(),
                  [&](const auto &item) {
                    auto lla = to_lla(item.first);
                    x0 = std::min(x0, boost::geometry::get<0>(lla));
                    x1 = std::max(x1, boost::geometry::get<0>(lla));
                    y0 = std::min(y0, boost::geometry::get<1>(lla));
                    y1 = std::max(y1, boost::geometry::get<1>(lla));
                    z0 = std::min(z0, boost::geometry::get<2>(lla));
                    z1 = std::max(z1, boost::geometry::get<2>(lla));
                  });

    return pybind11::make_tuple(pybind11::make_tuple(x0, y0, z0),
                                pybind11::make_tuple(x1, y1, z1));
  }

  /// Populates the RTree with coordinates using the packaging algorithm
  ///
  /// @param coordinates Coordinates to be copied
  void packing(const pybind11::array_t<CoordinateType, pybind11::array::c_style>
                   &coordinates,
               const pybind11::array_t<Type> &values) {
    detail::check_array_ndim("coordinates", 2, coordinates);
    detail::check_array_ndim("values", 1, values);
    if (coordinates.shape(0) != values.size()) {
      throw std::invalid_argument(
          "coordinates, values could not be broadcast together with shape " +
          detail::ndarray_shape(coordinates) + ", " +
          detail::ndarray_shape(values));
    }
    switch (coordinates.shape(1)) {
      case N - 1:
        _packing<N - 1>(&RTree<CoordinateType, Type, N>::from_lon_lat,
                        coordinates, values);
        break;
      case N:
        _packing<N>(&RTree<CoordinateType, Type, N>::from_lon_lat_alt,
                    coordinates, values);
        break;
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// Insert new data into the search tree
  ///
  /// @param coordinates Coordinates to be copied
  void insert(const pybind11::array_t<CoordinateType, pybind11::array::c_style>
                  &coordinates,
              const pybind11::array_t<CoordinateType> &values) {
    detail::check_array_ndim("coordinates", 2, coordinates);
    detail::check_array_ndim("values", 1, values);
    if (coordinates.shape(0) != values.size()) {
      throw std::invalid_argument(
          "coordinates, values could not be broadcast together with shape " +
          detail::ndarray_shape(coordinates) + ", " +
          detail::ndarray_shape(values));
    }
    switch (coordinates.shape(1)) {
      case N - 1:
        _insert<N - 1>(&RTree<CoordinateType, Type, N>::from_lon_lat,
                       coordinates, values);
        break;
      case N:
        _insert<N>(&RTree<CoordinateType, Type, N>::from_lon_lat_alt,
                   coordinates, values);
        break;
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// Search for the nearest K nearest neighbors of a given coordinates.
  auto query(const pybind11::array_t<CoordinateType, pybind11::array::c_style>
                 &coordinates,
             const uint32_t k, const bool within,
             const size_t num_threads) const -> pybind11::tuple {
    detail::check_array_ndim("coordinates", 2, coordinates);
    switch (coordinates.shape(1)) {
      case N - 1:
        return _query<N - 1>(&RTree<CoordinateType, Type, N>::from_lon_lat,
                             coordinates, k, within, num_threads);
      case N:
        return _query<N>(&RTree<CoordinateType, Type, N>::from_lon_lat,
                         coordinates, k, within, num_threads);
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// TODO
  auto inverse_distance_weighting(
      const pybind11::array_t<CoordinateType, pybind11::array::c_style>
          &coordinates,
      const std::optional<distance_t> &radius, const uint32_t k,
      const uint32_t p, const bool within, const size_t num_threads) const
      -> pybind11::tuple {
    detail::check_array_ndim("coordinates", 2, coordinates);

    switch (coordinates.shape(1)) {
      case N - 1:
        return _inverse_distance_weighting<N - 1>(
            &RTree<CoordinateType, Type, N>::from_lon_lat, coordinates,
            radius.value_or(std::numeric_limits<distance_t>::max()), k, p,
            within, num_threads);
      case N:
        return _inverse_distance_weighting<N>(
            &RTree<CoordinateType, Type, N>::from_lon_lat, coordinates,
            radius.value_or(std::numeric_limits<distance_t>::max()), k, p,
            within, num_threads);
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// TODO
  auto radial_basis_function(
      const pybind11::array_t<CoordinateType, pybind11::array::c_style>
          &coordinates,
      const std::optional<distance_t> &radius, const uint32_t k,
      const RadialBasisFunction rbf, const std::optional<promotion_t> &epsilon,
      const promotion_t smooth, const bool within,
      const size_t num_threads) const -> pybind11::tuple {
    detail::check_array_ndim("coordinates", 2, coordinates);
    switch (coordinates.shape(1)) {
      case N - 1:
        return _rbf<N - 1>(
            &RTree<CoordinateType, Type, N>::from_lon_lat, coordinates,
            radius.value_or(std::numeric_limits<distance_t>::max()), k, rbf,
            epsilon.value_or(std::numeric_limits<promotion_t>::quiet_NaN()),
            smooth, within, num_threads);
      case N:
        return _rbf<N>(
            &RTree<CoordinateType, Type, N>::from_lon_lat_alt, coordinates,
            radius.value_or(std::numeric_limits<distance_t>::max()), k, rbf,
            epsilon.value_or(std::numeric_limits<promotion_t>::quiet_NaN()),
            smooth, within, num_threads);
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// TODO
  auto window_function(
      const pybind11::array_t<CoordinateType, pybind11::array::c_style>
          &coordinates,
      const distance_t &radius, const uint32_t k, const WindowFunction wf,
      const std::optional<distance_t> &arg, const bool within,
      const size_t num_threads) const -> pybind11::tuple {
    detail::check_array_ndim("coordinates", 2, coordinates);
    switch (coordinates.shape(1)) {
      case N - 1:
        return _window_function<N - 1>(
            &RTree<CoordinateType, Type, N>::from_lon_lat, coordinates, radius,
            k, wf, arg.value_or(0), within, num_threads);
      case N:
        return _window_function<N>(
            &RTree<CoordinateType, Type, N>::from_lon_lat_alt, coordinates,
            radius, k, wf, arg.value_or(0), within, num_threads);
      default:
        throw std::invalid_argument(
            RTree<CoordinateType, Type, N>::invalid_shape());
    }
  }

  /// Get a tuple that fully encodes the state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    auto x = pybind11::array_t<CoordinateType>(
        pybind11::array::ShapeContainer{{this->size(), N}});
    auto u = pybind11::array_t<Type>(
        pybind11::array::ShapeContainer{{this->size()}});
    auto _x = x.template mutable_unchecked<2>();
    auto _u = u.template mutable_unchecked<1>();
    size_t ix = 0;
    std::for_each(this->tree_->begin(), this->tree_->end(),
                  [&](const auto &item) {
                    for (auto jx = 0UL; jx < N; ++jx) {
                      _x(ix, jx) = detail::geometry::point::get(item.first, jx);
                    }
                    _u(ix) = item.second;
                    ++ix;
                  });
    auto system = geodetic::System(this->coordinates_.system());
    return pybind11::make_tuple(system.getstate(), x, u);
  }

  /// Create a new instance from a registered state of an instance of this
  /// object.
  static auto setstate(const pybind11::tuple &state)
      -> RTree<CoordinateType, Type, N> {
    if (state.size() != 3) {
      throw std::runtime_error("invalid state");
    }
    auto system = geodetic::System::setstate(state[0].cast<pybind11::tuple>());
    auto x = state[1].cast<pybind11::array_t<CoordinateType>>();
    auto u = state[2].cast<pybind11::array_t<Type>>();

    if (x.shape(1) != N || x.shape(0) != u.size()) {
      throw std::runtime_error("invalid state");
    }

    auto _x = x.template mutable_unchecked<2>();
    auto _u = u.template mutable_unchecked<1>();

    auto vector =
        std::vector<typename RTree<CoordinateType, Type, N>::value_t>();
    vector.reserve(u.size());

    auto point = point_t();

    for (auto ix = 0; ix < u.size(); ++ix) {
      for (auto jx = 0UL; jx < N; ++jx) {
        detail::geometry::point::set(point, _x(ix, jx), jx);
      }
      vector.emplace_back(std::make_pair(point, _u(ix)));
    }
    auto result = RTree<CoordinateType, Type, N>(system);
    static_cast<detail::geometry::RTree<CoordinateType, Type, N>>(result)
        .packing(vector);
    return result;
  }

 private:
  /// System for converting Geodetic coordinates into Cartesian coordinates.
  detail::geodetic::Coordinates coordinates_;

  /// Create the cartesian point for the given coordinates: longitude and
  /// latitude in degrees, altitude in meters, then the other coordinates
  /// defined in a Euclidean space.
  auto from_lon_lat_alt(
      const Eigen::Map<const Vector<CoordinateType>> &coordinates) const
      -> point_t {
    auto ecef = coordinates_.lla_to_ecef(
        detail::geometry::EquatorialPoint3D<CoordinateType>{
            coordinates(0), coordinates(1), coordinates(2)});
    auto result = point_t();

    boost::geometry::set<0>(result, boost::geometry::get<0>(ecef));
    boost::geometry::set<1>(result, boost::geometry::get<1>(ecef));
    boost::geometry::set<2>(result, boost::geometry::get<2>(ecef));

    for (auto ix = 3UL; ix < N; ++ix) {
      detail::geometry::point::set(result, coordinates(ix), ix);
    }
    return result;
  }

  /// Create the cartesian point for the given coordinates: longitude and
  /// latitude in degrees, then the other coordinated defined in a Euclidean
  /// space.
  auto from_lon_lat(const Eigen::Map<const Vector<CoordinateType>> &coordinates)
      const -> point_t {
    auto ecef = coordinates_.lla_to_ecef(
        detail::geometry::EquatorialPoint3D<CoordinateType>{coordinates(0),
                                                            coordinates(1), 0});
    auto result = point_t();

    boost::geometry::set<0>(result, boost::geometry::get<0>(ecef));
    boost::geometry::set<1>(result, boost::geometry::get<1>(ecef));
    boost::geometry::set<2>(result, boost::geometry::get<2>(ecef));

    for (auto ix = 2UL; ix < N - 1; ++ix) {
      detail::geometry::point::set(result, coordinates(ix), ix + 1);
    }
    return result;
  }

  /// Create the geographic point (latitude, longitude, and altitude) from the
  /// cartesian coordinates
  auto to_lla(const point_t &point) const
      -> detail::geometry::EquatorialPoint3D<CoordinateType> {
    return coordinates_.ecef_to_lla(detail::geometry::Point3D<CoordinateType>(
        boost::geometry::get<0>(point), boost::geometry::get<1>(point),
        boost::geometry::get<2>(point)));
  }

  /// Raise an exception if the size of the matrix does not conform to the
  /// number of coordinates handled by this instance.
  static auto invalid_shape() -> std::string {
    return "coordinates must be a matrix (n, " + std::to_string(N - 1) +
           ") to handle points defined by their longitudes, latitudes and "
           "other "
           "coordinates or a matrix (n," +
           std::to_string(N) +
           ") to handle points defined by their longitudes, latitudes, "
           "altitudes and other coordinates";
  }

  /// Packing coordinates
  ///
  /// @param coordinates Coordinates to be copied
  template <size_t M>
  void _packing(Converter converter,
                const pybind11::array_t<CoordinateType> &coordinates,
                const pybind11::array_t<Type> &values) {
    auto _coordinates = coordinates.template unchecked<2>();
    auto _values = values.template unchecked<1>();
    auto observations = coordinates.shape(0);
    auto vector =
        std::vector<typename RTree<CoordinateType, Type, N>::value_t>();

    vector.reserve(observations);

    for (auto ix = 0; ix < observations; ++ix) {
      vector.emplace_back(
          std::make_pair(std::invoke(converter, *this,
                                     Eigen::Map<const Vector<CoordinateType>>(
                                         &_coordinates(ix, 0), M)),
                         _values(ix)));
    }
    detail::geometry::RTree<CoordinateType, Type, N>::packing(vector);
  }

  /// Insert coordinates
  ///
  /// @param coordinates Coordinates to be copied
  template <size_t M>
  void _insert(Converter converter,
               const pybind11::array_t<CoordinateType> &coordinates,
               const pybind11::array_t<Type> &values) {
    auto _coordinates = coordinates.template unchecked<2>();
    auto _values = values.template unchecked<1>();

    for (auto ix = 0; ix < coordinates.shape(0); ++ix) {
      detail::geometry::RTree<CoordinateType, Type, N>::insert(
          std::make_pair(std::invoke(converter, *this,
                                     Eigen::Map<const Vector<CoordinateType>>(
                                         &_coordinates(ix, 0), M)),
                         _values(ix)));
    }
  }

  /// Search for the nearest K nearest neighbors of a given coordinates.
  template <size_t M>
  auto _query(Converter converter,
              const pybind11::array_t<CoordinateType> &coordinates,
              const uint32_t k, const bool within,
              const size_t num_threads) const -> pybind11::tuple {
    Requester requester =
        within ? &detail::geometry::RTree<CoordinateType, Type, N>::query_within
               : &detail::geometry::RTree<CoordinateType, Type, N>::query;

    auto _coordinates = coordinates.template unchecked<2>();
    auto size = coordinates.shape(0);

    // Allocation of result matrices.
    auto distance =
        pybind11::array_t<distance_t>(pybind11::array::ShapeContainer{
            size, static_cast<pybind11::ssize_t>(k)});
    auto value = pybind11::array_t<Type>(pybind11::array::ShapeContainer{
        size, static_cast<pybind11::ssize_t>(k)});

    auto _distance = distance.template mutable_unchecked<2>();
    auto _value = value.template mutable_unchecked<2>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              auto point = point_t();

              for (size_t ix = start; ix < end; ++ix) {
                point = std::move(
                    std::invoke(converter, *this,
                                Eigen::Map<const Vector<CoordinateType>>(
                                    &_coordinates(ix, 0), M)));

                auto nearest = std::invoke(requester, *this, point, k);
                auto jx = 0ULL;

                // Fill in the calculation result for all neighbors found
                for (; jx < nearest.size(); ++jx) {
                  _distance(ix, jx) = nearest[jx].first;
                  _value(ix, jx) = nearest[jx].second;
                }

                // The rest of the result is filled with invalid values.
                for (; jx < k; ++jx) {
                  _distance(ix, jx) = -1;
                  _value(ix, jx) = Type(-1);
                }
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(distance, value);
  }

  /// Inverse distance weighting interpolation
  template <size_t M>
  auto _inverse_distance_weighting(
      Converter converter, const pybind11::array_t<CoordinateType> &coordinates,
      const distance_t radius, const uint32_t k, const uint32_t p,
      const bool within, const size_t num_threads) const -> pybind11::tuple {
    auto _coordinates = coordinates.template unchecked<2>();
    auto size = coordinates.shape(0);

    // Allocation of result vectors.
    auto data =
        pybind11::array_t<distance_t>(pybind11::array::ShapeContainer{size});
    auto neighbors =
        pybind11::array_t<uint32_t>(pybind11::array::ShapeContainer{size});

    auto _data = data.template mutable_unchecked<1>();
    auto _neighbors = neighbors.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              auto point = point_t();

              for (size_t ix = start; ix < end; ++ix) {
                point = std::move(
                    std::invoke(converter, *this,
                                Eigen::Map<const Vector<CoordinateType>>(
                                    &_coordinates(ix, 0), M)));

                auto result = detail::geometry::RTree<CoordinateType, Type, N>::
                    inverse_distance_weighting(point, radius, k, p, within);
                _data(ix) = result.first;
                _neighbors(ix) = result.second;
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(data, neighbors);
  }

  /// Radial basis function interpolation
  template <size_t M>
  auto _rbf(Converter converter,
            const pybind11::array_t<CoordinateType> &coordinates,
            const distance_t radius, const uint32_t k,
            const RadialBasisFunction rbf, const promotion_t epsilon,
            const promotion_t smooth, const bool within,
            const size_t num_threads) const -> pybind11::tuple {
    auto _coordinates = coordinates.template unchecked<2>();
    auto size = coordinates.shape(0);

    // Construction of the interpolator.
    auto rbf_handler = detail::math::RBF<promotion_t>(epsilon, smooth, rbf);

    // Allocation of result vectors.
    auto data =
        pybind11::array_t<promotion_t>(pybind11::array::ShapeContainer{size});
    auto neighbors =
        pybind11::array_t<uint32_t>(pybind11::array::ShapeContainer{size});

    auto _data = data.template mutable_unchecked<1>();
    auto _neighbors = neighbors.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              auto point = point_t();

              for (size_t ix = start; ix < end; ++ix) {
                point = std::move(
                    std::invoke(converter, *this,
                                Eigen::Map<const Vector<CoordinateType>>(
                                    &_coordinates(ix, 0), M)));

                auto result = detail::geometry::RTree<
                    CoordinateType, Type, N>::radial_basis_function(point,
                                                                    rbf_handler,
                                                                    radius, k,
                                                                    within);
                _data(ix) = result.first;
                _neighbors(ix) = result.second;
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(data, neighbors);
  }

  /// Window function interpolation
  template <size_t M>
  auto _window_function(Converter converter,
                        const pybind11::array_t<CoordinateType> &coordinates,
                        const distance_t radius, const uint32_t k,
                        const WindowFunction wf, const distance_t arg,
                        const bool within, const size_t num_threads) const
      -> pybind11::tuple {
    auto _coordinates = coordinates.template unchecked<2>();
    auto size = coordinates.shape(0);

    // Allocation of result vectors.
    auto data =
        pybind11::array_t<distance_t>(pybind11::array::ShapeContainer{size});
    auto neighbors =
        pybind11::array_t<uint32_t>(pybind11::array::ShapeContainer{size});

    auto wf_handler = detail::math::WindowFunction<distance_t>(wf);

    auto _data = data.template mutable_unchecked<1>();
    auto _neighbors = neighbors.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              auto point = point_t();

              for (size_t ix = start; ix < end; ++ix) {
                point = std::move(
                    std::invoke(converter, *this,
                                Eigen::Map<const Vector<CoordinateType>>(
                                    &_coordinates(ix, 0), M)));

                auto result =
                    detail::geometry::RTree<CoordinateType, Type,
                                            N>::window_function(point,
                                                                wf_handler, arg,
                                                                radius, k,
                                                                within);
                _data(ix) = result.first;
                _neighbors(ix) = result.second;
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(data, neighbors);
  }
};

}  // namespace pyinterp


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

/* 1000d.f -- translated by f2c (version 20050501).
You must link the resulting object file with libf2c:
on Microsoft Windows system, link with libf2c.lib;
on Linux or Unix systems, link with .../path/to/libf2c.a -lm
or, if you install libf2c.a in a standard place, with -lf2c -lm
-- in that order, at the end of the command line, as in
cc *.o -lf2c -lm
Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,

http://www.netlib.org/f2c/libf2c.zip
*/
#include <iostream>
#include <iomanip>
#include <cmath>

#if defined(TEST_GMPXX)
#include <gmpxx.h>
typedef mpf_class real_type;
#elif defined(TEST_MPFRXX)
#include <gmpfrxx.h>
typedef mpfr_class real_type;
#elif defined(TEST_CPP_DEC_FLOAT)
#include <boost/multiprecision/cpp_dec_float.hpp>
typedef boost::multiprecision::cpp_dec_float_50 real_type;
#elif defined(TEST_MPFR_50)
#include <boost/multiprecision/mpfr.hpp>
typedef boost::multiprecision::mpfr_float_50 real_type;
#elif defined(TEST_MPF_50)
#include <boost/multiprecision/gmp.hpp>
typedef boost::multiprecision::mpf_float_50 real_type;
#elif defined(NATIVE_FLOAT128)
#include <boost/multiprecision/float128.hpp>
typedef __float128 real_type;

std::ostream& operator<<(std::ostream& os, const __float128& f)
{
   return os << boost::multiprecision::float128(f);
}

#include <boost/type_traits/has_left_shift.hpp>

namespace boost{

template<>
struct has_left_shift<std::basic_ostream<char>, __float128> : public mpl::true_ {};

template<>
double lexical_cast<double, __float128>(const __float128& f)
{ return f; }

}

#elif defined(TEST_FLOAT128)
#include <boost/multiprecision/float128.hpp>
typedef boost::multiprecision::float128 real_type;
#else
typedef double real_type;
#endif

#include <boost/lexical_cast.hpp>

#ifndef CAST_TO_RT
#  define CAST_TO_RT(x) x
#endif

extern "C" {
#include "f2c.h"
   integer s_wsfe(cilist *), e_wsfe(void), do_fio(integer *, char *, ftnlen),
      s_wsle(cilist *), do_lio(integer *, integer *, char *, ftnlen),
      e_wsle(void);
   /* Subroutine */ int s_stop(char *, ftnlen);

#undef abs
#undef dabs
#define dabs abs
#undef dmin
#undef dmax
#define dmin min
#define dmax max

}
#include <time.h>

using std::min;
using std::max;

/* Table of constant values */

static integer c__0 = 0;
static real_type c_b7 = CAST_TO_RT(1);
static integer c__1 = 1;
static integer c__9 = 9;

inline double second_(void)
{
   return ((double)(clock())) / CLOCKS_PER_SEC;
}

int dgefa_(real_type *, integer *, integer *, integer *, integer *), dgesl_(real_type *, integer *, integer *, integer *, real_type *, integer *);
int dmxpy_(integer *, real_type *, integer *, integer *, real_type *, real_type *);
int matgen_(real_type *, integer *, integer *, real_type *, real_type *);
real_type epslon_(real_type *);
real_type ran_(integer *);
int dscal_(integer *, real_type *, real_type *, integer *);
int daxpy_(integer *, real_type *, real_type *, integer *, real_type *, integer *);
integer idamax_(integer *, real_type *, integer *);
real_type ddot_(integer *, real_type *, integer *, real_type *, integer *);
int daxpy_(integer *, real_type *, real_type *, integer *, real_type *, integer *);
int dmxpy_(integer *, real_type *, integer *, integer *, real_type *, real_type *);

extern "C" int MAIN__()
{
#ifdef TEST_MPF_50
   std::cout << "Testing number<mpf_float<50> >" << std::endl;
#elif defined(TEST_MPFR_50)
   std::cout << "Testing number<mpf_float<50> >" << std::endl;
#elif defined(TEST_GMPXX)
   std::cout << "Testing mpf_class at 50 decimal degits" << std::endl;
   mpf_set_default_prec(((50 + 1) * 1000L) / 301L);
#elif defined(TEST_MPFRXX)
   std::cout << "Testing mpfr_class at 50 decimal degits" << std::endl;
   mpfr_set_default_prec(((50 + 1) * 1000L) / 301L);
#elif defined(TEST_CPP_DEC_FLOAT)
   std::cout << "Testing number<cpp_dec_float<50> >" << std::endl;
#elif defined(NATIVE_FLOAT128)
   std::cout << "Testing __float128" << std::endl;
#elif defined(TEST_FLOAT128)
   std::cout << "Testing number<float128_backend, et_off>" << std::endl;
#else
   std::cout << "Testing double" << std::endl;
#endif

   /* Format strings */
   static char fmt_1[] = "(\002 Please send the results of this run to:\002"
      "//\002 Jack J. Dongarra\002/\002 Computer Science Department\002/"
      "\002 University of Tennessee\002/\002 Knoxville, Tennessee 37996"
      "-1300\002//\002 Fax: 615-974-8296\002//\002 Internet: dongarra@c"
      "s.utk.edu\002/)";
   static char fmt_40[] = "(\002     norm. resid      resid           mac"
      "hep\002,\002         x(1)          x(n)\002)";
   static char fmt_50[] = "(1p5e16.8)";
   static char fmt_60[] = "(//\002    times are reported for matrices of or"
      "der \002,i5)";
   static char fmt_70[] = "(6x,\002factor\002,5x,\002solve\002,6x,\002tota"
      "l\002,5x,\002mflops\002,7x,\002unit\002,6x,\002ratio\002)";
   static char fmt_80[] = "(\002 times for array with leading dimension o"
      "f\002,i4)";
   static char fmt_110[] = "(6(1pe11.3))";

   /* System generated locals */
   integer i__1;
   real_type d__1, d__2, d__3;

   /* Builtin functions */

   /* Local variables */
   static real_type a[1001000] /* was [1001][1000] */, b[1000];
   static integer i__, n;
   static real_type x[1000];
   static double t1;
   static integer lda;
   static double ops;
   static real_type eps;
   static integer info;
   static double time[6], cray, total;
   static integer ipvt[1000];
   static real_type resid, norma;
   static real_type normx;
   static real_type residn;

   /* Fortran I/O blocks */
   static cilist io___4 = { 0, 6, 0, fmt_1, 0 };
   static cilist io___20 = { 0, 6, 0, fmt_40, 0 };
   static cilist io___21 = { 0, 6, 0, fmt_50, 0 };
   static cilist io___22 = { 0, 6, 0, fmt_60, 0 };
   static cilist io___23 = { 0, 6, 0, fmt_70, 0 };
   static cilist io___24 = { 0, 6, 0, fmt_80, 0 };
   static cilist io___25 = { 0, 6, 0, fmt_110, 0 };
   static cilist io___26 = { 0, 6, 0, 0, 0 };

   lda = 1001;

   /*     this program was updated on 10/12/92 to correct a */
   /*     problem with the random number generator. The previous */
   /*     random number generator had a short period and produced */
   /*     singular matrices occasionally. */

   n = 1000;
   cray = .056f;
   s_wsfe(&io___4);
   e_wsfe();
   /* Computing 3rd power */
   d__1 = (real_type) n;
   /* Computing 2nd power */
   d__2 = (real_type) n;
   ops = boost::lexical_cast<double>(real_type(d__1 * (d__1 * d__1) * 2. / 3. + d__2 * d__2 * 2.));

   matgen_(a, &lda, &n, b, &norma);

   /* ****************************************************************** */
   /* ****************************************************************** */
   /*        you should replace the call to dgefa and dgesl */
   /*        by calls to your linear equation solver. */
   /* ****************************************************************** */
   /* ****************************************************************** */

   t1 = second_();
   dgefa_(a, &lda, &n, ipvt, &info);
   time[0] = second_() - t1;
   t1 = second_();
   dgesl_(a, &lda, &n, ipvt, b, &c__0);
   time[1] = second_() - t1;
   total = time[0] + time[1];
   /* ****************************************************************** */
   /* ****************************************************************** */

   /*     compute a residual to verify results. */

   i__1 = n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      x[i__ - 1] = b[i__ - 1];
      /* L10: */
   }
   matgen_(a, &lda, &n, b, &norma);
   i__1 = n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      b[i__ - 1] = -b[i__ - 1];
      /* L20: */
   }
   dmxpy_(&n, b, &n, &lda, x, a);
   resid = CAST_TO_RT(0);
   normx = CAST_TO_RT(0);
   i__1 = n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      /* Computing MAX */
      d__2 = resid, d__3 = (d__1 = b[i__ - 1], abs(d__1));
      resid = (max)(d__2,d__3);
      /* Computing MAX */
      d__2 = normx, d__3 = (d__1 = x[i__ - 1], abs(d__1));
      normx = (max)(d__2,d__3);
      /* L30: */
   }
   eps = epslon_(&c_b7);
   residn = resid / (n * norma * normx * eps);
   s_wsfe(&io___20);
   e_wsfe();
   s_wsfe(&io___21);
   /*
   do_fio(&c__1, (char *)&residn, (ftnlen)sizeof(real_type));
   do_fio(&c__1, (char *)&resid, (ftnlen)sizeof(real_type));
   do_fio(&c__1, (char *)&eps, (ftnlen)sizeof(real_type));
   do_fio(&c__1, (char *)&x[0], (ftnlen)sizeof(real_type));
   do_fio(&c__1, (char *)&x[n - 1], (ftnlen)sizeof(real_type));
   */
   std::cout << std::setw(12) << std::setprecision(5) << residn << " " << resid << " " << eps << " " << x[0] << " " << x[n-1] << std::endl;
   e_wsfe();

   s_wsfe(&io___22);
   do_fio(&c__1, (char *)&n, (ftnlen)sizeof(integer));
   e_wsfe();
   s_wsfe(&io___23);
   e_wsfe();

   time[2] = total;
   time[3] = ops / (total * 1e6);
   time[4] = 2. / time[3];
   time[5] = total / cray;
   s_wsfe(&io___24);
   do_fio(&c__1, (char *)&lda, (ftnlen)sizeof(integer));
   e_wsfe();
   s_wsfe(&io___25);
   for (i__ = 1; i__ <= 6; ++i__) {
      // do_fio(&c__1, (char *)&time[i__ - 1], (ftnlen)sizeof(real_type));
      std::cout << std::setw(12) << std::setprecision(5) << time[i__ - 1];
   }
   e_wsfe();
   s_wsle(&io___26);
   do_lio(&c__9, &c__1, " end of tests -- this version dated 10/12/92", (
      ftnlen)44);
   e_wsle();

   s_stop("", (ftnlen)0);
   return 0;
} /* MAIN__ */

/* Subroutine */ int matgen_(real_type *a, integer *lda, integer *n,
   real_type *b, real_type *norma)
{
   /* System generated locals */
   integer a_dim1, a_offset, i__1, i__2;
   real_type d__1, d__2;

   /* Local variables */
   static integer i__, j;
   static integer init[4];

   /* Parameter adjustments */
   a_dim1 = *lda;
   a_offset = 1 + a_dim1;
   a -= a_offset;
   --b;

   /* Function Body */
   init[0] = 1;
   init[1] = 2;
   init[2] = 3;
   init[3] = 1325;
   *norma = CAST_TO_RT(0);
   i__1 = *n;
   for (j = 1; j <= i__1; ++j) {
      i__2 = *n;
      for (i__ = 1; i__ <= i__2; ++i__) {
         a[i__ + j * a_dim1] = ran_(init) - .5f;
         /* Computing MAX */
         d__2 = (d__1 = a[i__ + j * a_dim1], abs(d__1));
         *norma = (max)(d__2,*norma);
         /* L20: */
      }
      /* L30: */
   }
   i__1 = *n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      b[i__] = CAST_TO_RT(0);
      /* L35: */
   }
   i__1 = *n;
   for (j = 1; j <= i__1; ++j) {
      i__2 = *n;
      for (i__ = 1; i__ <= i__2; ++i__) {
         b[i__] += a[i__ + j * a_dim1];
         /* L40: */
      }
      /* L50: */
   }
   return 0;
} /* matgen_ */

/* Subroutine */ int dgefa_(real_type *a, integer *lda, integer *n, integer *
   ipvt, integer *info)
{
   /* System generated locals */
   integer a_dim1, a_offset, i__1, i__2, i__3;

   /* Local variables */
   static integer j, k, l;
   static real_type t;
   static integer kp1, nm1;

   /*     dgefa factors a double precision matrix by gaussian elimination. */

   /*     dgefa is usually called by dgeco, but it can be called */
   /*     directly with a saving in time if  rcond  is not needed. */
   /*     (time for dgeco) = (1 + 9/n)*(time for dgefa) . */

   /*     on entry */

   /*        a       double precision(lda, n) */
   /*                the matrix to be factored. */

   /*        lda     integer */
   /*                the leading dimension of the array  a . */

   /*        n       integer */
   /*                the order of the matrix  a . */

   /*     on return */

   /*        a       an upper triangular matrix and the multipliers */
   /*                which were used to obtain it. */
   /*                the factorization can be written  a = l*u  where */
   /*                l  is a product of permutation and unit lower */
   /*                triangular matrices and  u  is upper triangular. */

   /*        ipvt    integer(n) */
   /*                an integer vector of pivot indices. */

   /*        info    integer */
   /*                = 0  normal value. */
   /*                = k  if  u(k,k) .eq. 0.0 .  this is not an error */
   /*                     condition for this subroutine, but it does */
   /*                     indicate that dgesl or dgedi will divide by zero */
   /*                     if called.  use  rcond  in dgeco for a reliable */
   /*                     indication of singularity. */

   /*     linpack. this version dated 08/14/78 . */
   /*     cleve moler, university of new mexico, argonne national lab. */

   /*     subroutines and functions */

   /*     blas daxpy,dscal,idamax */

   /*     internal variables */

   /*     gaussian elimination with partial pivoting */

   /* Parameter adjustments */
   a_dim1 = *lda;
   a_offset = 1 + a_dim1;
   a -= a_offset;
   --ipvt;

   /* Function Body */
   *info = 0;
   nm1 = *n - 1;
   if (nm1 < 1) {
      goto L70;
   }
   i__1 = nm1;
   for (k = 1; k <= i__1; ++k) {
      kp1 = k + 1;

      /*        find l = pivot index */

      i__2 = *n - k + 1;
      l = idamax_(&i__2, &a[k + k * a_dim1], &c__1) + k - 1;
      ipvt[k] = l;

      /*        zero pivot implies this column already triangularized */

      if (a[l + k * a_dim1] == 0.) {
         goto L40;
      }

      /*           interchange if necessary */

      if (l == k) {
         goto L10;
      }
      t = a[l + k * a_dim1];
      a[l + k * a_dim1] = a[k + k * a_dim1];
      a[k + k * a_dim1] = t;
L10:

      /*           compute multipliers */

      t = -1. / a[k + k * a_dim1];
      i__2 = *n - k;
      dscal_(&i__2, &t, &a[k + 1 + k * a_dim1], &c__1);

      /*           row elimination with column indexing */

      i__2 = *n;
      for (j = kp1; j <= i__2; ++j) {
         t = a[l + j * a_dim1];
         if (l == k) {
            goto L20;
         }
         a[l + j * a_dim1] = a[k + j * a_dim1];
         a[k + j * a_dim1] = t;
L20:
         i__3 = *n - k;
         daxpy_(&i__3, &t, &a[k + 1 + k * a_dim1], &c__1, &a[k + 1 + j *
            a_dim1], &c__1);
         /* L30: */
      }
      goto L50;
L40:
      *info = k;
L50:
      /* L60: */
      ;
   }
L70:
   ipvt[*n] = *n;
   if (a[*n + *n * a_dim1] == 0.) {
      *info = *n;
   }
   return 0;
} /* dgefa_ */

/* Subroutine */ int dgesl_(real_type *a, integer *lda, integer *n, integer *
   ipvt, real_type *b, integer *job)
{
   /* System generated locals */
   integer a_dim1, a_offset, i__1, i__2;

   /* Local variables */
   static integer k, l;
   static real_type t;
   static integer kb, nm1;

   /*     dgesl solves the double precision system */
   /*     a * x = b  or  trans(a) * x = b */
   /*     using the factors computed by dgeco or dgefa. */

   /*     on entry */

   /*        a       double precision(lda, n) */
   /*                the output from dgeco or dgefa. */

   /*        lda     integer */
   /*                the leading dimension of the array  a . */

   /*        n       integer */
   /*                the order of the matrix  a . */

   /*        ipvt    integer(n) */
   /*                the pivot vector from dgeco or dgefa. */

   /*        b       double precision(n) */
   /*                the right hand side vector. */

   /*        job     integer */
   /*                = 0         to solve  a*x = b , */
   /*                = nonzero   to solve  trans(a)*x = b  where */
   /*                            trans(a)  is the transpose. */

   /*     on return */

   /*        b       the solution vector  x . */

   /*     error condition */

   /*        a division by zero will occur if the input factor contains a */
   /*        zero on the diagonal.  technically this indicates singularity */
   /*        but it is often caused by improper arguments or improper */
   /*        setting of lda .  it will not occur if the subroutines are */
   /*        called correctly and if dgeco has set rcond .gt. 0.0 */
   /*        or dgefa has set info .eq. 0 . */

   /*     to compute  inverse(a) * c  where  c  is a matrix */
   /*     with  p  columns */
   /*           call dgeco(a,lda,n,ipvt,rcond,z) */
   /*           if (rcond is too small) go to ... */
   /*           do 10 j = 1, p */
   /*              call dgesl(a,lda,n,ipvt,c(1,j),0) */
   /*        10 continue */

   /*     linpack. this version dated 08/14/78 . */
   /*     cleve moler, university of new mexico, argonne national lab. */

   /*     subroutines and functions */

   /*     blas daxpy,ddot */

   /*     internal variables */

   /* Parameter adjustments */
   a_dim1 = *lda;
   a_offset = 1 + a_dim1;
   a -= a_offset;
   --ipvt;
   --b;

   /* Function Body */
   nm1 = *n - 1;
   if (*job != 0) {
      goto L50;
   }

   /*        job = 0 , solve  a * x = b */
   /*        first solve  l*y = b */

   if (nm1 < 1) {
      goto L30;
   }
   i__1 = nm1;
   for (k = 1; k <= i__1; ++k) {
      l = ipvt[k];
      t = b[l];
      if (l == k) {
         goto L10;
      }
      b[l] = b[k];
      b[k] = t;
L10:
      i__2 = *n - k;
      daxpy_(&i__2, &t, &a[k + 1 + k * a_dim1], &c__1, &b[k + 1], &c__1);
      /* L20: */
   }
L30:

   /*        now solve  u*x = y */

   i__1 = *n;
   for (kb = 1; kb <= i__1; ++kb) {
      k = *n + 1 - kb;
      b[k] /= a[k + k * a_dim1];
      t = -b[k];
      i__2 = k - 1;
      daxpy_(&i__2, &t, &a[k * a_dim1 + 1], &c__1, &b[1], &c__1);
      /* L40: */
   }
   goto L100;
L50:

   /*        job = nonzero, solve  trans(a) * x = b */
   /*        first solve  trans(u)*y = b */

   i__1 = *n;
   for (k = 1; k <= i__1; ++k) {
      i__2 = k - 1;
      t = ddot_(&i__2, &a[k * a_dim1 + 1], &c__1, &b[1], &c__1);
      b[k] = (b[k] - t) / a[k + k * a_dim1];
      /* L60: */
   }

   /*        now solve trans(l)*x = y */

   if (nm1 < 1) {
      goto L90;
   }
   i__1 = nm1;
   for (kb = 1; kb <= i__1; ++kb) {
      k = *n - kb;
      i__2 = *n - k;
      b[k] += ddot_(&i__2, &a[k + 1 + k * a_dim1], &c__1, &b[k + 1], &c__1);
      l = ipvt[k];
      if (l == k) {
         goto L70;
      }
      t = b[l];
      b[l] = b[k];
      b[k] = t;
L70:
      /* L80: */
      ;
   }
L90:
L100:
   return 0;
} /* dgesl_ */

/* Subroutine */ int daxpy_(integer *n, real_type *da, real_type *dx,
   integer *incx, real_type *dy, integer *incy)
{
   /* System generated locals */
   integer i__1;

   /* Local variables */
   static integer i__, m, ix, iy, mp1;

   /*     constant times a vector plus a vector. */
   /*     uses unrolled loops for increments equal to one. */
   /*     jack dongarra, linpack, 3/11/78. */

   /* Parameter adjustments */
   --dy;
   --dx;

   /* Function Body */
   if (*n <= 0) {
      return 0;
   }
   if (*da == 0.) {
      return 0;
   }
   if (*incx == 1 && *incy == 1) {
      goto L20;
   }

   /*        code for unequal increments or equal increments */
   /*          not equal to 1 */

   ix = 1;
   iy = 1;
   if (*incx < 0) {
      ix = (-(*n) + 1) * *incx + 1;
   }
   if (*incy < 0) {
      iy = (-(*n) + 1) * *incy + 1;
   }
   i__1 = *n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      dy[iy] += *da * dx[ix];
      ix += *incx;
      iy += *incy;
      /* L10: */
   }
   return 0;

   /*        code for both increments equal to 1 */

   /*        clean-up loop */

L20:
   m = *n % 4;
   if (m == 0) {
      goto L40;
   }
   i__1 = m;
   for (i__ = 1; i__ <= i__1; ++i__) {
      dy[i__] += *da * dx[i__];
      /* L30: */
   }
   if (*n < 4) {
      return 0;
   }
L40:
   mp1 = m + 1;
   i__1 = *n;
   for (i__ = mp1; i__ <= i__1; i__ += 4) {
      dy[i__] += *da * dx[i__];
      dy[i__ + 1] += *da * dx[i__ + 1];
      dy[i__ + 2] += *da * dx[i__ + 2];
      dy[i__ + 3] += *da * dx[i__ + 3];
      /* L50: */
   }
   return 0;
} /* daxpy_ */

real_type ddot_(integer *n, real_type *dx, integer *incx, real_type *dy,
   integer *incy)
{
   /* System generated locals */
   integer i__1;
   real_type ret_val;

   /* Local variables */
   static integer i__, m, ix, iy, mp1;
   static real_type dtemp;

   /*     forms the dot product of two vectors. */
   /*     uses unrolled loops for increments equal to one. */
   /*     jack dongarra, linpack, 3/11/78. */

   /* Parameter adjustments */
   --dy;
   --dx;

   /* Function Body */
   ret_val = CAST_TO_RT(0);
   dtemp = CAST_TO_RT(0);
   if (*n <= 0) {
      return ret_val;
   }
   if (*incx == 1 && *incy == 1) {
      goto L20;
   }

   /*        code for unequal increments or equal increments */
   /*          not equal to 1 */

   ix = 1;
   iy = 1;
   if (*incx < 0) {
      ix = (-(*n) + 1) * *incx + 1;
   }
   if (*incy < 0) {
      iy = (-(*n) + 1) * *incy + 1;
   }
   i__1 = *n;
   for (i__ = 1; i__ <= i__1; ++i__) {
      dtemp += dx[ix] * dy[iy];
      ix += *incx;
      iy += *incy;
      /* L10: */
   }
   ret_val = dtemp;
   return ret_val;

   /*        code for both increments equal to 1 */

   /*        clean-up loop */

L20:
   m = *n % 5;
   if (m == 0) {
      goto L40;
   }
   i__1 = m;
   for (i__ = 1; i__ <= i__1; ++i__) {
      dtemp += dx[i__] * dy[i__];
      /* L30: */
   }
   if (*n < 5) {
      goto L60;
   }
L40:
   mp1 = m + 1;
   i__1 = *n;
   for (i__ = mp1; i__ <= i__1; i__ += 5) {
      dtemp = dtemp + dx[i__] * dy[i__] + dx[i__ + 1] * dy[i__ + 1] + dx[
         i__ + 2] * dy[i__ + 2] + dx[i__ + 3] * dy[i__ + 3] + dx[i__ +
            4] * dy[i__ + 4];
         /* L50: */
   }
L60:
   ret_val = dtemp;
   return ret_val;
} /* ddot_ */

/* Subroutine */ int dscal_(integer *n, real_type *da, real_type *dx,
   integer *incx)
{
   /* System generated locals */
   integer i__1, i__2;

   /* Local variables */
   static integer i__, m, mp1, nincx;

   /*     scales a vector by a constant. */
   /*     uses unrolled loops for increment equal to one. */
   /*     jack dongarra, linpack, 3/11/78. */

   /* Parameter adjustments */
   --dx;

   /* Function Body */
   if (*n <= 0) {
      return 0;
   }
   if (*incx == 1) {
      goto L20;
   }

   /*        code for increment not equal to 1 */

   nincx = *n * *incx;
   i__1 = nincx;
   i__2 = *incx;
   for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
      dx[i__] = *da * dx[i__];
      /* L10: */
   }
   return 0;

   /*        code for increment equal to 1 */

   /*        clean-up loop */

L20:
   m = *n % 5;
   if (m == 0) {
      goto L40;
   }
   i__2 = m;
   for (i__ = 1; i__ <= i__2; ++i__) {
      dx[i__] = *da * dx[i__];
      /* L30: */
   }
   if (*n < 5) {
      return 0;
   }
L40:
   mp1 = m + 1;
   i__2 = *n;
   for (i__ = mp1; i__ <= i__2; i__ += 5) {
      dx[i__] = *da * dx[i__];
      dx[i__ + 1] = *da * dx[i__ + 1];
      dx[i__ + 2] = *da * dx[i__ + 2];
      dx[i__ + 3] = *da * dx[i__ + 3];
      dx[i__ + 4] = *da * dx[i__ + 4];
      /* L50: */
   }
   return 0;
} /* dscal_ */

integer idamax_(integer *n, real_type *dx, integer *incx)
{
   /* System generated locals */
   integer ret_val, i__1;
   real_type d__1;

   /* Local variables */
   static integer i__, ix;
   static real_type dmax__;

   /*     finds the index of element having max. dabsolute value. */
   /*     jack dongarra, linpack, 3/11/78. */

   /* Parameter adjustments */
   --dx;

   /* Function Body */
   ret_val = 0;
   if (*n < 1) {
      return ret_val;
   }
   ret_val = 1;
   if (*n == 1) {
      return ret_val;
   }
   if (*incx == 1) {
      goto L20;
   }

   /*        code for increment not equal to 1 */

   ix = 1;
   dmax__ = abs(dx[1]);
   ix += *incx;
   i__1 = *n;
   for (i__ = 2; i__ <= i__1; ++i__) {
      if ((d__1 = dx[ix], abs(d__1)) <= dmax__) {
         goto L5;
      }
      ret_val = i__;
      dmax__ = (d__1 = dx[ix], abs(d__1));
L5:
      ix += *incx;
      /* L10: */
   }
   return ret_val;

   /*        code for increment equal to 1 */

L20:
   dmax__ = abs(dx[1]);
   i__1 = *n;
   for (i__ = 2; i__ <= i__1; ++i__) {
      if ((d__1 = dx[i__], abs(d__1)) <= dmax__) {
         goto L30;
      }
      ret_val = i__;
      dmax__ = (d__1 = dx[i__], abs(d__1));
L30:
      ;
   }
   return ret_val;
} /* idamax_ */

real_type epslon_(real_type *x)
{
#if defined(TEST_MPF_100) || defined(TEST_MPFR_100) || defined(TEST_GMPXX) || defined(TEST_MPFRXX)
   return std::ldexp(1.0, 1 - ((100 + 1) * 1000L) / 301L);
#elif defined(TEST_CPP_DEC_FLOAT_BN)
   return std::pow(10.0, 1-std::numeric_limits<efx::cpp_dec_float_50>::digits10);
#elif defined(NATIVE_FLOAT128)
   return FLT128_EPSILON;
#else
   return CAST_TO_RT(std::numeric_limits<real_type>::epsilon());
#endif
} /* epslon_ */

/* Subroutine */ int mm_(real_type *a, integer *lda, integer *n1, integer *
   n3, real_type *b, integer *ldb, integer *n2, real_type *c__,
   integer *ldc)
{
   /* System generated locals */
   integer a_dim1, a_offset, b_dim1, b_offset, c_dim1, c_offset, i__1, i__2;

   /* Local variables */
   static integer i__, j;

   /*   purpose: */
   /*     multiply matrix b times matrix c and store the result in matrix a. */

   /*   parameters: */

   /*     a double precision(lda,n3), matrix of n1 rows and n3 columns */

   /*     lda integer, leading dimension of array a */

   /*     n1 integer, number of rows in matrices a and b */

   /*     n3 integer, number of columns in matrices a and c */

   /*     b double precision(ldb,n2), matrix of n1 rows and n2 columns */

   /*     ldb integer, leading dimension of array b */

   /*     n2 integer, number of columns in matrix b, and number of rows in */
   /*         matrix c */

   /*     c double precision(ldc,n3), matrix of n2 rows and n3 columns */

   /*     ldc integer, leading dimension of array c */

   /* ---------------------------------------------------------------------- */

   /* Parameter adjustments */
   a_dim1 = *lda;
   a_offset = 1 + a_dim1;
   a -= a_offset;
   b_dim1 = *ldb;
   b_offset = 1 + b_dim1;
   b -= b_offset;
   c_dim1 = *ldc;
   c_offset = 1 + c_dim1;
   c__ -= c_offset;

   /* Function Body */
   i__1 = *n3;
   for (j = 1; j <= i__1; ++j) {
      i__2 = *n1;
      for (i__ = 1; i__ <= i__2; ++i__) {
         a[i__ + j * a_dim1] = CAST_TO_RT(0);
         /* L10: */
      }
      dmxpy_(n2, &a[j * a_dim1 + 1], n1, ldb, &c__[j * c_dim1 + 1], &b[
         b_offset]);
         /* L20: */
   }

   return 0;
} /* mm_ */

/* Subroutine */ int dmxpy_(integer *n1, real_type *y, integer *n2, integer *
   ldm, real_type *x, real_type *m)
{
   /* System generated locals */
   integer m_dim1, m_offset, i__1, i__2;

   /* Local variables */
   static integer i__, j, jmin;

   /*   purpose: */
   /*     multiply matrix m times vector x and add the result to vector y. */

   /*   parameters: */

   /*     n1 integer, number of elements in vector y, and number of rows in */
   /*         matrix m */

   /*     y double precision(n1), vector of length n1 to which is added */
   /*         the product m*x */

   /*     n2 integer, number of elements in vector x, and number of columns */
   /*         in matrix m */

   /*     ldm integer, leading dimension of array m */

   /*     x double precision(n2), vector of length n2 */

   /*     m double precision(ldm,n2), matrix of n1 rows and n2 columns */

   /* ---------------------------------------------------------------------- */

   /*   cleanup odd vector */

   /* Parameter adjustments */
   --y;
   m_dim1 = *ldm;
   m_offset = 1 + m_dim1;
   m -= m_offset;
   --x;

   /* Function Body */
   j = *n2 % 2;
   if (j >= 1) {
      i__1 = *n1;
      for (i__ = 1; i__ <= i__1; ++i__) {
         y[i__] += x[j] * m[i__ + j * m_dim1];
         /* L10: */
      }
   }

   /*   cleanup odd group of two vectors */

   j = *n2 % 4;
   if (j >= 2) {
      i__1 = *n1;
      for (i__ = 1; i__ <= i__1; ++i__) {
         y[i__] = y[i__] + x[j - 1] * m[i__ + (j - 1) * m_dim1] + x[j] * m[
            i__ + j * m_dim1];
            /* L20: */
      }
   }

   /*   cleanup odd group of four vectors */

   j = *n2 % 8;
   if (j >= 4) {
      i__1 = *n1;
      for (i__ = 1; i__ <= i__1; ++i__) {
         y[i__] = y[i__] + x[j - 3] * m[i__ + (j - 3) * m_dim1] + x[j - 2]
         * m[i__ + (j - 2) * m_dim1] + x[j - 1] * m[i__ + (j - 1) *
            m_dim1] + x[j] * m[i__ + j * m_dim1];
         /* L30: */
      }
   }

   /*   cleanup odd group of eight vectors */

   j = *n2 % 16;
   if (j >= 8) {
      i__1 = *n1;
      for (i__ = 1; i__ <= i__1; ++i__) {
         y[i__] = y[i__] + x[j - 7] * m[i__ + (j - 7) * m_dim1] + x[j - 6]
         * m[i__ + (j - 6) * m_dim1] + x[j - 5] * m[i__ + (j - 5) *
            m_dim1] + x[j - 4] * m[i__ + (j - 4) * m_dim1] + x[j - 3]
         * m[i__ + (j - 3) * m_dim1] + x[j - 2] * m[i__ + (j - 2)
            * m_dim1] + x[j - 1] * m[i__ + (j - 1) * m_dim1] + x[j] *
            m[i__ + j * m_dim1];
         /* L40: */
      }
   }

   /*   main loop - groups of sixteen vectors */

   jmin = j + 16;
   i__1 = *n2;
   for (j = jmin; j <= i__1; j += 16) {
      i__2 = *n1;
      for (i__ = 1; i__ <= i__2; ++i__) {
         y[i__] = y[i__] + x[j - 15] * m[i__ + (j - 15) * m_dim1] + x[j -
            14] * m[i__ + (j - 14) * m_dim1] + x[j - 13] * m[i__ + (j
            - 13) * m_dim1] + x[j - 12] * m[i__ + (j - 12) * m_dim1]
         + x[j - 11] * m[i__ + (j - 11) * m_dim1] + x[j - 10] * m[
            i__ + (j - 10) * m_dim1] + x[j - 9] * m[i__ + (j - 9) *
               m_dim1] + x[j - 8] * m[i__ + (j - 8) * m_dim1] + x[j - 7]
            * m[i__ + (j - 7) * m_dim1] + x[j - 6] * m[i__ + (j - 6) *
               m_dim1] + x[j - 5] * m[i__ + (j - 5) * m_dim1] + x[j - 4]
            * m[i__ + (j - 4) * m_dim1] + x[j - 3] * m[i__ + (j - 3)
               * m_dim1] + x[j - 2] * m[i__ + (j - 2) * m_dim1] + x[j -
               1] * m[i__ + (j - 1) * m_dim1] + x[j] * m[i__ + j *
               m_dim1];
            /* L50: */
      }
      /* L60: */
   }
   return 0;
} /* dmxpy_ */

real_type ran_(integer *iseed)
{
   /* System generated locals */
   real_type ret_val;

   /* Local variables */
   static integer it1, it2, it3, it4;

   /*     modified from the LAPACK auxiliary routine 10/12/92 JD */
   /*  -- LAPACK auxiliary routine (version 1.0) -- */
   /*     Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd., */
   /*     Courant Institute, Argonne National Lab, and Rice University */
   /*     February 29, 1992 */

   /*     .. Array Arguments .. */
   /*     .. */

   /*  Purpose */
   /*  ======= */

   /*  DLARAN returns a random double number from a uniform (0,1) */
   /*  distribution. */

   /*  Arguments */
   /*  ========= */

   /*  ISEED   (input/output) INTEGER array, dimension (4) */
   /*          On entry, the seed of the random number generator; the array */
   /*          elements must be between 0 and 4095, and ISEED(4) must be */
   /*          odd. */
   /*          On exit, the seed is updated. */

   /*  Further Details */
   /*  =============== */

   /*  This routine uses a multiplicative congruential method with modulus */
   /*  2**48 and multiplier 33952834046453 (see G.S.Fishman, */
   /*  'Multiplicative congruential random number generators with modulus */
   /*  2**b: an exhaustive analysis for b = 32 and a partial analysis for */
   /*  b = 48', Math. Comp. 189, pp 331-344, 1990). */

   /*  48-bit integers are stored in 4 integer array elements with 12 bits */
   /*  per element. Hence the routine is portable across machines with */
   /*  integers of 32 bits or more. */

   /*     .. Parameters .. */
   /*     .. */
   /*     .. Local Scalars .. */
   /*     .. */
   /*     .. Intrinsic Functions .. */
   /*     .. */
   /*     .. Executable Statements .. */

   /*     multiply the seed by the multiplier modulo 2**48 */

   /* Parameter adjustments */
   --iseed;

   /* Function Body */
   it4 = iseed[4] * 2549;
   it3 = it4 / 4096;
   it4 -= it3 << 12;
   it3 = it3 + iseed[3] * 2549 + iseed[4] * 2508;
   it2 = it3 / 4096;
   it3 -= it2 << 12;
   it2 = it2 + iseed[2] * 2549 + iseed[3] * 2508 + iseed[4] * 322;
   it1 = it2 / 4096;
   it2 -= it1 << 12;
   it1 = it1 + iseed[1] * 2549 + iseed[2] * 2508 + iseed[3] * 322 + iseed[4]
   * 494;
   it1 %= 4096;

   /*     return updated seed */

   iseed[1] = it1;
   iseed[2] = it2;
   iseed[3] = it3;
   iseed[4] = it4;

   /*     convert 48-bit integer to a double number in the interval (0,1) */

   ret_val = ((real_type) it1 + ((real_type) it2 + ((real_type) it3 + (
      real_type) it4 * 2.44140625e-4) * 2.44140625e-4) * 2.44140625e-4)
      * 2.44140625e-4;
   return ret_val;

   /*     End of RAN */

} /* ran_ */

/*

Double results:
~~~~~~~~~~~~~~

norm. resid      resid           machep         x(1)          x(n)
6.4915           7.207e-013      2.2204e-016    1             1

times are reported for matrices of order  1000
factor     solve      total     mflops       unit      ratio
times for array with leading dimension of1001
1.443     0.003      1.446     462.43       0.004325  25.821

mpf_class results:
~~~~~~~~~~~~~~~~~~

norm. resid      resid           machep         x(1)          x(n)
3.6575e-05       5.2257e-103     2.8575e-101    1             1

times are reported for matrices of order  1000
factor     solve      total     mflops       unit      ratio
times for array with leading dimension of1001
266.45     0.798      267.24    2.5021       0.79933   4772.2

number<gmp_float<100> >:
~~~~~~~~~~~~~~~~~~~~~~~~~~~

     norm. resid      resid           machep         x(1)          x(n)
  0.36575e-4          0.52257e-102   0.28575e-100    0.1e1         0.1e1

    times are reported for matrices of order  1000
      factor     solve      total     mflops       unit      ratio
 times for array with leading dimension of1001
      279.96        0.84       280.8      2.3813     0.83988      5014.3

boost::multiprecision::ef::cpp_dec_float_50:
~~~~~~~~~~~~~~~~~~~~~~~~~

     norm. resid      resid           machep         x(1)          x(n)
     2.551330735e-16  1.275665107e-112 1e-99         1             1

    times are reported for matrices of order  1000
      factor     solve      total     mflops       unit      ratio
 times for array with leading dimension of1001
      363.89      1.074     364.97    1.8321       1.0916    6517.3
*/