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

#ifndef BOOST_MATH_TOOLS_SIMPLE_CONTINUED_FRACTION_HPP
#define BOOST_MATH_TOOLS_SIMPLE_CONTINUED_FRACTION_HPP

#include <vector>
#include <ostream>
#include <iomanip>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <boost/core/demangle.hpp>

namespace boost::math::tools {

template<typename Real, typename Z = int64_t>
class simple_continued_fraction {
public:
    simple_continued_fraction(Real x) : x_{x} {
        using std::floor;
        using std::abs;
        using std::sqrt;
        using std::isfinite;
        if (!isfinite(x)) {
            throw std::domain_error("Cannot convert non-finites into continued fractions.");
        }
        b_.reserve(50);
        Real bj = floor(x);
        b_.push_back(static_cast<Z>(bj));
        if (bj == x) {
           b_.shrink_to_fit();
           return;
        }
        x = 1/(x-bj);
        Real f = bj;
        if (bj == 0) {
           f = 16*std::numeric_limits<Real>::min();
        }
        Real C = f;
        Real D = 0;
        int i = 0;
        // the "1 + i++" lets the error bound grow slowly with the number of convergents.
        // I have not worked out the error propagation of the Modified Lentz's method to see if it does indeed grow at this rate.
        // Numerical Recipes claims that no one has worked out the error analysis of the modified Lentz's method.
        while (abs(f - x_) >= (1 + i++)*std::numeric_limits<Real>::epsilon()*abs(x_))
        {
          bj = floor(x);
          b_.push_back(static_cast<Z>(bj));
          x = 1/(x-bj);
          D += bj;
          if (D == 0) {
             D = 16*std::numeric_limits<Real>::min();
          }
          C = bj + 1/C;
          if (C==0) {
             C = 16*std::numeric_limits<Real>::min();
          }
          D = 1/D;
          f *= (C*D);
       }
       // Deal with non-uniqueness of continued fractions: [a0; a1, ..., an, 1] = a0; a1, ..., an + 1].
       // The shorter representation is considered the canonical representation,
       // so if we compute a non-canonical representation, change it to canonical:
       if (b_.size() > 2 && b_.back() == 1) {
          b_[b_.size() - 2] += 1;
          b_.resize(b_.size() - 1);
       }
       b_.shrink_to_fit();

       for (size_t i = 1; i < b_.size(); ++i) {
         if (b_[i] <= 0) {
            std::ostringstream oss;
            oss << "Found a negative partial denominator: b[" << i << "] = " << b_[i] << "."
                << " This means the integer type '" << boost::core::demangle(typeid(Z).name())
                << "' has overflowed and you need to use a wider type,"
                << " or there is a bug.";
            throw std::overflow_error(oss.str());
         }
       }
    }

    Real khinchin_geometric_mean() const {
        if (b_.size() == 1) {
         return std::numeric_limits<Real>::quiet_NaN();
        }
         using std::log;
         using std::exp;
         // Precompute the most probable logarithms. See the Gauss-Kuzmin distribution for details.
         // Example: b_i = 1 has probability -log_2(3/4) ≈ .415:
         // A random partial denominator has ~80% chance of being in this table:
         const std::array<Real, 7> logs{std::numeric_limits<Real>::quiet_NaN(), Real(0), log(static_cast<Real>(2)), log(static_cast<Real>(3)), log(static_cast<Real>(4)), log(static_cast<Real>(5)), log(static_cast<Real>(6))};
         Real log_prod = 0;
         for (size_t i = 1; i < b_.size(); ++i) {
            if (b_[i] < static_cast<Z>(logs.size())) {
               log_prod += logs[b_[i]];
            }
            else
            {
               log_prod += log(static_cast<Real>(b_[i]));
            }
         }
         log_prod /= (b_.size()-1);
         return exp(log_prod);
    }

    Real khinchin_harmonic_mean() const {
        if (b_.size() == 1) {
          return std::numeric_limits<Real>::quiet_NaN();
        }
        Real n = b_.size() - 1;
        Real denom = 0;
        for (size_t i = 1; i < b_.size(); ++i) {
            denom += 1/static_cast<Real>(b_[i]);
        }
        return n/denom;
    }

    const std::vector<Z>& partial_denominators() const {
      return b_;
    }

    template<typename T, typename Z2>
    friend std::ostream& operator<<(std::ostream& out, simple_continued_fraction<T, Z2>& scf);

private:
    const Real x_;
    std::vector<Z> b_;
};

template<typename Real, typename Z2>
std::ostream& operator<<(std::ostream& out, simple_continued_fraction<Real, Z2>& scf) {
   constexpr const int p = std::numeric_limits<Real>::max_digits10;
   if constexpr (p == 2147483647) {
      out << std::setprecision(scf.x_.backend().precision());
   } else {
      out << std::setprecision(p);
   }

   out << "[" << scf.b_.front();
   if (scf.b_.size() > 1)
   {
      out << "; ";
      for (size_t i = 1; i < scf.b_.size() -1; ++i)
      {
         out << scf.b_[i] << ", ";
      }
      out << scf.b_.back();
   }
   out << "]";
   return out;
}

}
#endif


/*****************************************************************************
 * Copyright (c) 2017, Massachusetts Institute of Technology.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its contributors
 *   may be used to endorse or promote products derived from this software
 *   without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/

#include <cmath>
#include <cstdio>

#include <Eigen/Core>

#include <drake/common/autodiff.h>  // IWYU pragma: keep
#include <drake/common/default_scalars.h>
#include <drake/common/drake_assert.h>
#include <drake/common/unused.h>
#include <drake/solvers/snopt_solver.h>
#include <drake/systems/analysis/simulator.h>
#include <drake/systems/framework/context.h>
#include <drake/systems/framework/continuous_state.h>
#include <drake/systems/framework/vector_system.h>
#include <drake/systems/trajectory_optimization/direct_collocation.h>

namespace shambhala {
namespace systems {

template <typename T>
class SimpleContinuousTimeSystem final : public drake::systems::VectorSystem<T> {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SimpleContinuousTimeSystem);
  SimpleContinuousTimeSystem() : drake::systems::VectorSystem<T>(::drake::systems::SystemTypeTag<SimpleContinuousTimeSystem>{}, 2, 1) { // n_in, n_out
    this->DeclareContinuousState(6); // n_state
  }

  // Scalar-converting copy constructor.  See @ref system_scalar_conversion.
  template <typename U>
  explicit SimpleContinuousTimeSystem(const SimpleContinuousTimeSystem<U>&) : SimpleContinuousTimeSystem<T>() {}

 private:
  // xdot = -x + x^3
  void DoCalcVectorTimeDerivatives(
      const drake::systems::Context<T>& context,
      const Eigen::VectorBlock<const drake::VectorX<T>>& input,
      const Eigen::VectorBlock<const drake::VectorX<T>>& state,
      Eigen::VectorBlock<drake::VectorX<T>>* derivatives) const override {

    // DS Kinetic 60
    const double g = 9.8;
    const double rho = 1.225 * .6; // ISA at 5km

    const double m = 2; // http://www.dskinetic.com/k60.aspx
    const double S = .23;

    // const double Vc2 = m*g/(.5*rho*S);
    // const double lambda = Vc2/g;

    const double cD0 = .005; // https://www.tngtech.com/fileadmin/Public/Images/BigTechday/BTD10/Folien/Folien_SpencerLisenby.pdf
    const double k = .006;

    const auto& speed = state(0);
    const auto& pitch = state(1);
    const auto& yaw   = state(2);
    const auto& z     = state(3);

    const auto& cL    = input(0);
    const auto& roll  = input(1);

    const double windspeed_gradient = .027; // jet stream at 5km: .02

    const auto zd = speed*sin(pitch);
    const auto W = windspeed_gradient*z;
    const auto Wd = windspeed_gradient*zd;

    const auto cD = cD0 + k*cL*cL;
    const auto D = .5*cD*rho*S*speed*speed;
    const auto L = .5*cL*rho*S*speed*speed;

    (*derivatives)(0) = 1/(m                 )*(     -D      - m*g*sin(pitch) + m*Wd*cos(pitch)*sin(yaw));
    (*derivatives)(1) = 1/(m*speed           )*( L*cos(roll) - m*g*cos(pitch) - m*Wd*sin(pitch)*sin(yaw));
    (*derivatives)(2) = 1/(m*speed*cos(pitch))*( L*sin(roll)                  + m*Wd           *cos(yaw));
    (*derivatives)(3) = zd;
    (*derivatives)(4) = speed*cos(pitch)*cos(yaw);
    (*derivatives)(5) = speed*cos(pitch)*sin(yaw) - W;
  }

  // y = x
  void DoCalcVectorOutput(
      const drake::systems::Context<T>& context,
      const Eigen::VectorBlock<const drake::VectorX<T>>& input,
      const Eigen::VectorBlock<const drake::VectorX<T>>& state,
      Eigen::VectorBlock<drake::VectorX<T>>* output) const /*override*/ {
    drake::unused(context, input);
    *output = state;
  }
};

}  // namespace systems
}  // namespace shambhala

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
    class ::shambhala::systems::SimpleContinuousTimeSystem)

int main() {
  // Create the simple system.
  shambhala::systems::SimpleContinuousTimeSystem<double> system;

  auto context = system.CreateDefaultContext();
  const int N = 201;
  const double dt_min = 9./N;
  const double dt_max = 13./N;
  drake::systems::trajectory_optimization::DirectCollocation dircol(
      &system, *context, N, dt_min, dt_max);
  dircol.AddEqualTimeIntervalsConstraints();

  // design limits
  auto tau = 4*acos(0);

  dircol.AddConstraintToAllKnotPoints(dircol.input()(0) >= 0); // 0 <= cL <= 1.2
  dircol.AddConstraintToAllKnotPoints(dircol.input()(0) <= 1.2);

  dircol.AddConstraintToAllKnotPoints(dircol.state()(0) >= 0);
  dircol.AddConstraintToAllKnotPoints(dircol.state()(0) <= 140); // DS Kinetic 60 speed record 140 m/s

  dircol.AddConstraintToAllKnotPoints(dircol.state()(1) >= -tau/2); // pitch looks forward
  dircol.AddConstraintToAllKnotPoints(dircol.state()(1) <=  tau/2);

  dircol.AddConstraintToAllKnotPoints(dircol.state()(2) >= -1.5*tau); // we want one turn in either direction
  dircol.AddConstraintToAllKnotPoints(dircol.state()(2) <=  1.5*tau);  // and allow an extra half turn

  dircol.AddConstraintToAllKnotPoints(dircol.state()(3) >= -500); // max 1km altitude differential (arbitrary)
  dircol.AddConstraintToAllKnotPoints(dircol.state()(3) <=  500);

  // planning target

  // dircol.AddConstraint(dircol.initial_state()(4) == 0); // x_N = x_0
  dircol.AddConstraint(dircol.initial_state()(5) == 0);

  dircol.AddConstraint(dircol.final_state()(0) >= dircol.initial_state()(0));
  dircol.AddConstraint(dircol.final_state()(1) == dircol.initial_state()(1));
  dircol.AddConstraint(dircol.final_state()(2) == dircol.initial_state()(2) + 1*tau);
  dircol.AddConstraint(dircol.final_state()(3) >= dircol.initial_state()(3));

  auto zmin = dircol.NewContinuousVariables(1, "zmin")(0);
  auto zmax = dircol.NewContinuousVariables(1, "zmax")(0);
  dircol.AddConstraintToAllKnotPoints(dircol.state()(3) >= zmin);
  dircol.AddConstraintToAllKnotPoints(dircol.state()(3) <= zmax);
  dircol.AddLinearCost(zmax-zmin);
  dircol.AddLinearConstraint(zmax == -zmin);

  // solver spoonfeeding

  dircol.AddConstraintToAllKnotPoints(dircol.state()(0) >= 1); // nonzero speed!

  dircol.AddConstraintToAllKnotPoints(dircol.input()(1) >= -tau/4); // do not turn the plane upside down
  dircol.AddConstraintToAllKnotPoints(dircol.input()(1) <=  tau/4);

  dircol.SetSolverOption(drake::solvers::SnoptSolver::id(), "Iterations limit", 10000000);
  dircol.SetSolverOption(drake::solvers::SnoptSolver::id(), "Major iterations limit", 10000);

  // solving

  auto result = dircol.Solve();
  if (result != drake::solvers::SolutionResult::kSolutionFound) {
    auto s = drake::solvers::to_string(result);
    printf("solving failed: %s\n", s.c_str());
    return 1;
  }

  // printing

  {
    auto inputs = dircol.ReconstructInputTrajectory();
    auto traj = dircol.ReconstructStateTrajectory();
    auto timestamps = traj.get_segment_times();
    printf("[\n");
    for (size_t i = 0; i < timestamps.size(); i++) {
      auto t = timestamps[i];
      auto V = traj.value(timestamps[i]).coeff(0);
      auto pitch = traj.value(timestamps[i]).coeff(1);
      auto yaw = traj.value(timestamps[i]).coeff(2);
      auto altitude = traj.value(timestamps[i]).coeff(3);
      auto x = traj.value(timestamps[i]).coeff(4);
      auto y = traj.value(timestamps[i]).coeff(5);
      auto cL = inputs.value(timestamps[i]).coeff(0);
      auto roll = inputs.value(timestamps[i]).coeff(1);
      printf("(%03.3f,  %03.3f, %03.3f, %03.3f, %03.3f, %03.3f, %03.3f,  %03.3f, %03.3f), # %3.3f\n", t, V, pitch, yaw, altitude, x, y, cL, roll, 9.8*altitude + .5*V*V);
    }
    printf("]\n");
  }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    Notes
 *      The current test cases are used to check the code of the mathematical functions, each case
 *      represents a special case where certain elements of the function can be eliminated.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/lambda/lambda.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/math/basic/mathematicalConstants.h"

#include "tudat/astro/mission_segments/improvedInversePolynomialWall.h"
#include "tudat/astro/mission_segments/oscillatingFunctionNovak.h"

using namespace boost::placeholders;

namespace tudat
{
namespace unit_tests
{

//! Test the spherical shape function code.
BOOST_AUTO_TEST_SUITE( test_spherical_shape_function )

//! Test case: inverse polynomial function.
BOOST_AUTO_TEST_CASE( ImprovedInversePolynomialWall )
{
    // Using declaration.
    using mathematical_constants::PI;

    // Initialize parameters of the inverse polynomial function.
    std::pair< Eigen::Vector3d, Eigen::Vector3d > inversePolynomialParameters;
    double timeDepParameter;
    double azimuthalAngle = 0.0;

    // Error tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon( );

    //*********************************************************************************************
    // CASE 1: b = 1.0, polar angle = 0.0, all other parameters 0.0
    //*********************************************************************************************

    // Case specific changes.
    inversePolynomialParameters.first = Eigen::Vector3d::Zero(  );
    inversePolynomialParameters.second = Eigen::Vector3d::Zero(  );
    timeDepParameter = 0.0;
    inversePolynomialParameters.first( 1 ) = 1.0; // b

    // Expected results.
    double expectedFunctionValue = 1.0;
    double expectedFirstDerivative = 0.0;
    double expectedSecondDerivative = 1.0;
    double expectedThirdDerivative = 0.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase1(
                [ & ]( ){ return timeDepParameter; } ,
                [ & ]( ){ return inversePolynomialParameters; } );

    // Calculate function value and derivatives.
    double functionValue = myFunctionCase1.evaluate( azimuthalAngle );
    double firstDerivative = myFunctionCase1.computeDerivative( 1 , azimuthalAngle );
    double secondDerivative = myFunctionCase1.computeDerivative( 2 , azimuthalAngle );
    double thirdDerivative = myFunctionCase1.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 2: c = 0.0, polar angle = 0.0, all other parameters 1.0
    //*********************************************************************************************

    // Case-specific changes.
    inversePolynomialParameters.first = Eigen::Vector3d::Ones(  );
    inversePolynomialParameters.second = Eigen::Vector3d::Ones(  );
    timeDepParameter = 1.0;
    inversePolynomialParameters.first( 2 ) = 0.0; // c

    // Expected results.
    expectedFunctionValue = 1.0 / 2.0;
    expectedFirstDerivative = 0.0;
    expectedSecondDerivative = 1.0 / 4.0;
    expectedThirdDerivative = - 6.0 / 4.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase2(
                [ & ]( ){ return timeDepParameter; } ,
                [ & ]( ){ return inversePolynomialParameters; } );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase2.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase2.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase2.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase2.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 3: c = pi - 1.0, polar angle = 1.0, all other parameters 1.0
    //*********************************************************************************************

    // Case-specific changes.
    inversePolynomialParameters.first( 2 ) = PI - 1.0; // c
    azimuthalAngle = 1.0;

    // Expected results.
    expectedFunctionValue = 1.0 / 4.0;
    expectedFirstDerivative = -9.0 / 8.0;
    expectedSecondDerivative = 93.0 / 16.0;
    expectedThirdDerivative = -267.0 / 8.0;

    // Initialize mathematical function.
    mission_segments::ImprovedInversePolynomialWall myFunctionCase3(
                [ & ]( ){ return timeDepParameter; } ,
                [ & ]( ){ return inversePolynomialParameters; } );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase3.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase3.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase3.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase3.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );
}

//! Test case: oscillating function.
BOOST_AUTO_TEST_CASE( OscillatingFunctionNovak )
{
    // Using declaration.
    using mathematical_constants::PI;

    // Initialize parameters of the inverse polynomial function.
    std::pair< Eigen::Vector2d, Eigen::Vector2d > oscillatingFunctionParameters;
    double azimuthalAngle = 0.0;

    // Error tolerance.
    const double tolerance = std::numeric_limits< double >::epsilon(  );

    //*********************************************************************************************
    // CASE 1: theta = 0.0, all parameters = 1.0
    //*********************************************************************************************

    // Case specific changes.
    oscillatingFunctionParameters.first = Eigen::Vector2d::Ones(  );
    oscillatingFunctionParameters.second = Eigen::Vector2d::Ones(  );

    // Expected results.
    double expectedFunctionValue = 1.0;
    double expectedFirstDerivative = 2.0;
    double expectedSecondDerivative = 1.0;
    double expectedThirdDerivative = - 4.0;

    // Initialize mathematical function.
    mission_segments::OscillatingFunctionNovak myFunctionCase1(
                [ & ]( ){ return oscillatingFunctionParameters; } );

    // Calculate function value and derivatives.
    double functionValue = myFunctionCase1.evaluate( azimuthalAngle );
    double firstDerivative = myFunctionCase1.computeDerivative( 1 , azimuthalAngle );
    double secondDerivative = myFunctionCase1.computeDerivative( 2 , azimuthalAngle );
    double thirdDerivative = myFunctionCase1.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                tolerance );

    //*********************************************************************************************
    // CASE 2: theta = pi/2, all parameters = 1.0
    //*********************************************************************************************

    // Case-specific changes.
    azimuthalAngle = PI / 2.0;

    // Expected results.
    expectedFunctionValue = 1.0 + PI / 2.0;
    expectedFirstDerivative = -PI / 2.0;
    expectedSecondDerivative = -3.0 - PI / 2.0;
    expectedThirdDerivative = -2.0 + PI / 2.0;

    // The term cos(PI/2) introduces a small error in the function value, and in the derivative
    // values. [ cos(PI/2) is approximately 6.12323e-017 ]
    // The calculation of the function value, the first derivative and the second derivative are
    // within the numeric limits, however the error in the third derivative is larger than this
    // limit. The tolerance for the third derivative is therefore more flexible than that of the
    // other calculations.
    const double toleranceThirdDerivative = 4.0 * tolerance;

    // The exact value of the third derivative, due to the introduced error. This vaue is checked
    // separately.
    double exactThirdDerivative =
            -2.0 + PI / 2.0 - 1.0 * ( 4.0 + PI / 2.0 ) * std::cos( PI / 2.0 );

    // Initialize mathematical function.
    mission_segments::OscillatingFunctionNovak myFunctionCase2(
                [ & ]( ){ return oscillatingFunctionParameters; } );

    // Calculate function value and derivatives.
    functionValue = myFunctionCase2.evaluate( azimuthalAngle );
    firstDerivative = myFunctionCase2.computeDerivative( 1 , azimuthalAngle );
    secondDerivative = myFunctionCase2.computeDerivative( 2 , azimuthalAngle );
    thirdDerivative = myFunctionCase2.computeDerivative( 3 , azimuthalAngle );

    // Test if the computed values correspond to the expected values, within the specified
    // tolerance.
    BOOST_CHECK_CLOSE_FRACTION( functionValue,
                                expectedFunctionValue,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( firstDerivative,
                                expectedFirstDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( secondDerivative,
                                expectedSecondDerivative,
                                tolerance );

    BOOST_CHECK_CLOSE_FRACTION( thirdDerivative,
                                expectedThirdDerivative,
                                toleranceThirdDerivative );

    BOOST_CHECK_EQUAL( thirdDerivative, exactThirdDerivative );
}

//! Test case: request wrong derivative or integral of one of the mathematical functions.
BOOST_AUTO_TEST_CASE( wrongRequestMathematicalFunctions )
{
    // Initialize parameters of the mathematical functions.
    // Inverse polynomial parameters.
    std::pair< Eigen::Vector3d, Eigen::Vector3d > inversePolynomialParameters;
    inversePolynomialParameters.first.Zero();
    inversePolynomialParameters.second.Zero();
    double timeDepParameter = 0.0;

    // Oscillating function parameters.
    std::pair< Eigen::Vector2d, Eigen::Vector2d > OscillatingShapeParameters;
    OscillatingShapeParameters.first.Zero(  );
    OscillatingShapeParameters.second.Zero(  );

    // Initialize mathematical functions.
    mission_segments::ImprovedInversePolynomialWall myInversePolynomial(
                [ & ]( ){ return timeDepParameter ; },
                [ & ]( ){ return inversePolynomialParameters; } );

    mission_segments::OscillatingFunctionNovak myOscillatingFunction(
                [ & ]( ){ return OscillatingShapeParameters; } );

    // Set flags.
    bool isFourthDerivativeInversePolynomial = true;
    bool isFourthDerivativeOscillatingFunction = true;
    bool isDefiniteIntegralInversePolynomial = true;
    bool isDefiniteIntegralOscillatingFunction = true;

    // Try to calculate the fourth derivative of the inverse polynomial function, which should
    // result in a runtime error.
    try
    {
        // Calculate the fourth derivative of the inverse polynomial function.
        myInversePolynomial.computeDerivative( 4 , 0.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch( std::runtime_error const& )

    {
        isFourthDerivativeInversePolynomial = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isFourthDerivativeInversePolynomial );

    // Try to calculate the definite integral of the inverse polynomial function, which should
    // result in a runtime error.
    try
    {
        // Calculate the definite integral of the inverse polynomial function.
        myInversePolynomial.computeDefiniteIntegral( 1 , 0.0 , 1.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch( std::runtime_error const& )

    {
        isDefiniteIntegralInversePolynomial = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isDefiniteIntegralInversePolynomial );

    // Try to calculate the fourth derivative of the oscillating function, which should result in a
    // runtime error.
    try
    {
        // Calculate the fourth derivative of the oscillating function.
        myOscillatingFunction.computeDerivative( 4 , 0.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch( std::runtime_error const& )

    {
        isFourthDerivativeOscillatingFunction = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isFourthDerivativeOscillatingFunction );

    // Try to calculate the definite integral of the oscillating function, which should result in a
    // runtime error.
    try
    {
        // Calculate the definite integral of the oscillating function.
        myOscillatingFunction.computeDefiniteIntegral( 1 , 0.0 , 1.0 );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch( std::runtime_error const& )

    {
        isDefiniteIntegralOscillatingFunction = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isDefiniteIntegralOscillatingFunction );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "verification/production/impl/threshold_util.hpp"

#include <boost/range/adaptors.hpp>
#include <boost/range/numeric.hpp>

namespace sgns::verification {

  Threshold calculateThreshold(
      const std::pair<uint64_t, uint64_t> &c_pair,
      const primitives::AuthorityList &authorities,
      primitives::AuthorityIndex authority_index) {
    double c = double(c_pair.first) / c_pair.second;

    using boost::adaptors::transformed;
    double theta =
        double(authorities[authority_index].weight)
        / boost::accumulate(authorities | transformed([](auto &authority) {
                              return authority.weight;
                            }),
                            0.);

    using namespace boost::multiprecision;  // NOLINT
    cpp_rational p_rat(1. - pow(1. - c, theta));
    static const auto a = (uint256_t{1} << 128);
    cpp_int t = a * numerator(p_rat) / denominator(p_rat);
    return Threshold{t};
  }

}  // namespace sgns::verification


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines the class Wedge2D.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/OrientationMap.hpp"
#include "Utilities/TypeTraits.hpp"

namespace PUP {
class er;
}  // namespace PUP

namespace CoordinateMaps {

/*!
 * \ingroup CoordinateMapsGroup
 *
 * \brief Two dimensional map from the logical square to a wedge, which is used
 * by domains which use disks or annuli.
 *
 * \details The coordinate map is constructed by
 * linearly interpolating between a bulged arc which is circumscribed by a
 * circular arc of radius `radius_outer` and a bulged arc which is
 * circumscribed by a circular arc of radius `radius_inner`. These arcs can be
 * made to be straight or circular, based on the value of `circularity_inner`
 * or `circularity_outer`, which can take on values between 0 (for straight)
 * and 1 (for circular). These arcs extend \f$\pi/2\f$ in angle, and can be
 * oriented along the +/- x or y axis. The choice of using either equiangular
 * or equidistant coordinates along the arcs is specifiable with
 * `with_equiangular_map`. The default logical coordinate that points in the
 * angular direction is \f$\eta\f$. We introduce the auxiliary variable
 * \f$\mathrm{H}\f$ which is a function of \f$\eta\f$. If we are using
 * equiangular coordinates, we have:
 *
 * \f[\mathrm{H}(\eta) = \textrm{tan}(\eta\pi/4)\f]
 *
 * With derivative:
 *
 * \f[\mathrm{H}'(\eta) = \frac{\pi}{4}(1+\mathrm{H}^2)\f]
 *
 * If we are using equidistant coordinates, we have:
 *
 * \f[\mathrm{H}(\eta) = \eta\f]
 *
 * with derivative:
 *
 * <center>\f$\mathrm{H}'(\eta) = 1\f$</center>
 *
 * We also define the variable \f$\rho\f$, given by:
 *
 * \f[\rho = \sqrt{1+\mathrm{H}^2}\f]
 *
 * In terms of the the circularity \f$c\f$ and the radius \f$R\f$,
 * the mapping is:
 *
 * \f[\vec{x}(\xi,\eta) =
 * \frac{1}{2}\left\{(1-\xi)\Big[(1-c_{inner})\frac{R_{inner}}{\sqrt 2}
 * + c_{inner}\frac{R_{inner}}{\rho}\Big] +
 * (1+\xi)\Big[(1-c_{outer})\frac{R_{outer}}{\sqrt 2} +c_{outer}
 * \frac{R_{outer}}{\rho}\Big] \right\}\begin{bmatrix}
 * 1\\
 * \mathrm{H}\\
 * \end{bmatrix}\f]
 *
 * We will define the variables \f$T(\xi)\f$ and \f$A(\xi)\f$, the trapezoid
 * and annulus factors: \f[T(\xi) = T_0 + T_1\xi\f] \f[A(\xi) = A_0 +
 * A_1\xi\f]
 * Where \f{align*}T_0 &= \frac{1}{2} \big\{ (1-c_{outer})R_{outer} +
 * (1-c_{inner})R_{inner}\big\}\\
 * T_1 &= \partial_{\xi} T = \frac{1}{2} \big\{ (1-c_{outer})R_{outer} -
 * (1-c_{inner})R_{inner}\big\}\\
 * A_0 &= \frac{1}{2} \big\{ c_{outer}R_{outer} + c_{inner}R_{inner}\big\}\\
 * A_1 &= \partial_{\xi} A = \frac{1}{2} \big\{ c_{outer}R_{outer} -
 * c_{inner}R_{inner}\big\}\f}
 *
 * The map can then be rewritten as:
 * \f[\vec{x}(\xi,\eta) = \left\{\frac{T(\xi)}{\sqrt 2} +
 * \frac{A(\xi)}{\rho}\right\}\begin{bmatrix}
 * 1\\
 * \mathrm{H}\\
 * \end{bmatrix}\f]
 *
 *
 * The Jacobian is: \f[J =
 * \begin{bmatrix}
 * \frac{T_1}{\sqrt 2} + \frac{A_1}{\rho} &
 * \mathrm{H}\mathrm{H}'\frac{A(\xi)}{\rho^3} \\
 * \mathrm{H}\partial_{\xi}x &
 * \mathrm{H}\partial_{\eta}x + \mathrm{H}'x\\
 * \end{bmatrix}
 * \f]
 *
 * The inverse Jacobian is: \f[J^{-1} =
 * \frac{1}{x}\begin{bmatrix}
 * \frac{1}{\partial_{\xi}x}\Big\{
 * \frac{T(\xi)}{\sqrt 2}+\frac{A(\xi)}{\rho^3}
 * \Big\} & \mathrm{H}\frac{1}{\partial_{\xi}x}\frac{A(\xi)}{\rho^3}\\
 * -\mathrm{H}\mathrm{H}'^{-1} & \mathrm{H}'^{-1}\\
 * \end{bmatrix}
 * \f]
 *
 * For a more detailed discussion, see the
 * documentation for Wedge3D, where the default logical coordinate that points
 * in the radial direction is \f$\zeta\f$ (In Wedge2D the logical coordinate
 * that points in the radial direction is \f$\xi\f$), and one may set either
 * of the two other logical coordinates to zero to obtain an equivalent
 * Wedge2D map.
 */

class Wedge2D {
 public:
  static constexpr size_t dim = 2;

  Wedge2D(double radius_inner, double radius_outer, double circularity_inner,
          double circularity_outer, OrientationMap<2> orientation_of_wedge,
          bool with_equiangular_map) noexcept;

  Wedge2D() = default;
  ~Wedge2D() = default;
  Wedge2D(Wedge2D&&) = default;
  Wedge2D& operator=(Wedge2D&&) = default;
  Wedge2D(const Wedge2D&) = default;
  Wedge2D& operator=(const Wedge2D&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 2> operator()(
      const std::array<T, 2>& source_coords) const noexcept;

  /// Returns invalid if \f$x<=0\f$ (for a \f$+x\f$-oriented `Wedge2D`).
  boost::optional<std::array<double, 2>> inverse(
      const std::array<double, 2>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 2, Frame::NoFrame> jacobian(
      const std::array<T, 2>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 2, Frame::NoFrame> inv_jacobian(
      const std::array<T, 2>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p);  // NOLINT
 private:
  friend bool operator==(const Wedge2D& lhs, const Wedge2D& rhs) noexcept;

  double radius_inner_{};
  double radius_outer_{};
  double circularity_inner_{};
  double circularity_outer_{};
  OrientationMap<2> orientation_of_wedge_{};
  bool with_equiangular_map_ = false;
  double scaled_trapezoid_zero_{std::numeric_limits<double>::signaling_NaN()};
  double annulus_zero_{std::numeric_limits<double>::signaling_NaN()};
  double scaled_trapezoid_rate_{std::numeric_limits<double>::signaling_NaN()};
  double annulus_rate_{std::numeric_limits<double>::signaling_NaN()};
};
bool operator!=(const Wedge2D& lhs, const Wedge2D& rhs) noexcept;
}  // namespace CoordinateMaps


/*
Copyright 2018 Dennis Rohde

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

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

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

#pragma once

#include <algorithm>
#include <vector>
#include <limits>
#include <cmath>

#include <boost/python/numpy.hpp>

#include "relation.hpp"

namespace np = boost::python::numpy;

template <typename T = double>
class KNN_Graph {
public:
    typedef unsigned long long index_type;

    class Adjacency_List : public std::vector<index_type> {
    public:
        inline auto get(const index_type i) const {
            if (i >= this->size()) return std::numeric_limits<index_type>::signaling_NaN();
            return (*this)[i];
        }

        inline auto length() const {
            return this->size();
        }

        inline auto pbegin() const {
            return this->begin();
        }

        inline auto pend() const {
            return this->end();
        }
    };

    typedef Tuple<T> location_type;
    typedef Relation<T> vertices_type;
    typedef Adjacency_List adjacency_list_type;
    typedef std::vector<adjacency_list_type> edges_type;

    inline static T euclidean_distance(const location_type &a, const location_type &b) {
        auto diff = a - b;
        return sqrt(diff * diff);
    }

    inline static T euclidean_distance_squared(const location_type &a, const location_type &b) {
        auto diff = a - b;
        return diff * diff;
    }

    KNN_Graph() : k{0}, vertices{}, edges{} {}

    KNN_Graph(const unsigned long k) : k{k}, vertices{}, edges{} {}

    void sort() {
        std::vector<index_type> index(number_vertices());

        #pragma omp parallel for shared(index)
        for (index_type i = 0; i < number_vertices(); ++i) {
            index[i] = i;
        }

        auto less = [&](const index_type lhs, const index_type rhs) -> bool {
            return vertices[lhs] < vertices[rhs];
        };

        std::sort(index.begin(), index.end(), less);

        vertices_type new_vertices(number_vertices());
        edges_type new_edges(number_vertices());

        #pragma omp parallel for shared(new_vertices, new_edges, index)
        for (index_type i = 0; i < number_vertices(); ++i) {
            new_vertices[i] = vertices[index[i]];
            new_edges[i] = edges[index[i]];
            std::sort(new_edges[i].begin(), new_edges[i].end(), less);
        }

        vertices = new_vertices;
        edges = new_edges;
    }

    auto epsilon(const KNN_Graph<T> &hp) const {
        double epsilon = 0.0;
        auto g = *this;
        auto h = hp;

        g.sort();
        h.sort();

        index_type i = 0, j = 0;
        unsigned long long denominator = 0;

        while (i < g.number_vertices() and j < h.number_vertices()) {
            if (g.get_vertex(i) == h.get_vertex(j)) {
                auto g_neighbors = g.get_neighbors(i);
                auto h_neighbors = h.get_neighbors(j);
                auto g_number_neighbors = g_neighbors.size();
                auto h_number_neighbors = h_neighbors.size();
                denominator += g_number_neighbors;
                index_type k = 0, l = 0;
                while (k < g_number_neighbors and l < h_number_neighbors) {
                    if (g_neighbors[k] < h_neighbors[l]) {
                        epsilon += 1;
                        ++k;
                    } else if (g_neighbors[k] > h_neighbors[l]) {
                        ++l;
                    } else {
                        ++k;
                        ++l;
                    }
                }
                epsilon += g_number_neighbors - k;
                ++i;
                ++j;
            } else if (g.get_vertex(i) < h.get_vertex(j)) {
                auto number_neighbors = g.get_neighbors(i).size();
                epsilon += number_neighbors;
                denominator += number_neighbors;
                ++i;
            } else {
                ++j;
            }
        }

        for(; i < g.number_vertices(); ++i) {
            auto number_neighbors = g.get_neighbors(i).size();
            epsilon += number_neighbors;
            denominator += number_neighbors;
        }

        epsilon /= denominator;

        return epsilon;
    }

    inline auto dimension() const {
        return number_vertices() == 0 ? 0 : vertices[0].dimension();
    }

    inline auto number_vertices() const {
        return vertices.size();
    }

    inline auto number_edges() const {
        return edges_number;
    }

    inline auto number_wrongly_connected_vertices() const {
        unsigned long long result = 0;

        #pragma omp parallel for shared(result)
        for (index_type i = 0; i < number_vertices(); ++i) {
            T distN = 0;
            auto wrongly_connected = false;
            auto &adj_list = edges[i];

            for (index_type j = 0; j < adj_list.size(); ++j) {
                auto dist = euclidean_distance_squared(vertices[i], vertices[adj_list[j]]);
                if (std::fabs(dist - distN) > std::numeric_limits<T>::epsilon() and dist > distN) {
                    distN = dist;
                }
            }

            for (index_type j = 0; j < number_vertices(); ++j) {
                if (wrongly_connected) continue;
                else if (std::find(adj_list.begin(), adj_list.end(), j) == adj_list.end() and i != j) {
                    auto dist = euclidean_distance_squared(vertices[i], vertices[j]);
                    if (std::fabs(dist - distN) > std::numeric_limits<T>::epsilon() and dist < distN) {
                        #pragma omp critical
                        {
                            if (not wrongly_connected) {
                                ++result;
                                wrongly_connected = true;
                            }
                        }
                    }
                }
            }
        }
        return result;
    }

    inline auto get_vertex(const index_type i) const {
        return vertices[i];
    }

    inline auto get_neighbors(const index_type i) const {
        return edges[i];
    }

    inline auto number_neighbors(const index_type i) const {
        return edges[i].size();
    }

    inline auto get_k() const {
        return k;
    }

    void add_edge(const index_type i, const index_type j) {
		this->edges[i].push_back(j);
		this->edges_number++;
	}

    virtual void build(const vertices_type &vertices) {
        this->vertices = vertices;
        this->edges = edges_type(vertices.size());
    }

    virtual void build(const np::ndarray &in) {
        this->vertices = vertices_type{in};
        this->edges = edges_type(vertices.size());
    }

    void edges_from_ndarray(const np::ndarray &in) {
        auto dimensions = in.get_nd();
        if (dimensions != 2 or in.get_dtype() != np::dtype::get_builtin<bool>()) {
            std::cerr << "Need 2-dimensional numpy array of type bool!" << std::endl;
            return;
        }
        this->edges = edges_type(vertices.size());
        auto size = in.get_shape();
        auto strides = in.get_strides();
        auto data = in.get_data();
        bool adjacent = false;
        for (index_type i = 0; i < size[0]; ++i) {
            for (index_type j = 0; j < size[1]; ++j) {
                adjacent = *reinterpret_cast<const bool*>(data + i * strides[0] + j * strides[1]);
                if (adjacent) {
                    this->edges[i].push_back(j);
                    ++this->edges_number;
                }
            }
        }
    }

    inline const auto& get_edges() const {
        return edges;
    }

    inline const auto& get_vertices() const {
        return vertices;
    }

    inline auto& get_edges() {
        return edges;
    }

    inline auto& get_vertices() {
        return vertices;
    }

    inline auto edges_begin() const {
        return edges.begin();
    }

    inline auto edges_end() const {
        return edges.end();
    }

    inline auto vertices_begin() const {
        return vertices.begin();
    }

    inline auto vertices_end() const {
        return vertices.end();
    }

    inline auto as_str() const {
        std::stringstream ss;
        ss << *this;
        return ss.str();
    }

    inline auto repr() const {
        std::stringstream ss;
        ss << get_k() <<"-Nearest Neighbor Graph of Dimension " << dimension() << ":" << std::endl;
        ss << as_str();
        return ss.str();
    }

protected:
    unsigned long k;
    unsigned long long edges_number = 0;
    vertices_type vertices;
    edges_type edges;

};

template <typename T = double>
class KNN_Graph_Exact : public KNN_Graph<T> {
    typedef KNN_Graph<T> super;
    typedef typename super::index_type index_type;

public:
    KNN_Graph_Exact(const unsigned long k = 10) {
        this->k = k;
        this->vertices = typename super::vertices_type{};
        this->edges = typename super::edges_type{};
    }

    void build(const np::ndarray &in) {
        this->build(typename super::vertices_type(in));
    }

    void build(const typename super::vertices_type &vertices) {
        this->vertices = vertices;
        this->edges = typename super::edges_type(vertices.size());
        std::cout << "Building exact " << this->k << "-NNGraph with " << vertices.size() << " vertices:" << std::endl;

        auto n = vertices.size();

        for (index_type i = 0; i < n; ++i) {
            auto distance_furthest = std::numeric_limits<double>::infinity();

            for (index_type j = 0; j < n; ++j) {

                if (i != j) {

                    auto dist = this->euclidean_distance_squared(this->vertices[i], this->vertices[j]);

                    if (this->edges[i].length() >= this->k) {
                        if (dist < distance_furthest) {
                            index_type furthest = 0;
                            index_type neighbor = 0;
                            index_type furthest_neighbor = 0;

                            for (index_type l = 0; l < this->edges[i].length(); ++l) {
                                neighbor = this->edges[i][l];
                                furthest_neighbor = this->edges[i][furthest];

                                if (this->euclidean_distance_squared(this->vertices[i], this->vertices[furthest_neighbor]) < this->euclidean_distance_squared(this->vertices[i], this->vertices[neighbor])) {
                                    furthest = l;
                                }
                            }
                            this->edges[i][furthest] = j;

                            furthest = 0;
                            for (index_type l = 0; l < this->edges[i].length(); ++l) {
                                neighbor = this->edges[i][l];
                                furthest_neighbor = this->edges[i][furthest];

                                if (this->euclidean_distance_squared(this->vertices[i], this->vertices[furthest_neighbor]) < this->euclidean_distance_squared(this->vertices[i], this->vertices[neighbor])) {
                                    furthest = l;
                                }
                            }
                            distance_furthest = this->euclidean_distance_squared(this->vertices[i], this->vertices[this->edges[i][furthest]]);
                        }
                    } else {
                        if (this->edges[i].length() == 0) {
                            distance_furthest = dist;
                        } else {
                            distance_furthest = std::max(dist, distance_furthest);
                        }
                        this->edges[i].push_back(j);
                    }
                }
            }

        }
        this->edges_number = n * this->k;
        std::cout << "Exact " << this->k << "-NNGraph built." << std::endl;
    }
};

template <typename T = double>
std::ostream& operator<<(std::ostream &out, const typename KNN_Graph<T>::Adjacency_List &a) {
    const auto size = a.size();
    for (auto i = size-size; i < size-1; ++i) {
        out << a.get(i);
        out << ", ";
    }
    out << a.get(size-1) << std::endl;
    return out;
}

template <typename T>
std::ostream& operator<<(std::ostream &out, const KNN_Graph<T> &g) {
    const auto size = g.number_vertices();
    for (auto i = size-size; i < size; ++i) {
        out << i << "-> ";
        out << g.get_neighbors(i);
    }
    return out;
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/fft/bsl_backend.hpp>
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/multiprecision/cpp_complex.hpp>

#include <complex>
#include <vector>
#include <array>

template< class fft_engine,int N >
void transform_api()
{
  using T = typename fft_engine::value_type;
  using boost::math::fft::transform;

  // test same type of iterator
  std::vector<T> A(N),B(A.size());
  transform<fft_engine>::forward(A.begin(),A.end(),B.begin());
  transform<fft_engine>::backward(A.begin(),A.end(),B.begin());

  // experimental
  // backend_t<T,std::allocator<T>>::static_forward(A.begin(),A.end(),B.begin());

  // test with raw pointers
  transform<fft_engine>::forward(A.data(),A.data()+A.size(),B.data());
  transform<fft_engine>::backward(A.data(),A.data()+A.size(),B.data());

  const auto & cA = A;
  // const iterator as input
  transform<fft_engine>::forward(cA.begin(),cA.end(),B.begin());
  transform<fft_engine>::backward(cA.begin(),cA.end(),B.begin());

  // const pointer as input
  transform<fft_engine>::forward(cA.data(),cA.data()+cA.size(),B.data());
  transform<fft_engine>::backward(cA.data(),cA.data()+cA.size(),B.data());

  std::array<T,N> C; // lets temporarily align this array here to avoid seg. fault
  // input as vector::iterator, output as array::iterator
  transform<fft_engine>::forward(A.begin(),A.end(),C.begin());
  transform<fft_engine>::backward(A.begin(),A.end(),C.begin());
  transform<fft_engine>::forward(A.data(),A.data()+A.size(),C.data());
  transform<fft_engine>::backward(A.data(),A.data()+A.size(),C.data());

  // input as array::iterator, output as vector::iterator
  transform<fft_engine>::forward(C.begin(),C.end(),B.begin());
  transform<fft_engine>::backward(C.begin(),C.end(),B.begin());
  transform<fft_engine>::forward(C.data(),C.data()+C.size(),B.data());
  transform<fft_engine>::backward(C.data(),C.data()+C.size(),B.data());
}

template<class Backend >
void plan_api(int N)
{
  using T = typename Backend::value_type;
  Backend P(N);
  std::vector<T> A(N),B(N);
  P.forward(A.data(),A.data()+N,B.data());
  P.backward(A.data(),A.data()+N,B.data());

  P.forward(A.begin(),A.end(),B.begin());
  P.backward(A.begin(),A.end(),B.begin());
}

struct my_type{};

void test_traits()
{
  using boost::multiprecision::is_boost_complex;
  static_assert(is_boost_complex< std::complex<float> >::value,"");
  static_assert(is_boost_complex< std::complex<double> >::value,"");
  static_assert(is_boost_complex< std::complex<long double> >::value,"");
  static_assert(is_boost_complex< float >::value==false,"");
  static_assert(is_boost_complex< my_type >::value==false,"");
  static_assert(is_boost_complex< my_type >::value==false,"");
  static_assert(is_boost_complex<
    std::complex<boost::multiprecision::cpp_bin_float_50> >::value==false,"");
  static_assert(is_boost_complex< boost::multiprecision::cpp_complex_quad >::value,"");
}

int main()
{
  #if defined(__GNUC__)
  using boost::math::fft::fftw_dft;
  using boost::math::fft::gsl_dft;
  #endif
  using boost::math::fft::bsl_dft;

  test_traits();

  #if defined(__GNUC__)
  transform_api<fftw_dft<std::complex<float>>,      4 >();
  transform_api<fftw_dft<std::complex<double>>,     4 >();
  transform_api<fftw_dft<std::complex<long double>>,4 >();
  #endif

  #if defined(__GNUC__)
  transform_api<gsl_dft<std::complex<double>>,4 >();
  #endif

  transform_api<bsl_dft<std::complex<float>>,      4 >();
  transform_api<bsl_dft<std::complex<double>>,     4 >();
  transform_api<bsl_dft<std::complex<long double>>,4 >();

  #if defined(__GNUC__)
  plan_api<fftw_dft<std::complex<double>> >(5);
  plan_api<fftw_dft<std::complex<float>> >(5);
  plan_api<fftw_dft<std::complex<long double>> >(5);
  #endif

  #if defined(__GNUC__)
  plan_api<gsl_dft<std::complex<double>> >(5);
  #endif

  plan_api<bsl_dft<std::complex<float>> >(5);
  plan_api<bsl_dft<std::complex<double>> >(5);
  plan_api<bsl_dft<std::complex<long double>> >(5);

  return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_WEIBULL_CDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_WEIBULL_CDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <boost/random/weibull_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * Returns the Weibull cumulative distribution function for the given
     * location and scale. Given containers of matching sizes, returns the
     * product of probabilities.
     *
     * @tparam T_y type of real parameter
     * @tparam T_shape type of shape parameter
     * @tparam T_scale type of scale paramater
     * @param y real parameter
     * @param alpha shape parameter
     * @param sigma scale parameter
     * @return probability or product of probabilities
     * @throw std::domain_error if y is negative, alpha sigma is nonpositive
     */
    template <typename T_y, typename T_shape, typename T_scale>
    typename return_type<T_y, T_shape, T_scale>::type
    weibull_cdf(const T_y& y, const T_shape& alpha, const T_scale& sigma) {
      typedef typename stan::partials_return_type<T_y, T_shape, T_scale>::type
        T_partials_return;

      static const char* function("weibull_cdf");

      using boost::math::tools::promote_args;
      using std::log;
      using std::exp;

      if (!(stan::length(y) && stan::length(alpha) && stan::length(sigma)))
        return 1.0;

      T_partials_return cdf(1.0);
      check_nonnegative(function, "Random variable", y);
      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Scale parameter", sigma);

      operands_and_partials<T_y, T_shape, T_scale>
        ops_partials(y, alpha, sigma);

      scalar_seq_view<T_y> y_vec(y);
      scalar_seq_view<T_scale> sigma_vec(sigma);
      scalar_seq_view<T_shape> alpha_vec(alpha);
      size_t N = max_size(y, sigma, alpha);
      for (size_t n = 0; n < N; n++) {
        const T_partials_return y_dbl = value_of(y_vec[n]);
        const T_partials_return sigma_dbl = value_of(sigma_vec[n]);
        const T_partials_return alpha_dbl = value_of(alpha_vec[n]);
        const T_partials_return pow_ = pow(y_dbl / sigma_dbl, alpha_dbl);
        const T_partials_return exp_ = exp(-pow_);
        const T_partials_return cdf_ = 1.0 - exp_;

        cdf *= cdf_;

        const T_partials_return rep_deriv = exp_ * pow_ / cdf_;
        if (!is_constant_struct<T_y>::value)
          ops_partials.edge1_.partials_[n] += rep_deriv * alpha_dbl / y_dbl;
        if (!is_constant_struct<T_shape>::value)
          ops_partials.edge2_.partials_[n]
            += rep_deriv * log(y_dbl / sigma_dbl);
        if (!is_constant_struct<T_scale>::value)
          ops_partials.edge3_.partials_[n] -= rep_deriv * alpha_dbl / sigma_dbl;
      }

      if (!is_constant_struct<T_y>::value) {
        for (size_t n = 0; n < stan::length(y); ++n)
          ops_partials.edge1_.partials_[n] *= cdf;
      }
      if (!is_constant_struct<T_shape>::value) {
        for (size_t n = 0; n < stan::length(alpha); ++n)
          ops_partials.edge2_.partials_[n] *= cdf;
      }
      if (!is_constant_struct<T_scale>::value) {
        for (size_t n = 0; n < stan::length(sigma); ++n)
          ops_partials.edge3_.partials_[n] *= cdf;
      }
      return ops_partials.build(cdf);
    }

  }
}
#endif


#include <boost/graph/kamada_kawai_spring_layout.hpp>


#define CATCH_CONFIG_MAIN
#include <Data/RIGID_STRUCTURE.h>
#include <Data/RIGID_STRUCTURE_DATA.h>
#include <Driver/SIMULATION.h>
#include <Equation/NONLINEAR_EQUATION.h>
#include <Force/FORCE.h>
#include <Force/VOLUME_EXCLUSION_CONSTRAINT.h>
#include <Indexing/RIGID_STRUCTURE_INDEX_MAP.h>
#include <Math/F.h>
#include <Math/F_NF.h>
#include <Math/NF.h>
#include <Math/NFINV.h>
#include <Math/R1XRCXR2INV.h>
#include <Math/RCF_NF.h>
#include <Math/RXO.h>
#include <Math/Relative_Position_Force.h>
#include <Math/Spring_Force.h>
#include <Utilities/RANDOM.h>
#include <Eigen/CXX11/Tensor>
#include <Eigen/KroneckerProduct>
#include <catch.hpp>

using namespace Mechanics;
using namespace Eigen;
typedef double T;
typedef Matrix<T,3,1> TV;
typedef Matrix<T,3,1> T_SPIN;
typedef Matrix<T,3,3> M_VxV;
typedef TensorFixedSize<T,Sizes<3,3,3>> T_TENSOR;
typedef Dimension::LINEARITY LINEARITY;

M_VxV Contract(const T_TENSOR& t,const TV& v,const std::array<int,3>& indices){
    M_VxV result;result.setZero();
    std::array<int,3> index{};
    for(index[0]=0;index[0]<3;index[0]++){
        for(index[1]=0;index[1]<3;index[1]++){
            for(index[2]=0;index[2]<3;index[2]++){
                result(index[1],index[2])+=t(index[indices[0]],index[indices[1]],index[indices[2]])*v(index[0]);
            }}}
    return result;
}

TV Contract(const T_TENSOR& t,const TV& v1,const TV& v2,const std::array<int,3>& indices){
    TV result;result.setZero();
    std::array<int,3> index{};
    for(index[0]=0;index[0]<3;index[0]++){
        for(index[1]=0;index[1]<3;index[1]++){
            for(index[2]=0;index[2]<3;index[2]++){
                result(index[2])+=t(index[indices[0]],index[indices[1]],index[indices[2]])*v1(index[0])*v2(index[1]);
            }}}
    return result;
}

TV Evaluate(const std::array<TV,2>& positions,const std::array<T_SPIN,2>& spins,const std::array<TV,2>& offsets){
    return positions[1]+ROTATION<TV>::From_Rotation_Vector(spins[1])*offsets[1]-
        (positions[0]+ROTATION<TV>::From_Rotation_Vector(spins[0])*offsets[0]);}

TEST_CASE("Hessian"){
    RANDOM<T> random;
    T epsilon=1e-3;
    T divisor=2;
    std::array<TV,2> positions;
    std::array<T_SPIN,2> spins;
    std::array<TV,2> offsets,spun_offsets;
    for(int i=0;i<2;i++){
        positions[i]=random.template Direction<TV>();
        spins[i]=random.template Direction<T_SPIN>();
        offsets[i]=random.template Direction<TV>();
        spun_offsets[i]=ROTATION<TV>::From_Rotation_Vector(spins[i])*offsets[i];}
    TV f=Evaluate(positions,spins,offsets);
    TV dx=random.template Direction<TV>();

    SECTION("dnf_dVelocity"){
        TV dnf_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::dnf_dVelocity<LINEARITY::LINEAR>(f,f.norm(),1,spins[0],offsets[0]);
        auto testlambda=[&](T eps){
            T predicted=dnf_dv.dot(eps*dx);
            T actual=Evaluate({positions[0],positions[1]+eps*dx},spins,offsets).norm()-f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);}

    SECTION("dnf_dSpin"){
        T_SPIN spin=random.template Direction<T_SPIN>();
        TV dnf_ds=RIGID_STRUCTURE_INDEX_MAP<TV>::dnf_dVelocity<LINEARITY::ANGULAR>(f,f.norm(),1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            T predicted=dnf_ds.dot(eps*dx);
            T actual=Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).norm()-f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);

        M_VxV d2nf_ds2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2n_dVelocity2<LINEARITY::ANGULAR,LINEARITY::ANGULAR>(f,{1,1},{spins[1],spins[1]},{offsets[1],offsets[1]});
        auto test_second=[&](T eps){
            T predicted=dnf_ds.dot(eps*dx)+(T).5*eps*eps*dx.transpose()*d2nf_ds2*dx;
            T actual=Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).norm()-f.norm();
            return actual-predicted;};
        ratio=test_second(epsilon)/test_second(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("dnfinv_dVelocity","d2nfinv_dVelocity2"){
        TV dnfinv_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::dnfinv_dVelocity<LINEARITY::LINEAR>(f,f.norm(),1,spins[1],offsets[1]);
        M_VxV d2nfinv_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::d2nfinv_dVelocity2<LINEARITY::LINEAR,LINEARITY::LINEAR>(f,f.norm(),{1,1},spins,offsets);
        auto testlambda=[&](T eps){
            T predicted=dnfinv_dv.dot(eps*dx)+((T).5*eps*eps*dx.transpose()*d2nfinv_dv*dx);
            T actual=1/Evaluate({positions[0],positions[1]+eps*dx},spins,offsets).norm()-1/f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("dnfinv_dSpin"){
        TV dnfinv_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::dnfinv_dVelocity<LINEARITY::ANGULAR>(f,f.norm(),1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            T predicted=dnfinv_dv.dot(eps*dx);
            T actual=1/Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).norm()-1/f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);}

    SECTION("d2nfinv_dSpin2"){
        M_VxV d2nfinv_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::d2nfinv_dVelocity2<LINEARITY::ANGULAR,LINEARITY::ANGULAR>(f,f.norm(),{1,1},{spins[1],spins[1]},{offsets[1],offsets[1]});
        TV d0=RIGID_STRUCTURE_INDEX_MAP<TV>::dnfinv_dVelocity<LINEARITY::ANGULAR>(f,f.norm(),1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            T predicted=d0.dot(eps*dx)+((T).5*eps*eps*dx.transpose()*d2nfinv_dv*dx);
            T actual=1/Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).norm()-1/f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("dnainv_dA"){
        TV dnainv_da=RIGID_STRUCTURE_INDEX_MAP<TV>::dnainv_dA(f,f.norm());
        auto testlambda=[&](T eps){
            T predicted=dnainv_da.dot(dx)*eps;
            T actual=1/(f+eps*dx).norm()-1/f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);}

    SECTION("da_na_dA"){
        M_VxV da_na_da=RIGID_STRUCTURE_INDEX_MAP<TV>::da_na_dA(f,f.norm());
        T_TENSOR d2a_na_da2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2a_na_dA2(f,f.norm());
        auto test_second=[&](T eps){
            TV contracted=Contract(d2a_na_da2,dx,dx,{0,1,2});
            TV predicted=da_na_da*dx*eps+((T).5*eps*eps*contracted);
            TV actual=(f+eps*dx).normalized()-f.normalized();
            return (actual-predicted).norm();};
        T ratio=test_second(epsilon)/test_second(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("df_nf_dVelocity"){
        M_VxV df_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::df_nf_dVelocity<LINEARITY::LINEAR>(f,1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            TV predicted=df_dv*(eps*dx);
            TV actual=Evaluate({positions[0],positions[1]+eps*dx},spins,offsets).normalized()-f.normalized();
            return (actual-predicted).norm();};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);}

    SECTION("df_dSpin"){
        M_VxV df_ds=RIGID_STRUCTURE_INDEX_MAP<TV>::df_dVelocity<LINEARITY::ANGULAR>(1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            TV predicted=df_ds*(eps*dx);
            TV actual=Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets)-Evaluate(positions,spins,offsets);
            return (actual-predicted).norm();};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);}

    SECTION("d2f_nf_dVelocity2"){
        T_TENSOR d2f_dv2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2f_nf_dVelocity2<LINEARITY::LINEAR,LINEARITY::LINEAR>(f,{1,1},spins,offsets);
        M_VxV df_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::df_nf_dVelocity<LINEARITY::LINEAR>(f,1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            TV predicted=df_dv*(eps*dx)+(T).5*eps*eps*Contract(d2f_dv2,dx,dx,{0,1,2});
            TV actual=Evaluate({positions[0],positions[1]+eps*dx},spins,offsets).normalized()-f.normalized();
            return (actual-predicted).norm();};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("d2f_nf_dSpin2"){
        T_TENSOR d2f_ds2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2f_nf_dVelocity2<LINEARITY::ANGULAR,LINEARITY::ANGULAR>(f,{1,1},{spins[1],spins[1]},{offsets[1],offsets[1]});
        M_VxV df_ds=RIGID_STRUCTURE_INDEX_MAP<TV>::df_nf_dVelocity<LINEARITY::ANGULAR>(f,1,spins[1],offsets[1]);
        auto testlambda=[&](T eps){
            TV predicted=df_ds*(eps*dx)+(T).5*eps*eps*Contract(d2f_ds2,dx,dx,{0,1,2});
            TV actual=Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).normalized()-f.normalized();
            return (actual-predicted).norm();};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("d2n_dVelocity2"){
        M_VxV d2n_dv2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2n_dVelocity2<LINEARITY::LINEAR,LINEARITY::LINEAR>(f,{1,1},spins,offsets);
        TV dnf_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::dnf_dVelocity<LINEARITY::LINEAR>(f,f.norm(),1,spins[1],offsets[1]);

        auto testlambda=[&](T eps){
            T predicted=dnf_dv.dot(eps*dx)+(T)0.5*eps*eps*dx.transpose()*d2n_dv2*dx;
            T actual=Evaluate({positions[0],positions[1]+eps*dx},spins,offsets).norm()-f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("d2n_dSpin2"){
        M_VxV d2n_dv2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2n_dVelocity2<LINEARITY::ANGULAR,LINEARITY::ANGULAR>(f,{1,1},{spins[1],spins[1]},{offsets[1],offsets[1]});
        TV dnf_dv=RIGID_STRUCTURE_INDEX_MAP<TV>::dnf_dVelocity<LINEARITY::ANGULAR>(f,f.norm(),1,spins[1],offsets[1]);

        auto testlambda=[&](T eps){
            T predicted=dnf_dv.dot(eps*dx)+(T)0.5*eps*eps*dx.transpose()*d2n_dv2*dx;
            T actual=Evaluate(positions,{spins[0],spins[1]+eps*dx},offsets).norm()-f.norm();
            return actual-predicted;};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    /*
      for a proper evaluation, in theory, have to vary all of the things: which derivatives wrt,

      for a scalar function:
      assume we have 1st and second derivatives for it, wrt angular and linear
      build all of the necessary first derivatives (four) and second derivatives (16)

     */

    // for vector functions

    // VTYPE: LINEAR or ANGULAR
    // VSIGN:: -1 or 1
    // V: 0 or 1 (storage index)
    //
    // need second derivatives for all combinations (4 x 4)

    std::array<std::array<TV,2>,2> dxs;
    for(int i=0;i<2;i++){
        for(int j=0;j<2;j++){
            dxs[i][j]=random.template Direction<TV>();
        }}

    SECTION("F"){
        T ratio=F<TV>::Test_Error(positions,spins,offsets,dxs,epsilon)/F<TV>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("NF"){
        T ratio=NF<TV>::Test_Error(positions,spins,offsets,dxs,epsilon)/NF<TV>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("NFINV"){
        T ratio=NFINV<TV>::Test_Error(positions,spins,offsets,dxs,epsilon)/NFINV<TV>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("F_NF"){
        T ratio=F_NF<TV>::Test_Error(positions,spins,offsets,dxs,epsilon)/F_NF<TV>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("RXO"){
        T ratio=RXO<TV,1>::Test_Error(positions,spins,offsets,dxs,epsilon)/RXO<TV,1>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    SECTION("RXF_NF"){
        T ratio0=RCF_NF<TV,0>::Test_Error(positions,spins,offsets,dxs,epsilon)/RCF_NF<TV,0>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio0-cube(divisor))<0.1);
    }

    SECTION("RXF_NF"){
        T ratio1=RCF_NF<TV,1>::Test_Error(positions,spins,offsets,dxs,epsilon)/RCF_NF<TV,1>::Test_Error(positions,spins,offsets,dxs,epsilon/divisor);
        REQUIRE(fabs(ratio1-cube(divisor))<0.1);
    }

    SECTION("practical"){
        std::vector<Triplet<T>> hessian_terms;
        TV f=F<TV>::Evaluate(positions,spins,offsets);
        Relative_Position_Force<TV>::Second_Derivatives(f,spins,offsets,{0,1},dxs,hessian_terms);
    }

    SECTION("Spring_Force"){
        T target=3;
        T stiffness=10;
        M_VxV derivative=Spring_Force<TV>::template First_Derivative<1,1,0,1>(stiffness,target,f,spins,offsets);
        auto testlambda=[&](T eps){
            TV predicted=derivative.transpose()*dxs[0][0]*eps;
            TV actual=Spring_Force<TV>::template Evaluate<1,1>(stiffness,target,{positions[0],positions[1]},{spins[0]+eps*dxs[0][0],spins[1]},offsets)-Spring_Force<TV>::template Evaluate<1,1>(stiffness,target,positions,spins,offsets);
            return (actual-predicted).norm();
        };
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);
    }

    SECTION("association dissociation"){
        ROTATION<TV> RC=ROTATION<TV>::From_Rotation_Vector(positions[0]);
        M_VxV derivative=R1XRCXR2INV<TV>::template First_Derivative<0>(RC,spins);
        auto testlambda=[&](T eps){
            TV predicted=derivative.transpose()*dxs[0][0]*eps;
            TV actual=R1XRCXR2INV<TV>::Evaluate(RC,{spins[0]+eps*dxs[0][0],spins[1]})-R1XRCXR2INV<TV>::Evaluate(RC,spins);
            return (actual-predicted).norm();
        };
        T ratio=testlambda(epsilon)/testlambda(epsilon/2);
        REQUIRE(fabs(ratio-sqr(divisor))<0.1);
    }

    SECTION("d2f_nf_dVelocity2 full"){
        std::array<M_VxV,2> df_dvs;
        std::array<TV,2> dxs;
        Matrix<T_TENSOR,2,2> d2f_dv2s;
        TV zero;zero.setZero();
        TV f=Evaluate(positions,spins,{zero,zero});
        for(int s1=0,s1_sgn=-1;s1<2;s1++,s1_sgn+=2){
            for(int s2=0,s2_sgn=-1;s2<2;s2++,s2_sgn+=2){
                d2f_dv2s(s1,s2)=RIGID_STRUCTURE_INDEX_MAP<TV>::d2f_nf_dVelocity2<LINEARITY::LINEAR,LINEARITY::LINEAR>(f,{s1_sgn,s2_sgn},{spins[s1],spins[s1]},{offsets[s1],offsets[s2]});}
            df_dvs[s1]=RIGID_STRUCTURE_INDEX_MAP<TV>::df_nf_dVelocity<LINEARITY::LINEAR>(f,s1_sgn,spins[s1],offsets[s1]);
            dxs[s1]=random.template Direction<TV>();}

        auto testlambda=[&](T eps){
            TV predicted;predicted.setZero();
            for(int s1=0;s1<2;s1++){
                predicted+=df_dvs[s1].transpose()*(eps*dxs[s1]);
                for(int s2=0;s2<2;s2++){
                    predicted+=.5*eps*eps*dxs[s1].transpose()*Contract(d2f_dv2s(s1,s2),dxs[s2],{2,0,1});}}
            TV actual=Evaluate({positions[0]+eps*dxs[0],positions[1]+eps*dxs[1]},spins,{zero,zero}).normalized()-f.normalized();
            return (actual-predicted).norm();};
        T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    // ROTATIONAL PARTS
    SECTION("dw_dSpin","d2w_dSpin2"){
        T_SPIN spin=random.template Direction<T_SPIN>();
        T norm_spin=spin.norm();
        T initial=cos(norm_spin/2);
        T_SPIN dw_ds=RIGID_STRUCTURE_INDEX_MAP<TV>::dw_dSpin(spin,norm_spin);
        T_SPIN ds=random.template Direction<T_SPIN>();
        M_VxV d2d_ds2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2w_dSpin2(spin,norm_spin);
        auto testlambda_hessian=[&](T eps){
            T predicted=dw_ds.dot(eps*ds)+(T).5*eps*eps*ds.transpose()*d2d_ds2*ds;;
            T actual=cos((spin+eps*ds).norm()/2)-initial;
            return actual-predicted;};

        T ratio_hessian=testlambda_hessian(epsilon)/testlambda_hessian(epsilon/divisor);
        REQUIRE(fabs(ratio_hessian-cube(divisor))<0.1);}

    SECTION("dq_dSpin","d2q_dSpin2"){
        T_SPIN spin=random.template Direction<T_SPIN>();
        T norm_spin=spin.norm();

        T_SPIN initial=sinc(norm_spin/2)*spin/2;
        M_VxV dq_dspin=RIGID_STRUCTURE_INDEX_MAP<TV>::dq_dSpin(spin/norm_spin,norm_spin);
        T_SPIN ds=random.template Direction<T_SPIN>();

        T_TENSOR d2d_ds2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2q_dSpin2(spin,norm_spin);
        auto testlambda_hessian=[&](T eps){
            T_SPIN predicted=dq_dspin*(eps*ds)+(T).5*eps*eps*Contract(d2d_ds2,ds,ds,{0,1,2});
            T_SPIN final_spin=spin+eps*ds;
            T_SPIN actual=sinc(final_spin.norm()/2)*final_spin/2-initial;
            return (actual-predicted).norm();};

        T ratio_hessian=testlambda_hessian(epsilon)/testlambda_hessian(epsilon/divisor);
        REQUIRE(fabs(ratio_hessian-cube(divisor))<0.1);}

    SECTION("d2sxo_dSpin2"){
        M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dRotatedOffset_dSpin(spins[1],offsets[1]);
        TV delta=random.template Direction<TV>();
        T_TENSOR d2so_ds2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2so_dSpin2(spins[1],offsets[1]);
        auto test_second=[&](T eps){
            T_SPIN dspin=eps*dx;
            TV predicted=derivative*dspin+(T).5*Contract(d2so_ds2,dspin,dspin,{0,1,2});
            TV final=ROTATION<TV>::From_Rotation_Vector(spins[1]+dspin)*offsets[1];
            return (final-spun_offsets[1]-predicted).norm();};
        T ratio=test_second(epsilon)/test_second(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}

    // r x f, where f=(f_2-f_1)/|f_2-f_1|
    SECTION("dtau_dSpin","d2tau_dSpin2"){
        TV tau_initial=spun_offsets[1].cross(f.normalized());
        M_VxV dtau_da=RIGID_STRUCTURE_INDEX_MAP<TV>::dtau_dA<LINEARITY::ANGULAR>(f,1,spins[1],offsets[1]);
        T_TENSOR d2tau_da2=RIGID_STRUCTURE_INDEX_MAP<TV>::d2tau_dV2<LINEARITY::ANGULAR,LINEARITY::ANGULAR>(f,spun_offsets[1],{1,1},{spins[1],spins[1]},{offsets[1],offsets[1]});
        auto test_second=[&](T eps){
            T_SPIN dspin=eps*dx;
            TV predicted=dtau_da*dspin+(T).5*Contract(d2tau_da2,dspin,dspin,{0,1,2});
            TV r_final=ROTATION<TV>::From_Rotation_Vector(spins[1]+dspin)*offsets[1];
            TV tau_final=r_final.cross(Evaluate(positions,{spins[0],spins[1]+dspin},offsets).normalized());
            T error=(tau_final-tau_initial-predicted).norm();
            return error;};
        T ratio=test_second(epsilon)/test_second(epsilon/divisor);
        REQUIRE(fabs(ratio-cube(divisor))<0.1);}
}

TEST_CASE("ASSOCIATION_DISSOCATION_CONSTRAINT"){
    RANDOM<T> random;
    int tests=10;
    T epsilon=1e-4;
    SECTION("dRdS x F"){
        for(int i=0;i<tests;i++){
            /*T_SPIN s1=random.template Direction<T_SPIN>();
            T_SPIN s2=random.template Direction<T_SPIN>();
            ROTATION<TV> r1(ROTATION<TV>::From_Rotation_Vector(s1));
            ROTATION<TV> r2(ROTATION<TV>::From_Rotation_Vector(s2));
            TV x1=random.template Direction<TV>();

            std::cout<<"R2: "<<(r2.inverse()*x1).transpose()<<std::endl;
            std::cout<<"Complicated: "<<((r1.inverse().toRotationMatrix()*(r2.toRotationMatrix()*r1.toRotationMatrix().inverse()).inverse())*x1).transpose()<<std::endl;*/

            T_SPIN spin=random.template Direction<T_SPIN>();
            TV base_offset=random.template Direction<TV>();
            TV f=random.template Direction<TV>();
            ROTATION<TV> rotation(ROTATION<TV>::From_Rotation_Vector(spin));
            T_SPIN dspin=epsilon*random.template Direction<T_SPIN>();

            TV initial=(rotation*base_offset).cross(f);
            TV final=(ROTATION<TV>::From_Rotation_Vector(spin+dspin)*base_offset).cross(f);

            M_VxV dFdS;

            M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dRotatedOffset_dSpin(spin,base_offset);
            for(int j=0;j<3;j++){
                dFdS.block<3,1>(0,j)=derivative.block<3,1>(0,j).cross(f);
            }
            TV predicted_delta=dFdS*dspin;
            REQUIRE((final-initial-predicted_delta).norm()<1e-2*epsilon);
        }
    }
    SECTION("dCrdS"){
        // derivative of the rotation constraint
        for(int i=0;i<tests;i++){
            T_SPIN spin=random.template Direction<T_SPIN>();
            ROTATION<TV> base_rotation=ROTATION<TV>::From_Rotation_Vector(random.template Direction<T_SPIN>());
            ROTATION<TV> rotation(ROTATION<TV>::From_Rotation_Vector(spin));
            T_SPIN dspin_direction=random.template Direction<T_SPIN>();
            ROTATION<TV> total_rotation=base_rotation*rotation.inverse();
            TV initial=total_rotation.vec();
            M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::Compute_Simple_Orientation_Constraint_Matrix(rotation,total_rotation,-1);
            auto testlambda=[&](T eps){
                T_SPIN dspin=eps*dspin_direction;
                TV final=(base_rotation*ROTATION<TV>::From_Rotation_Vector(spin+dspin).inverse()).vec();
                TV predicted=derivative*dspin;
                return (final-initial-predicted).norm();
            };
            T ratio=testlambda(epsilon)/testlambda(epsilon/2);
            REQUIRE(fabs(ratio-4)<1e-2);
        }
    }
}

TEST_CASE("VOLUME_EXCLUSION_CONSTRAINT","[derivatives]"){
    /*RANDOM<T> random;
    SECTION("derivative"){
        SIMULATION<TV> simulation;
        auto rigid_data=simulation.data.template Find_Or_Create<RIGID_STRUCTURE_DATA<TV>>();
        auto volume_exclusion_constraint=simulation.force.template Find_Or_Create<VOLUME_EXCLUSION_CONSTRAINT<TV>>();
        auto structure=std::make_shared<RIGID_STRUCTURE<TV>>();
        structure->frame.position=random.template Direction<TV>();
        structure->radius=1;
        structure->collision_radius=1;
        structure->Initialize_Inertia(3.5);
        rigid_data->structures.push_back(structure);

        auto structure2=std::make_shared<RIGID_STRUCTURE<TV>>();
        structure2->frame.position=structure->frame.position+TV::UnitX()*1.8;
        structure2->radius=1;
        structure2->collision_radius=1;
        structure2->Initialize_Inertia(3.5);
        rigid_data->structures.push_back(structure2);

        NONLINEAR_EQUATION<TV> equation;
        equation.Linearize(simulation.data,simulation.force,1,0,1);
        //std::cout<<equation.jacobian<<std::endl;

        REQUIRE(1);
        }*/
}

TEST_CASE("Derivatives","[derivatives]"){
    RANDOM<T> random;
    int tests=10;
    SECTION("dConstraint_dTwist"){
        // the constraint is |x2-x1|
        for(int i=0;i<tests;i++){
            TV x1=random.template Direction<TV>();
            TV x2=random.template Direction<TV>();
            T_SPIN spin=random.template Direction<T_SPIN>();
            TV base_offset=random.template Direction<TV>();;

            TV rotated_offset=ROTATION<TV>::From_Rotation_Vector(spin)*base_offset;
            TV relative_position=x2+rotated_offset-x1;
            Matrix<T,1,6> derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dConstraint_dTwist(spin,base_offset,relative_position);

            T epsilon=1e-5;
            Matrix<T,6,1> dx_direction;random.Direction(dx_direction);

            T initial=(x2+rotated_offset-x1).norm();

            auto testlambda=[&](T eps){
                Matrix<T,6,1> dx=eps*dx_direction;
                T final=(x2+dx.block<3,1>(0,0)+ROTATION<TV>::From_Rotation_Vector(spin+dx.block<3,1>(3,0))*base_offset-x1).norm();
                T predicted=derivative*dx;
                //std::cout<<"final: "<<final<<" initial: "<<initial<<" predicted: "<<predicted<<std::endl;
                return (final-initial-predicted);
            };
            T ratio=testlambda(epsilon)/testlambda(epsilon/2);
            //std::cout<<"dC_dT ratio: "<<ratio<<std::endl;
            REQUIRE(fabs(ratio-4)<0.1);
        }
    }
    SECTION("dForce_dVelocity"){
        for(int i=0;i<tests;i++){
            TV x1=random.template Direction<TV>();
            TV x2=random.template Direction<TV>();
            TV relative_position=x2-x1;
            TV direction=relative_position.normalized();
            M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dForce_dVelocity(relative_position);
            T epsilon=1e-6;
            TV delta=epsilon*random.template Direction<TV>();
            TV estimated_direction=direction+derivative*delta;
            TV actual_direction=(x2+delta-x1).normalized();
            //std::cout<<"Quality: "<<(actual_direction-estimated_direction).norm()/epsilon<<std::endl;
            REQUIRE((actual_direction-estimated_direction).norm()<epsilon);
        }
    }
    SECTION("dRotatedOffset_dSpin"){
        T epsilon=1e-6;
        for(int i=0;i<tests;i++){
            TV base_offset=random.template Direction<TV>();
            TV spin=random.template Direction<TV>();
            TV rotated_offset=ROTATION<TV>::From_Rotation_Vector(spin)*base_offset;
            M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dRotatedOffset_dSpin(spin,base_offset);
            TV delta=random.template Direction<TV>();

            auto testlambda=[&](T eps){
                T_SPIN dspin=eps*delta;
                TV predicted=derivative*dspin;
                TV final=ROTATION<TV>::From_Rotation_Vector(spin+dspin)*base_offset;
                T error=(final-rotated_offset-predicted).norm();
                return error;
            };
            T divisor=2;
            T ratio=testlambda(epsilon)/testlambda(epsilon/divisor);
            //std::cout<<"Ratio: "<<ratio<<std::endl;
            REQUIRE(fabs(ratio-sqr(divisor))<0.1);
        }
    }
    SECTION("dForce_dSpin"){
        for(int i=0;i<tests;i++){
            std::vector<TV> positions(2);
            std::vector<TV> base_offsets(2);
            std::vector<TV> spins(2);
            std::vector<ROTATION<TV>> rotations(2);
            for(int j=0;j<2;j++){
                positions[j]=random.template Direction<TV>();
                base_offsets[j]=random.template Direction<TV>();
                spins[j]=random.template Direction<TV>();
                rotations[j]=ROTATION<TV>::From_Rotation_Vector(spins[j]);
            }
            TV relative_position=positions[1]+rotations[1]*base_offsets[1]-(positions[0]+rotations[0]*base_offsets[0]);
            TV direction=relative_position.normalized();
            T epsilon=1e-8;
            for(int s1=0;s1<2;s1++){
                int overall_sign=s1==0?-1:1;
                for(int s2=0;s2<2;s2++){
                    int term_sign=s1==s2?1:-1;
                    M_VxV derivative=term_sign*RIGID_STRUCTURE_INDEX_MAP<TV>::dForce_dSpin(relative_position,spins[s2],base_offsets[s2]);
                    TV delta=epsilon*random.template Direction<TV>();
                    TV estimated_direction=overall_sign*(direction)+derivative*delta;
                    std::vector<TV> mod_spins(2);
                    for(int j=0;j<2;j++){mod_spins[j]=spins[j];}
                    mod_spins[s2]+=delta;
                    TV actual_direction=overall_sign*(positions[1]+ROTATION<TV>::From_Rotation_Vector(mod_spins[1])*base_offsets[1]-(positions[0]+ROTATION<TV>::From_Rotation_Vector(mod_spins[0])*base_offsets[0])).normalized();
                    REQUIRE((actual_direction-estimated_direction).norm()<2*delta.norm());}}}}

    SECTION("Penalty force","dPenaltyForce_dVelocity"){
        for(int i=0;i<tests;i++){
            std::vector<TV> positions(2);
            std::vector<TV> base_offsets(2);
            std::vector<TV> spins(2);
            std::vector<ROTATION<TV>> rotations(2);
            for(int j=0;j<2;j++){
                positions[j]=random.template Direction<TV>();
                base_offsets[j]=random.template Direction<TV>();
                spins[j]=random.template Direction<TV>();
                rotations[j]=ROTATION<TV>::From_Rotation_Vector(spins[j]);}
            T threshold=(positions[1]-positions[0]).norm()+random.Uniform((T)0,(T).1);
            //TV relative_position=positions[1]+rotations[1]*base_offsets[1]-(positions[0]+rotations[0]*base_offsets[0]);
            TV relative_position=positions[1]-positions[0];
            TV direction=relative_position.normalized();
            T epsilon=1e-8;
            TV force=sqr(relative_position.norm()-threshold)*relative_position.normalized();
            M_VxV derivative=RIGID_STRUCTURE_INDEX_MAP<TV>::dPenaltyForce_dVelocity(relative_position,threshold);
            TV delta=epsilon*random.template Direction<TV>();
            TV estimated_force=force+derivative*delta;
            TV new_relative=(positions[1]+delta-positions[0]);
            TV actual_force=sqr(new_relative.norm()-threshold)*new_relative.normalized();
            //std::cout<<"Quality: "<<(actual_force-estimated_force).norm()/delta.norm()<<std::endl;
            REQUIRE((actual_force-estimated_force).norm()<delta.norm());
        }
    }

    SECTION("Penalty force","dPenaltyTorque_dSpin"){
        for(int i=0;i<tests;i++){
            std::vector<TV> positions(2);
            std::vector<TV> base_offsets(2);
            std::vector<TV> spins(2);
            std::vector<ROTATION<TV>> rotations(2);
            for(int j=0;j<2;j++){
                positions[j]=random.template Direction<TV>();
                base_offsets[j]=random.template Direction<TV>();
                spins[j]=random.template Direction<TV>();
                rotations[j]=ROTATION<TV>::From_Rotation_Vector(spins[j]);
            }
            TV relative_position=positions[1]+rotations[1]*base_offsets[1]-(positions[0]+rotations[0]*base_offsets[0]);
            T threshold=relative_position.norm()+random.Uniform((T)0,(T).1);
            std::vector<TV> rotated_offsets={rotations[0]*base_offsets[0],rotations[1]*base_offsets[1]};
            T epsilon=1e-8;
            for(int s1=0;s1<2;s1++){
                int overall_sign=s1==0?-1:1;
                for(int s2=0;s2<2;s2++){
                    int term_sign=s1==s2?1:-1;
                    M_VxV derivative=term_sign*RIGID_STRUCTURE_INDEX_MAP<TV>::dPenaltyTorque_dSpin(relative_position,s1,s2,spins[s2],base_offsets[s1],base_offsets[s2],threshold);
                    TV delta=epsilon*random.template Direction<TV>();

                    TV torque=overall_sign*(ROTATION<TV>::From_Rotation_Vector(spins[s1])*base_offsets[s1]).cross(sqr(relative_position.norm()-threshold)*relative_position.normalized());

                    TV estimated_torque=torque+derivative*delta;
                    std::vector<TV> mod_spins(2);
                    for(int j=0;j<2;j++){mod_spins[j]=spins[j];}
                    mod_spins[s2]+=delta;
                    TV new_relative_position=positions[1]+ROTATION<TV>::From_Rotation_Vector(mod_spins[1])*base_offsets[1]-(positions[0]+ROTATION<TV>::From_Rotation_Vector(mod_spins[0])*base_offsets[0]);
                    TV actual_torque=overall_sign*(ROTATION<TV>::From_Rotation_Vector(mod_spins[s1])*base_offsets[s1]).cross(sqr(new_relative_position.norm()-threshold)*new_relative_position.normalized());
                    //std::cout<<"Quality: "<<(actual_torque-estimated_torque).norm()/delta.norm()<<std::endl;
                    REQUIRE((actual_torque-estimated_torque).norm()<delta.norm());

                }
            }
        }
    }
}


#ifndef STAN_MATH_FWD_FUN_TGAMMA_HPP
#define STAN_MATH_FWD_FUN_TGAMMA_HPP

#include <stan/math/fwd/meta.hpp>
#include <stan/math/fwd/core.hpp>
#include <stan/math/prim/scal/fun/tgamma.hpp>
#include <boost/math/special_functions/digamma.hpp>

namespace stan {
namespace math {

/**
 * Return the result of applying the gamma function to the
 * specified argument.
 *
 * @tparam T inner type of the fvar
 * @param x Argument.
 * @return Gamma function applied to argument.
 */
template <typename T>
inline fvar<T> tgamma(const fvar<T>& x) {
  T u = tgamma(x.val_);
  return fvar<T>(u, x.d_ * u * digamma(x.val_));
}

}  // namespace math
}  // namespace stan
#endif


#include <tiny.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(convex_signed_distance_to_triangle);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>        math_types;
  typedef math_types::vector3_type      vector3_type;
  typedef math_types::real_type         real_type;

  vector3_type a = vector3_type::make(0.0, 0.0, 0.0);
  vector3_type b = vector3_type::make(1.0, 0.0, 0.0);;
  vector3_type c = vector3_type::make(0.0, 1.0, 0.0);;
  vector3_type q = vector3_type::make(0.33, 0.33, -1.0);

  {
    vector3_type p = vector3_type::make(0.33, 0.33,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_triangle(p, a, b, c, q );
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }

  {
    vector3_type p = vector3_type::make(0.1, 0.1,  -1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_triangle(p, a, b, c, q );
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }

  {
    vector3_type p = vector3_type::make(0.1, 0.1,  0.0);
    real_type sign_p = 10.0;
    sign_p = convex::signed_distance_to_triangle(p, a, b, c, q );
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


//-*-c++-*-
//=======================================================================
// Copyright 1997-2001 University of Notre Dame.
// Authors: Lie-Quan Lee
//
// 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)
//=======================================================================

/*
  This file is to demo how to use minimum_degree_ordering algorithm.

  Important Note: This implementation requires the BGL graph to be
  directed.  Therefore, nonzero entry (i, j) in a symmetrical matrix
  A coresponds to two directed edges (i->j and j->i).

  The bcsstk01.rsa is an example graph in Harwell-Boeing format,
  and bcsstk01 is the ordering produced by Liu's MMD implementation.
  Link this file with iohb.c to get the harwell-boeing I/O functions.
  To run this example, type:

  ./minimum_degree_ordering bcsstk01.rsa bcsstk01

*/

#include <boost/config.hpp>
#include <fstream>
#include <iostream>
#include "boost/graph/adjacency_list.hpp"
#include "boost/graph/graph_utility.hpp"
#include "boost/graph/minimum_degree_ordering.hpp"
#include "iohb.h"

//copy and modify from mtl harwell boeing stream
struct harwell_boeing
{
  harwell_boeing(char* filename) {
    int Nrhs;
    char* Type;
    Type = new char[4];
    isComplex = false;
    readHB_info(filename, &M, &N, &nonzeros, &Type, &Nrhs);
    colptr = (int *)malloc((N+1)*sizeof(int));
    if ( colptr == NULL ) IOHBTerminate("Insufficient memory for colptr.\n");
    rowind = (int *)malloc(nonzeros*sizeof(int));
    if ( rowind == NULL ) IOHBTerminate("Insufficient memory for rowind.\n");

    if ( Type[0] == 'C' ) {
      isComplex = true;
      val = (double *)malloc(nonzeros*sizeof(double)*2);
      if ( val == NULL ) IOHBTerminate("Insufficient memory for val.\n");

    } else {
      if ( Type[0] != 'P' ) {
        val = (double *)malloc(nonzeros*sizeof(double));
        if ( val == NULL ) IOHBTerminate("Insufficient memory for val.\n");
      }
    }

    readHB_mat_double(filename, colptr, rowind, val);

    cnt = 0;
    col = 0;
    delete [] Type;
  }

  ~harwell_boeing() {
    free(colptr);
    free(rowind);
    free(val);
  }

  inline int nrows() const { return M; }

  int cnt;
  int col;
  int* colptr;
  bool isComplex;
  int M;
  int N;
  int nonzeros;
  int* rowind;
  double* val;
};

int main(int argc, char* argv[])
{
  using namespace std;
  using namespace boost;

  if (argc < 2) {
    cout << argv[0] << " HB file"  << endl;
    return -1;
  }

  int delta = 0;

  if ( argc >= 4 )
  delta = atoi(argv[3]);

  harwell_boeing hbs(argv[1]);

  //must be BGL directed graph now
  typedef adjacency_list<vecS, vecS, directedS>  Graph;

  int n = hbs.nrows();

  cout << "n is " << n << endl;

  Graph G(n);

  int num_edge = 0;

  for (int i = 0; i < n; ++i)
    for (int j = hbs.colptr[i]; j < hbs.colptr[i+1]; ++j)
      if ( (hbs.rowind[j - 1] - 1 ) > i ) {
        add_edge(hbs.rowind[j - 1] - 1, i, G);
        add_edge(i, hbs.rowind[j - 1] - 1, G);
        num_edge++;
      }

  cout << "number of off-diagnal elements: " << num_edge << endl;

  typedef std::vector<int> Vector;

  Vector inverse_perm(n, 0);
  Vector perm(n, 0);

  Vector supernode_sizes(n, 1); // init has to be 1

  boost::property_map<Graph, vertex_index_t>::type
    id = get(vertex_index, G);

  Vector degree(n, 0);

  minimum_degree_ordering
    (G,
     make_iterator_property_map(&degree[0], id, degree[0]),
     &inverse_perm[0],
     &perm[0],
     make_iterator_property_map(&supernode_sizes[0], id, supernode_sizes[0]),
     delta, id);

  if ( argc >= 3 ) {
    ifstream  input(argv[2]);
    if ( input.fail() ) {
      cout << argv[3] << " is failed to open!. " << endl;
      return -1;
    }
    int comp;
    bool is_correct = true;
    int i;
    for ( i=0; i<n; i++ ) {
      input >> comp;
      if ( comp != inverse_perm[i]+1 ) {
        cout << "at i= " << i << ": " << comp
             << " ***is NOT EQUAL to*** " << inverse_perm[i]+1 << endl;
        is_correct = false;
      }
    }
    for ( i=0; i<n; i++ ) {
      input >> comp;
      if ( comp != perm[i]+1 ) {
        cout << "at i= " << i << ": " << comp
             << " ***is NOT EQUAL to*** " << perm[i]+1 << endl;
        is_correct = false;
      }
    }
    if ( is_correct )
      cout << "Permutation and inverse permutation are correct. "<< endl;
    else
      cout << "WARNING -- Permutation or inverse permutation is not the "
           << "same ones generated by Liu's " << endl;

  }
  return 0;
}


#define BOOST_TEST_MAIN TestIdGenerator
#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include <string>
#include <sam/IdGenerator.hpp>
#include <atomic>
#include <thread>

using namespace sam;

BOOST_AUTO_TEST_CASE( test_simple_id_generator )
{

  SimpleIdGenerator* idGenerator = idGenerator->getInstance();

  int numThreads = 10000;
  int numTimes = 10000; // How many times each thread requests an id
  std::atomic<std::uint32_t> sum(0);

  std::vector<std::thread> threads;

  for(int i = 0; i < numThreads; i++) {
    threads.push_back(std::thread([&idGenerator, &sum, numTimes]() {
      for (int i = 0; i < numTimes; i++) {
        sum.fetch_add(idGenerator->generate());
      }
    }));
  }

  for (int i = 0; i < numThreads; i++) {
    threads[i].join();
  }

  uint32_t n = numThreads * numTimes;
  uint32_t expected = n * (n - 1) / 2;
  BOOST_CHECK_EQUAL(expected, sum);
}