// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_CODATA_MUON_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_MUON_CONSTANTS_HPP

#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/magnetic_flux_density.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/wavenumber.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

/// \file
/// CODATA recommended values of fundamental atomic and nuclear constants
/// CODATA 2006 values as of 2007/03/30

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

/// CODATA recommended values of the fundamental physical constants: NIST SP 961

/// muon mass
BOOST_UNITS_PHYSICAL_CONSTANT(m_mu,quantity<mass>,1.88353130e-28*kilograms,1.1e-35*kilograms);
/// muon-electron mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_mu_over_m_e,quantity<dimensionless>,206.7682823*dimensionless(),5.2e-6*dimensionless());
/// muon-tau mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_mu_over_m_tau,quantity<dimensionless>,5.94592e-2*dimensionless(),9.7e-6*dimensionless());
/// muon-proton mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_mu_over_m_p,quantity<dimensionless>,0.1126095261*dimensionless(),2.9e-9*dimensionless());
/// muon-neutron mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_mu_over_m_n,quantity<dimensionless>,0.1124545167*dimensionless(),2.9e-9*dimensionless());
/// muon molar mass
BOOST_UNITS_PHYSICAL_CONSTANT(M_mu,quantity<mass_over_amount>,0.1134289256e-3*kilograms/mole,2.9e-12*kilograms/mole);
/// muon Compton wavelength
BOOST_UNITS_PHYSICAL_CONSTANT(lambda_C_mu,quantity<length>,11.73444104e-15*meters,3.0e-22*meters);
/// muon magnetic moment
BOOST_UNITS_PHYSICAL_CONSTANT(mu_mu,quantity<energy_over_magnetic_flux_density>,-4.49044786e-26*joules/tesla,1.6e-33*joules/tesla);
/// muon-Bohr magneton ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_mu_over_mu_B,quantity<dimensionless>,-4.84197049e-3*dimensionless(),1.2e-10*dimensionless());
/// muon-nuclear magneton ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_mu_over_mu_N,quantity<dimensionless>,-8.89059705*dimensionless(),2.3e-7*dimensionless());
/// muon magnetic moment anomaly
BOOST_UNITS_PHYSICAL_CONSTANT(a_mu,quantity<dimensionless>,1.16592069e-3*dimensionless(),6.0e-10*dimensionless());
/// muon g-factor
BOOST_UNITS_PHYSICAL_CONSTANT(g_mu,quantity<dimensionless>,-2.0023318414*dimensionless(),1.2e-9*dimensionless());
/// muon-proton magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_mu_over_mu_p,quantity<dimensionless>,-3.183345137*dimensionless(),8.5e-8*dimensionless());

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_MUON_CONSTANTS_HPP


/*    Copyright (c) 2010-2016, 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.
 */

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

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamicCoefficientInterface.h"
#include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h"
#include "Tudat/SimulationSetup/createFlightConditions.h"

namespace tudat
{

namespace simulation_setup
{

//! Function to create an aerodynamic coefficient interface containing constant coefficients.
boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createConstantCoefficientAerodynamicCoefficientInterface(
        const Eigen::Vector3d constantForceCoefficient,
        const Eigen::Vector3d constantMomentCoefficient,
        const double referenceLength,
        const double referenceArea,
        const double lateralReferenceLength,
        const Eigen::Vector3d& momentReferencePoint,
        const bool areCoefficientsInAerodynamicFrame,
        const bool areCoefficientsInNegativeAxisDirection  )
{
    // Create coefficient interface
    boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > coefficientInterface =
            boost::make_shared< aerodynamics::CustomAerodynamicCoefficientInterface >(
                boost::lambda::constant( constantForceCoefficient ),
                boost::lambda::constant( constantMomentCoefficient ),
                referenceLength, referenceArea, lateralReferenceLength, momentReferencePoint,
                std::vector< aerodynamics::AerodynamicCoefficientsIndependentVariables >( ),
                areCoefficientsInAerodynamicFrame, areCoefficientsInNegativeAxisDirection );
    coefficientInterface->updateCurrentCoefficients( std::vector< double >( ) );

    return coefficientInterface;
}

//! Function to create and aerodynamic coefficient interface.
boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface >
createAerodynamicCoefficientInterface(
        const boost::shared_ptr< AerodynamicCoefficientSettings > coefficientSettings,
        const std::string& body )
{
    using namespace tudat::aerodynamics;

    boost::shared_ptr< AerodynamicCoefficientInterface > coefficientInterface;

    // Check type of interface that is to be created.
    switch( coefficientSettings->getAerodynamicCoefficientType( ) )
    {
    case constant_aerodynamic_coefficients:
    {
        // Check consistency of type.
        boost::shared_ptr< ConstantAerodynamicCoefficientSettings > constantCoefficientSettings =
                boost::dynamic_pointer_cast< ConstantAerodynamicCoefficientSettings >(
                    coefficientSettings );
        if( constantCoefficientSettings == NULL )
        {
            throw std::runtime_error(
                        "Error, expected constant aerodynamic coefficients for body " + body );
        }
        else
        {
            // create constant interface.
            coefficientInterface = createConstantCoefficientAerodynamicCoefficientInterface(
                        constantCoefficientSettings->getConstantForceCoefficient( ),
                        constantCoefficientSettings->getConstantMomentCoefficient( ),
                        constantCoefficientSettings->getReferenceLength( ),
                        constantCoefficientSettings->getReferenceArea( ),
                        constantCoefficientSettings->getReferenceLength( ),
                        constantCoefficientSettings->getMomentReferencePoint( ),
                        constantCoefficientSettings->getAreCoefficientsInAerodynamicFrame( ),
                        constantCoefficientSettings->getAreCoefficientsInNegativeAxisDirection( ) );
        }
        break;
    }
    case tabulated_coefficients:
    {
        // Check number of dimensions of tabulated coefficients.
        int numberOfDimensions = coefficientSettings->getIndependentVariableNames( ).size( );
        switch( numberOfDimensions )
        {
        case 1:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 1 >(
                        coefficientSettings, body );
            break;
        }
        case 2:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 2 >(
                        coefficientSettings, body );
            break;
        }
        case 3:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 3 >(
                        coefficientSettings, body );
            break;
        }
        case 4:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 4 >(
                        coefficientSettings, body );
            break;
        }
        case 5:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 5 >(
                        coefficientSettings, body );
            break;
        }
        case 6:
        {
            coefficientInterface = createTabulatedCoefficientAerodynamicCoefficientInterface< 6 >(
                        coefficientSettings, body );
            break;
        }
        default:
            throw std::runtime_error( "Error when making tabulated aerodynamic coefficient interface, " +
                       boost::lexical_cast< std::string >( numberOfDimensions ) + " dimensions not yet implemented" );
        }

    }
    default:
        throw std::runtime_error( "Error, do not recognize aerodynamic coefficient settings for " + body );
    }
    return coefficientInterface;
}

//! Function to create a flight conditions object
boost::shared_ptr< aerodynamics::FlightConditions > createFlightConditions(
        const boost::shared_ptr< Body > bodyWithFlightConditions,
        const boost::shared_ptr< Body > centralBody,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration,
        const boost::function< double( ) > angleOfAttackFunction,
        const boost::function< double( ) > angleOfSideslipFunction,
        const boost::function< double( ) > bankAngleFunction )
{
    // Check whether all required environment models are set.
    if( centralBody->getAtmosphereModel( ) == NULL )
    {
        throw std::runtime_error(
                    "Error when making flight conditions, body " + nameOfBodyExertingAcceleration +
                    " has no atmosphere model." );
    }

    if( centralBody->getShapeModel( ) == NULL )
    {
        throw std::runtime_error(
                    "Error when making flight conditions, body " + nameOfBodyExertingAcceleration +
                    " has no shape model." );
    }

    if( centralBody->getRotationalEphemeris( ) == NULL )
    {
        throw std::runtime_error(
                    "Error when making flight conditions, body " + nameOfBodyExertingAcceleration +
                    " has no rotation model." );
    }

    if( bodyWithFlightConditions->getAerodynamicCoefficientInterface( ) == NULL )
    {
        throw std::runtime_error(
                    "Error when making flight conditions, body "+ nameOfBodyUndergoingAcceleration +
                    " has no aerodynamic coefficients." );
    }

    // Create function to calculate the altitude from current body-fixed state
    boost::function< double( const Eigen::Vector3d ) > altitudeFunction =
            boost::bind( &basic_astrodynamics::BodyShapeModel::getAltitude,
                         centralBody->getShapeModel( ), _1 );

    // Create function to rotate state from intertial to body-fixed frame.
    boost::function< Eigen::Quaterniond( ) > rotationToFrameFunction =
            boost::bind( &Body::getCurrentRotationToLocalFrame, centralBody );
    boost::function< Eigen::Matrix3d( ) > rotationMatrixToFrameDerivativeFunction =
            boost::bind( &Body::getCurrentRotationMatrixDerivativeToLocalFrame, centralBody );
    boost::function< basic_mathematics::Vector6d( const basic_mathematics::Vector6d& ) >
            transformationToCentralBodyFrame =
            boost::bind(
                static_cast< basic_mathematics::Vector6d(&)(
                    const basic_mathematics::Vector6d&,
                    const boost::function< Eigen::Quaterniond( ) >,
                    const boost::function< Eigen::Matrix3d( ) > ) >(
                    &ephemerides::transformStateToFrame ),
                _1, rotationToFrameFunction,
                rotationMatrixToFrameDerivativeFunction );

    // Create flight conditions.
    boost::shared_ptr< aerodynamics::FlightConditions > flightConditions =
            boost::make_shared< aerodynamics::FlightConditions >(
                centralBody->getAtmosphereModel( ), altitudeFunction,
                boost::bind( &Body::getState, bodyWithFlightConditions ),
                boost::bind( &Body::getState, centralBody ),
                transformationToCentralBodyFrame,
                bodyWithFlightConditions->getAerodynamicCoefficientInterface( ) );

    // Create aerodynamic angles calculator and set in flight conditions.
    boost::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator =
            boost::make_shared< reference_frames::AerodynamicAngleCalculator >(
                boost::bind( &aerodynamics::FlightConditions::getCurrentBodyCenteredBodyFixedState,
                             flightConditions ),
                angleOfAttackFunction, angleOfSideslipFunction, bankAngleFunction, 1 );
    flightConditions->setAerodynamicAngleCalculator( aerodynamicAngleCalculator );

    return flightConditions;

}

} // namespace simulation_setup

} // namespace tudat


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_REVERSER_HPP
#define CRYPTO3_DETAIL_REVERSER_HPP

#include <boost/integer.hpp>
#include <boost/static_assert.hpp>
#include <boost/endian/conversion.hpp>

#include <nil/crypto3/detail/unbounded_shift.hpp>
#include <nil/crypto3/detail/stream_endian.hpp>
#include <nil/crypto3/detail/predef.hpp>

#include <climits>

namespace nil {
    namespace crypto3 {
        namespace detail {

            /*!
             * @defgroup reverser Reverser functions
             */

            typedef typename boost::uint_t<CHAR_BIT>::exact byte_type;

            /*!
             * @brief This function reverses bit order in the byte b depending on the machine word size.
             * The underlying algorithms used in this function are described in
             * http://graphics.stanford.edu/~seander/bithacks.html#ReverseByteWith32Bits and in
             * http://graphics.stanford.edu/~seander/bithacks.html#ReverseByteWith64BitsDiv .
             *
             * @ingroup reverser
             *
             * @param b
             *
             * @return
             */
            inline void reverse_byte(byte_type &b) {

#if (BOOST_ARCH_CURRENT_WORD_BITS == 32)
                b = unbounded_shr<16>(((b * 0x0802LU & 0x22110LU) | (b * 0x8020LU & 0x88440LU)) * 0x10101LU);
#elif (BOOST_ARCH_CURRENT_WORD_BITS == 64)
                b = (b * 0x0202020202ULL & 0x010884422010ULL) % 1023;
#else
#error "BOOST_ARCH_CURRENT_WORD_BITS not set"
#endif
            }

            /*!
             * @brief bit_in_unit_byte_reverser transforms the sequence of bits in each byte of
             * the input unit into reversed sequence of bits in each byte of the output unit.
             * The function reverse is recursively invoked and the parameter k is used to track
             * the number of already processed input bytes. The recursion ends, when all input
             * bytes have been processed, i.e. when k == UnitBits.
             *
             * @ingroup reverser
             *
             * @tparam UnitBits
             * @tparam k
             */
            template<int UnitBits, int k = 0>
            struct bit_in_unit_byte_reverser {

                BOOST_STATIC_ASSERT(!(UnitBits % CHAR_BIT));

                typedef bit_in_unit_byte_reverser<UnitBits, k + CHAR_BIT> next_type;
                typedef typename boost::uint_t<UnitBits>::exact UnitType;

                inline static void reverse(UnitType &in, UnitType &out) {
                    int const shift = UnitBits - (CHAR_BIT + k);
                    byte_type byte = byte_type(low_bits<CHAR_BIT>(unbounded_shr(in, shift)));
                    reverse_byte(byte);
                    out |= unbounded_shl(low_bits<CHAR_BIT>(UnitType(byte)), shift);

                    next_type::reverse(in, out);
                }
            };

            template<int UnitBits>
            struct bit_in_unit_byte_reverser<UnitBits, UnitBits> {
                inline static void reverse(typename boost::uint_t<UnitBits>::exact &,
                                           typename boost::uint_t<UnitBits>::exact &) {
                }
            };

            /*!
             * @brief The functions listed below deal with bit reversal in a unit.
             */

            /*!
             * @brief This function deals with the case of UnitBits > CHAR_BIT. To reverse
             * the order of bits, first, it reverses the byte order of unit, and then, it
             * invokes bit_in_unit_byte_reverser to reverse bits in each byte of unit.
             *
             * @ingroup reverser
             *
             * @tparam UnitType
             * @tparam UnitBits
             *
             * @param unit
             *
             * @return
             */
            template<typename UnitType,
                     int UnitBits = sizeof(UnitType) * CHAR_BIT,
                     typename std::enable_if<(UnitBits > CHAR_BIT), int>::type = 0>
            inline void reverse_bits(UnitType &unit) {
                boost::endian::endian_reverse_inplace(unit);
                UnitType out = UnitType();
                bit_in_unit_byte_reverser<UnitBits>::reverse(unit, out);
                unit = out;
            }
            /*!
             * @brief This function deals with the special case of UnitBits == CHAR_BIT,
             * it just reverses the bit order in the byte.
             * @ingroup reverser
             *
             * @tparam UnitType
             * @tparam UnitBits
             *
             * @param unit
             *
             * @return
             */
            template<typename UnitType,
                     int UnitBits = sizeof(UnitType) * CHAR_BIT,
                     typename std::enable_if<(UnitBits == CHAR_BIT), int>::type = 0>
            inline void reverse_bits(UnitType &unit) {
                reverse_byte(unit);
            }

            /*!
             * @brief bit_in_unit_reverser transforms the sequence of bits in each unit of
             * the input value into reversed sequence of bytes in each unit of the output value.
             * The function reverse is recursively invoked and the parameter k is used to track
             * the number of already processed input units. The recursion ends, when all input
             * units have been processed, i.e. when k == InputBits.
             *
             * @ingroup reverser
             *
             * @tparam InputBits
             * @tparam UnitBits
             * @tparam k
             */
            template<int InputBits, int UnitBits, int k = 0>
            struct bit_in_unit_reverser {

                BOOST_STATIC_ASSERT(!(InputBits % UnitBits) && !(UnitBits % CHAR_BIT));

                typedef bit_in_unit_reverser<InputBits, UnitBits, k + UnitBits> next_type;
                typedef typename boost::uint_t<UnitBits>::exact UnitType;

                template<typename ValueType>
                inline static void reverse(ValueType &in, ValueType &out) {
                    int const shift = InputBits - (UnitBits + k);
                    UnitType unit = UnitType(low_bits<UnitBits>(unbounded_shr(in, shift)));
                    reverse_bits(unit);
                    out |= unbounded_shl(low_bits<UnitBits>(ValueType(unit)), shift);

                    next_type::reverse(in, out);
                }
            };

            template<int InputBits, int UnitBits>
            struct bit_in_unit_reverser<InputBits, UnitBits, InputBits> {
                template<typename ValueType>
                inline static void reverse(ValueType &, ValueType &) {
                }
            };

            /*!
             * @brief The group of traits below is used to determine the order of bits defined
             * by the endianness.
             */

            /*!
             * @brief Trait to determine whether the order of bits defined by Endianness endianness
             * is big.
             *
             * @ingroup reverser
             *
             * @tparam Endianness
             * @tparam UnitBits
             */
            template<typename Endianness, int UnitBits>
            struct is_big_bit {
                constexpr static const bool value =
                    std::is_same<Endianness, stream_endian::big_unit_big_bit<UnitBits>>::value ||
                    std::is_same<Endianness, stream_endian::little_unit_big_bit<UnitBits>>::value;
            };

            /*!
             * @brief Trait to determine whether the order of bits defined by Endianness endianness
             * is little.
             *
             * @ingroup reverser
             *
             * @tparam Endianness
             * @tparam UnitBits
             */
            template<typename Endianness, int UnitBits>
            struct is_little_bit {
                constexpr static const bool value =
                    std::is_same<Endianness, stream_endian::big_unit_little_bit<UnitBits>>::value ||
                    std::is_same<Endianness, stream_endian::little_unit_little_bit<UnitBits>>::value;
            };

            /*!
             * @brief Trait to determine whether the orders of bits defined by Endianness1 endianness
             * and Endianness2 endianness are the same.
             *
             * @ingroup reverser
             *
             * @tparam Endianness1
             * @tparam Endianness2
             * @tparam UnitBits
             */
            template<typename Endianness1, typename Endianness2, int UnitBits>
            struct is_same_bit {
                constexpr static const bool value =
                    (is_big_bit<Endianness1, UnitBits>::value && is_big_bit<Endianness2, UnitBits>::value) ||
                    (is_little_bit<Endianness1, UnitBits>::value && is_little_bit<Endianness2, UnitBits>::value);
            };

            /*!
             * @brief bit_reverser reverses the sequence of bits in each unit of the given value,
             * if InputEndianness and OutputEndianness endiannesses have different bit orders, and
             * does nothing, otherwise.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam IsSameBit
             */
            template<typename InputEndianness,
                     typename OutputEndianness,
                     int UnitBits,
                     bool IsSameBit = is_same_bit<InputEndianness, OutputEndianness, UnitBits>::value>
            struct bit_reverser;

            /*!
             * @brief This bit_reverser is a dummy and deals with the case of the endiannesses with
             * the same order of bits.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits>
            struct bit_reverser<InputEndianness, OutputEndianness, UnitBits, true> {
                template<typename ValueType>
                inline static void reverse(ValueType &) {
                }

                template<typename ValueType>
                inline static ValueType reverse(ValueType const &val) {
                    return val;
                }
            };

            /*!
             * @brief This bit_reverser deals with the case of the endiannesses with different order of
             * bits and invokes bit_in_unit_reverser which reverses bits in each unit of the input value.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits>
            struct bit_reverser<InputEndianness, OutputEndianness, UnitBits, false> {
                template<typename ValueType, int ValueBits = sizeof(ValueType) * CHAR_BIT>
                inline static void reverse(ValueType &val) {
                    ValueType out = ValueType();
                    bit_in_unit_reverser<ValueBits, UnitBits>::reverse(val, out);
                    val = out;
                }

                template<typename ValueType, int ValueBits = sizeof(ValueType) * CHAR_BIT>
                inline static ValueType reverse(ValueType const &val) {
                    ValueType tmp = val;
                    ValueType out = ValueType();
                    bit_in_unit_reverser<ValueBits, UnitBits>::reverse(tmp, out);
                    return out;
                }
            };

            /*!
             * @brief byte_in_unit_reverser transforms the sequence of bytes in each unit of
             * the input value into reversed sequence of bytes in each unit of the output value.
             * The function reverse is recursively invoked and the parameter k is used to track
             * the number of already processed input units. The recursion ends, when all input
             * units have been processed, i.e. when k == InputBits.
             *
             * @ingroup reverser
             *
             * @tparam InputBits
             * @tparam UnitBits
             * @tparam k
             */
            template<int InputBits, int UnitBits, int k = 0>
            struct byte_in_unit_reverser {

                BOOST_STATIC_ASSERT(!(InputBits % UnitBits) && !(UnitBits % CHAR_BIT));

                typedef byte_in_unit_reverser<InputBits, UnitBits, k + UnitBits> next_type;
                typedef typename boost::uint_t<UnitBits>::exact UnitType;

                template<typename ValueType>
                inline static void reverse(ValueType &in, ValueType &out) {
                    int const shift = InputBits - (UnitBits + k);
                    UnitType unit = UnitType(low_bits<UnitBits>(unbounded_shr(in, shift)));
                    boost::endian::endian_reverse_inplace(unit);
                    out |= unbounded_shl(low_bits<UnitBits>(ValueType(unit)), shift);

                    next_type::reverse(in, out);
                }
            };

            template<int InputBits, int UnitBits>
            struct byte_in_unit_reverser<InputBits, UnitBits, InputBits> {
                template<typename ValueType>
                inline static void reverse(ValueType &, ValueType &) {
                }
            };

            /*!
             * @brief The group of traits below is used to determine the order of units defined
             * by the endianness.
             */

            /*!
             * @brief Trait to determine whether the order of units defined by Endianness endianness
             * is big.
             *
             * @ingroup reverser
             *
             * @tparam Endianness
             * @tparam UnitBits
             */
            template<typename Endianness, int UnitBits>
            struct is_big_unit {
                constexpr static const bool value =
                    std::is_same<Endianness, stream_endian::big_unit_big_bit<UnitBits>>::value ||
                    std::is_same<Endianness, stream_endian::big_unit_little_bit<UnitBits>>::value;
            };

            /*!
             * @brief Trait to determine whether the order of units defined by Endianness endianness
             * is little.
             *
             * @ingroup reverser
             *
             * @tparam Endianness
             * @tparam UnitBits
             */
            template<typename Endianness, int UnitBits>
            struct is_little_unit {
                constexpr static const bool value =
                    std::is_same<Endianness, stream_endian::little_unit_big_bit<UnitBits>>::value ||
                    std::is_same<Endianness, stream_endian::little_unit_little_bit<UnitBits>>::value;
            };

            /*!
             * @brief Trait to determine whether the orders of units defined by Endianness1 endianness
             * and Endianness2 endianness are the same.
             *
             * @ingroup reverser
             *
             * @tparam Endianness1
             * @tparam Endianness2
             * @tparam UnitBits
             */
            template<typename Endianness1, typename Endianness2, int UnitBits>
            struct is_same_unit {
                constexpr static const bool value =
                    (is_big_unit<Endianness1, UnitBits>::value && is_big_unit<Endianness2, UnitBits>::value) ||
                    (is_little_unit<Endianness1, UnitBits>::value && is_little_unit<Endianness2, UnitBits>::value);
            };

            /*!
             * @brief unit_reverser reverses the sequence of units in the given value, if InputEndianness
             * and OutputEndianness endiannesses have different unit orders, and does nothing, otherwise.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             * @tparam Enable
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits, typename Enable = void>
            struct unit_reverser;

            /*!
             * @brief This unit_reverser is a dummy and deals with the case of the endiannesses with
             * the same order of units.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits>
            struct unit_reverser<
                InputEndianness,
                OutputEndianness,
                UnitBits,
                typename std::enable_if<is_same_unit<InputEndianness, OutputEndianness, UnitBits>::value>::type> {
                template<typename ValueType>
                inline static void reverse(ValueType &) {
                }

                template<typename ValueType>
                inline static ValueType reverse(ValueType const &val) {
                    return val;
                }
            };

            /*!
             * @brief This unit_reverser deals with the case of UnitBits == CHAR_BIT. This case is
             * special since it is sufficient to reverse the order of bytes in an input value.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits>
            struct unit_reverser<
                InputEndianness,
                OutputEndianness,
                UnitBits,
                typename std::enable_if<!is_same_unit<InputEndianness, OutputEndianness, UnitBits>::value &&
                                        UnitBits == CHAR_BIT>::type> {
                template<typename ValueType>
                inline static void reverse(ValueType &val) {
                    boost::endian::endian_reverse_inplace(val);
                }

                template<typename ValueType>
                inline static ValueType reverse(ValueType const &val) {
                    return boost::endian::endian_reverse(val);
                }
            };

            /*!
             * @brief This unit_reverser deals with the case of UnitBits > CHAR_BIT. To reverse the
             * order of units, first, it reverses the byte order in an input value, and then, it
             * invokes byte_in_unit_reverser to reverse the byte order in each unit of the input value.
             *
             * @ingroup reverser
             *
             * @tparam InputEndianness
             * @tparam OutputEndianness
             * @tparam UnitBits
             */
            template<typename InputEndianness, typename OutputEndianness, int UnitBits>
            struct unit_reverser<
                InputEndianness,
                OutputEndianness,
                UnitBits,
                typename std::enable_if<!is_same_unit<InputEndianness, OutputEndianness, UnitBits>::value &&
                                        (UnitBits > CHAR_BIT)>::type> {
                template<typename ValueType, int ValueBits = sizeof(ValueType) * CHAR_BIT>
                inline static void reverse(ValueType &val) {
                    boost::endian::endian_reverse_inplace(val);
                    ValueType out = ValueType();
                    byte_in_unit_reverser<ValueBits, UnitBits>::reverse(val, out);
                    val = out;
                }

                template<typename ValueType, int ValueBits = sizeof(ValueType) * CHAR_BIT>
                inline static ValueType reverse(ValueType const &val) {
                    ValueType tmp = boost::endian::endian_reverse(val);
                    ValueType out = ValueType();
                    byte_in_unit_reverser<ValueBits, UnitBits>::reverse(tmp, out);
                    return out;
                }
            };
        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_DETAIL_REVERSER_HPP


///////////////////////////////////////////////////////////////////////////////
/// \file algorithm.hpp
///   Contains range-based versions of the std algorithms
//
/////////////////////////////////////////////////////////////////////////////
// Copyright 2009 Neil Groves.
// 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)
//

// Copyright 2006 Thorsten Ottosen.
// 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)
//
// Copyright 2004 Eric Niebler.
// 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)

#if defined(_MSC_VER) && _MSC_VER >= 1000
    #pragma once
#endif

#ifndef BOOST_RANGE_NUMERIC_HPP
#define BOOST_RANGE_NUMERIC_HPP

#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/concepts.hpp>
#include <boost/range/distance.hpp>
#include <numeric>

namespace boost
{
    template< class SinglePassRange, class Value >
    inline Value accumulate( const SinglePassRange& rng, Value init )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange> ));
        return std::accumulate( boost::begin(rng), boost::end(rng), init );
    }

    template< class SinglePassRange, class Value, class BinaryOperation >
    inline Value accumulate( const SinglePassRange& rng, Value init, BinaryOperation op )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange> ));
        return std::accumulate( boost::begin(rng), boost::end(rng), init, op );
    }

    template< class SinglePassRange1, class SinglePassRange2, class Value >
    inline Value inner_product( const SinglePassRange1& rng1, const SinglePassRange2& rng2, Value init )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange1> ));
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange2> ));
        BOOST_ASSERT( boost::distance(rng2) >= boost::distance(rng1) );
        return std::inner_product( boost::begin(rng1), boost::end(rng1),
            boost::begin(rng2), init );
    }

    template< class SinglePassRange1,
              class SinglePassRange2,
              class Value,
              class BinaryOperation1, class BinaryOperation2 >
    inline Value inner_product( const SinglePassRange1& rng1, const SinglePassRange2& rng2,
                                Value init,
                                BinaryOperation1 op1, BinaryOperation2 op2 )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange1> ));
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange2> ));
        BOOST_ASSERT( boost::distance(rng2) >= boost::distance(rng1) );

        return std::inner_product( boost::begin(rng1), boost::end(rng1),
                                   boost::begin(rng2), init, op1, op2 );
    }

    template< class SinglePassRange, class OutputIterator >
    inline OutputIterator partial_sum ( const SinglePassRange& rng,
                                        OutputIterator result )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange> ));
        return std::partial_sum( boost::begin(rng), boost::end(rng), result );
    }

    template< class SinglePassRange, class OutputIterator, class BinaryOperation >
    inline OutputIterator partial_sum ( const SinglePassRange& rng, OutputIterator result,
                                        BinaryOperation op )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange> ));
        return std::partial_sum( boost::begin(rng), boost::end(rng), result, op );
    }

    template< class SinglePassRange, class OutputIterator >
    inline OutputIterator adjacent_difference ( const SinglePassRange& rng,
                                                OutputIterator result )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<const SinglePassRange> ));
        return std::adjacent_difference( boost::begin(rng), boost::end(rng),
                                         result );
    }

    template< class SinglePassRange, class OutputIterator, class BinaryOperation >
    inline OutputIterator adjacent_difference ( const SinglePassRange& rng,
                                                OutputIterator result,
                                                BinaryOperation op )
    {
        BOOST_RANGE_CONCEPT_ASSERT(( SinglePassRangeConcept<SinglePassRange> ));
        return std::adjacent_difference( boost::begin(rng), boost::end(rng),
                                         result, op );
    }

}

#endif


#ifndef FDJAC_HPP_
#define FDJAC_HPP_

/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

/*!
 * \file fdjac.hpp
 * \ingroup Solution
 * \brief Finite-difference Jacobian helper functions for multidimensional root finders
 * \remark Because this function is actually a template, the entire definition goes in the header file.
 * \remark We already have a procedure for finding the Jacobian, but it is a little confusing
 *         to use.  This version will use the functors we have defined to abstract much of that
 *         complexity.  A consequence of this is that if there is any lazy evaluation or other
 *         optimization to be done in the function evaluation, it will be up to the functor to
 *         arrange it.
 */

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "functor.hpp"
#include <iostream>
#include "solution/util/include/ublas-helpers.hpp"

#define UBLAS boost::numeric::ublas

#include "util/base/include/timer.h"

/*!
 * Compute a single column in a Jacobian matrix.  We have broken this
 * out from the fdjac subroutine so that we can easily test a single
 * column for nonsingularity without duplicating any code.
 */
template<class FTYPE,class MTRAIT>
inline void jacol(VecFVec<FTYPE,FTYPE> &F, const UBLAS::vector<FTYPE> &x,
                  const UBLAS::vector<FTYPE> &fx, int j,
                  UBLAS::matrix<FTYPE,MTRAIT> &J,
                  bool usepartial=true, std::ostream *diagnostic=NULL) {
  const FTYPE heps = 1.0e-6;
  const FTYPE TINY = 1.0e-6;
  UBLAS::vector<FTYPE> xx(x); // temporary, so we can respect the const on x
  UBLAS::vector<FTYPE> fxx(fx.size());        // hold the values of F(xx)
  FTYPE t = xx[j];            // store the old value
  FTYPE h = heps * (fabs(t)+TINY);

  xx[j] = t+h;
  h     = xx[j]-t; // reduce roundoff error, since (t+h)-t is not
                   // necessarily identical to the original h
  if(diagnostic) {
      (*diagnostic) << "j= " << j << "\th= " << h << "\nxx:\n" << xx << "\n";
  }
  if(usepartial) {F.partial(j);}    // hint to the function that this is a partial derivative calculation
  F(xx,fxx);       // eval the function
  xx[j] = t;       // restore the old value

  if(diagnostic) {
    (*diagnostic) << "fxx:\n" << fxx << "\n";
  }

  // compute the finite difference derivatives
  FTYPE hinv = 1.0/h;
  for(size_t i=0; i<fxx.size(); ++i) {
    J(i,j) = (fxx[i] - fx[i]) * hinv;
  }
}

/*!
 * Compute the Jacobian of a vector function F at point x.
 * \param[in] F: The function to have its Jacobian calculated
 * \param[in] x: The point at which to calculate the Jacobian
 * \param[out] J: The Jacobian of F
 * \remark This function evaluates F(x) and then calls fdjac(F,x,Fx,J).  If you have already
 *         evaluated F(x), you should call the latter version directly.
 * \warning The Jacobian calculated here is defined as J(i,j) = \partial F_i / \partial x_j.
 *          The version in use in GCAM looks as though it might reverse the roles of i and j;
 *          therefore, any function making use of either of these functions should take care
 *          that the right convention is being used.
 */
template <class FTYPE, class MTRAIT>
void fdjac(VecFVec<FTYPE,FTYPE> &F, const UBLAS::vector<FTYPE> &x,
           UBLAS::matrix<FTYPE,MTRAIT> &J, bool usepartial=true)
{
  UBLAS::vector<FTYPE> fx(F.nrtn());

  F(x,fx);                      // fx = F(x)
  fdjac(F,x,fx,J,usepartial);
}

/*!
 * Compute the Jacobian of a vector function F at point x.
 * \tparam FTYPE: The floating point type of the input and output vectors
 * \param[in] F: The function to have its Jacobian calculated
 * \param[in] x: The point at which to calculate the Jacobian
 * \param[in] fx: F(x)
 * \param[out] J: The Jacobian of F
 * \param[in] usepartial: (optional) use partial model evaluation for partial derivatives
 * \param[in] diagnostic: (optional) ostream pointer to which to send additional diagnostics
 *
 */
template<class FTYPE, class MTRAIT>
void fdjac(VecFVec<FTYPE,FTYPE> &F, const UBLAS::vector<FTYPE> &x,
           const UBLAS::vector<FTYPE> &fx, UBLAS::matrix<FTYPE,MTRAIT> &J, bool usepartial=true,
           std::ostream *diagnostic=NULL)
{
  if(diagnostic) {
    (*diagnostic) << "fdjac: usepartial = " << usepartial << "\nInitial x:\n" << x
        << "\nInitial fx:\n" << fx << "\n";
  }

  Timer& jacTimer = TimerRegistry::getInstance().getTimer( TimerRegistry::JACOBIAN );
  jacTimer.start();

  for(size_t j=0; j<x.size(); ++j) {
    jacol(F, x, fx, j, J, usepartial, diagnostic);
  }

  jacTimer.stop();
}

#undef UBLAS

#endif


// This file is part of the dune-gdt project:
//   https://github.com/dune-community/dune-gdt
// Copyright 2010-2017 dune-gdt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2013 - 2017)
//   Kirsten Weber   (2013)
//   Rene Milk       (2014, 2016 - 2017)
//   Tobias Leibner  (2014, 2016)

#ifndef DUNE_GDT_LOCAL_INTEGRANDS_ELLIPTIC_HH
#define DUNE_GDT_LOCAL_INTEGRANDS_ELLIPTIC_HH

#include <tuple>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/dynmatrix.hh>
#include <dune/common/typetraits.hh>

#include <dune/xt/common/fmatrix.hh>
#include <dune/xt/common/memory.hh>
#include <dune/xt/functions/constant.hh>
#include <dune/xt/functions/interfaces.hh>

#include "interfaces.hh"

namespace Dune {
namespace GDT {

// forward
template <class DiffusionFactorImp, class DiffusionTensorImp = void>
class LocalEllipticIntegrand;

namespace internal {

/**
 * \brief Traits for the Elliptic evaluation (variant for given diffusion factor and tensor).
 */
template <class DiffusionFactorImp, class DiffusionTensorImp>
class LocalEllipticIntegrandTraits
{
  static_assert(XT::Functions::is_localizable_function<DiffusionFactorImp>::value,
                "DiffusionFactorType has to be a localizable function!");

public:
  typedef typename DiffusionFactorImp::EntityType EntityType;
  typedef typename DiffusionFactorImp::DomainFieldType DomainFieldType;
  static const size_t dimDomain = DiffusionFactorImp::dimDomain;

  // we need to distinguish three cases here (since DiffusionTensorImp may be void):
  // given two functions, a factor and a tensor
  // given a single factor (then set the tensor to default)
  // given a single tensor (then set the factor to default)
private:
  typedef EntityType E;
  typedef DomainFieldType D;
  static const size_t d = dimDomain;

  template <bool factor, bool tensor, bool anything = true>
  struct Helper
  {
    static_assert(AlwaysFalse<DiffusionTensorImp>::value, "Unsupported combination of functions given!");
  };

  // given both
  template <bool anything>
  struct Helper<false, false, anything>
  {
    static_assert(XT::Functions::is_localizable_function<DiffusionTensorImp>::value,
                  "DiffusionTensorType has to be a localizable function!");
    static_assert(std::is_same<typename DiffusionFactorImp::EntityType, typename DiffusionTensorImp::EntityType>::value,
                  "EntityTypes have to agree!");
    static_assert(
        std::is_same<typename DiffusionFactorImp::DomainFieldType, typename DiffusionTensorImp::DomainFieldType>::value,
        "DomainFieldTypes have to agree!");
    static_assert(DiffusionFactorImp::dimDomain == DiffusionTensorImp::dimDomain, "Dimensions have to agree!");
    static_assert(DiffusionFactorImp::dimRange == 1, "DiffusionFactorType has to be scalar!");
    static_assert(DiffusionFactorImp::dimRangeCols == 1, "DiffusionFactorType has to be scalar!");
    static_assert(DiffusionTensorImp::dimRange == DiffusionTensorImp::dimDomain,
                  "DiffusionTensorType has to be matrix valued!");
    static_assert(DiffusionTensorImp::dimRangeCols == DiffusionTensorImp::dimDomain,
                  "DiffusionTensorType has to be matrix valued!");
    typedef DiffusionFactorImp FactorType;
    typedef DiffusionTensorImp TensorType;
  };

  // given only one, and this is scalar
  template <bool anything>
  struct Helper<true, false, anything>
  {
    typedef DiffusionFactorImp FactorType;
    typedef XT::Functions::ConstantFunction<E, D, d, D, d, d> TensorType;
  };

  // given only one, and this is a tensor
  template <bool anything>
  struct Helper<false, true, anything>
  {
    typedef XT::Functions::ConstantFunction<E, D, d, D, 1, 1> FactorType;
    typedef DiffusionFactorImp TensorType;
  };

  static const bool single_factor_given = DiffusionFactorImp::dimRange == 1
                                          && DiffusionFactorImp::dimRangeCols == DiffusionFactorImp::dimRange
                                          && std::is_same<DiffusionTensorImp, void>::value;
  static const bool single_tensor_given = DiffusionFactorImp::dimRange != 1
                                          && DiffusionFactorImp::dimRange == DiffusionFactorImp::dimRangeCols
                                          && std::is_same<DiffusionTensorImp, void>::value;

public:
  typedef typename Helper<single_factor_given, single_tensor_given>::FactorType DiffusionFactorType;
  typedef typename Helper<single_factor_given, single_tensor_given>::TensorType DiffusionTensorType;
  typedef LocalEllipticIntegrand<DiffusionFactorType, DiffusionTensorType> derived_type;
  typedef std::tuple<std::shared_ptr<typename DiffusionFactorType::LocalfunctionType>,
                     std::shared_ptr<typename DiffusionTensorType::LocalfunctionType>>
      LocalfunctionTupleType;
}; // class LocalEllipticIntegrandTraits

} // namespace internal

/**
 * \brief Computes an elliptic evaluation.
 */
template <class DiffusionFactorImp, class DiffusionTensorImp>
class LocalEllipticIntegrand
    : public LocalVolumeIntegrandInterface<internal::LocalEllipticIntegrandTraits<DiffusionFactorImp,
                                                                                DiffusionTensorImp>,
                                           2>
{
  typedef LocalVolumeIntegrandInterface<internal::LocalEllipticIntegrandTraits<DiffusionFactorImp, DiffusionTensorImp>,
                                        2>
      BaseType;
  typedef LocalEllipticIntegrand<DiffusionFactorImp, DiffusionTensorImp> ThisType;

public:
  typedef internal::LocalEllipticIntegrandTraits<DiffusionFactorImp, DiffusionTensorImp> Traits;
  typedef typename Traits::DiffusionFactorType DiffusionFactorType;
  typedef typename Traits::DiffusionTensorType DiffusionTensorType;
  typedef typename Traits::LocalfunctionTupleType LocalfunctionTupleType;
  typedef typename Traits::EntityType EntityType;
  typedef typename Traits::DomainFieldType DomainFieldType;
  static const size_t dimDomain = Traits::dimDomain;

private:
  typedef XT::Common::ConstStorageProvider<DiffusionFactorType> DiffusionFactorProvider;
  typedef XT::Common::ConstStorageProvider<DiffusionTensorType> DiffusionTensorProvider;
  using typename BaseType::E;
  using typename BaseType::D;
  using BaseType::d;

public:
  LocalEllipticIntegrand(const DiffusionFactorType& diff_factor,
                         const DiffusionTensorType& diff_tensor,
                         const XT::Common::Parameter& param = {})
    : diffusion_factor_(diff_factor)
    , diffusion_tensor_(diff_tensor)
    , param_(param)
  {
  }

  LocalEllipticIntegrand(const DiffusionFactorType& diff_factor, const XT::Common::Parameter& param = {})
    : diffusion_factor_(diff_factor)
    , diffusion_tensor_(new DiffusionTensorType(
          XT::Functions::internal::UnitMatrix<typename DiffusionTensorType::RangeFieldType, dimDomain>::value()))
    , param_(param)
  {
  }

  template < // This disables the ctor if dimDomain == 1, since factor and tensor are then identical and the
      typename DiffusionType, //                                                            ctors ambiguous.
      typename = typename std::enable_if<(std::is_same<DiffusionType, DiffusionTensorType>::value) && (dimDomain > 1)
                                         && sizeof(DiffusionType)>::type>
  LocalEllipticIntegrand(const DiffusionType& diffusion, const XT::Common::Parameter& param = {})
    : diffusion_factor_(new DiffusionFactorType(1.))
    , diffusion_tensor_(diffusion)
    , param_(param)
  {
  }

  /**
   * \attention Due to the nature of XT::Common::ConstStorageProvider, this copy may leave you with a dead reference,
   *            if other is destructed.
   */
  LocalEllipticIntegrand(const ThisType& other)
    : diffusion_factor_(other.diffusion_factor())
    , diffusion_tensor_(other.diffusion_tensor())
    , param_(other.param_)
  {
  }

  LocalEllipticIntegrand(ThisType&& source) = default;

  /// \name Required by LocalVolumeIntegrandInterface< ..., 2 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_factor_.access().local_function(entity),
                           diffusion_tensor_.access().local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template <class R, size_t rT, size_t rCT, size_t rA, size_t rCA>
  size_t order(const LocalfunctionTupleType& local_functions_tuple,
               const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rT, rCT>& test_base,
               const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rA, rCA>& ansatz_base) const
  {
    const auto local_diffusion_factor = std::get<0>(local_functions_tuple);
    const auto local_diffusion_tensor = std::get<1>(local_functions_tuple);
    return order(*local_diffusion_factor, *local_diffusion_tensor, test_base, ansatz_base);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template <class R, size_t rT, size_t rCT, size_t rA, size_t rCA>
  void evaluate(const LocalfunctionTupleType& local_functions_tuple,
                const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rT, rCT>& test_base,
                const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rA, rCA>& ansatz_base,
                const Dune::FieldVector<D, d>& localPoint,
                Dune::DynamicMatrix<R>& ret) const
  {
    const auto local_diffusion_factor = std::get<0>(local_functions_tuple);
    const auto local_diffusion_tensor = std::get<1>(local_functions_tuple);
    evaluate(*local_diffusion_factor, *local_diffusion_tensor, test_base, ansatz_base, localPoint, ret);
  }

  /// \}
  /// \name Actual implementations of order
  /// \{

  template <class R, size_t rDF, size_t rCDF, size_t rDT, size_t rCDT, size_t rT, size_t rCT, size_t rA, size_t rCA>
  size_t order(const XT::Functions::LocalfunctionInterface<E, D, d, R, rDF, rCDF>& local_diffusion_factor,
               const XT::Functions::LocalfunctionInterface<E, D, d, R, rDT, rCDT>& local_diffusion_tensor,
               const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rT, rCT>& test_base,
               const XT::Functions::LocalfunctionSetInterface<E, D, d, R, rA, rCA>& ansatz_base) const
  {
    return local_diffusion_factor.order() + local_diffusion_tensor.order()
           + std::max(ssize_t(test_base.order()) - 1, ssize_t(0))
           + std::max(ssize_t(ansatz_base.order()) - 1, ssize_t(0));
  }

  /// \}
  /// \name Actual implementations of evaluate
  /// \{

  template <class R, size_t r>
  void evaluate(const XT::Functions::LocalfunctionInterface<E, D, d, R, 1, 1>& local_diffusion_factor,
                const XT::Functions::LocalfunctionInterface<E, D, d, R, d, d>& local_diffusion_tensor,
                const XT::Functions::LocalfunctionSetInterface<E, D, d, R, r, 1>& test_base,
                const XT::Functions::LocalfunctionSetInterface<E, D, d, R, r, 1>& ansatz_base,
                const Dune::FieldVector<D, d>& localPoint,
                Dune::DynamicMatrix<R>& ret) const
  {
    typedef XT::Common::FieldMatrix<R, d, d> TensorType;
    ret *= 0.0;
    // evaluate local functions
    const auto diffusion_factor_value = local_diffusion_factor.evaluate(localPoint, param_);
    const TensorType diffusion_tensor_value = local_diffusion_tensor.evaluate(localPoint, param_);
    const auto diffusion_value = diffusion_tensor_value * diffusion_factor_value;
    // evaluate bases
    const auto testGradients = test_base.jacobian(localPoint, param_);
    const auto ansatzGradients = ansatz_base.jacobian(localPoint, param_);
    // compute elliptic evaluation
    const size_t rows = test_base.size();
    const size_t cols = ansatz_base.size();
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      for (size_t jj = 0; jj < cols; ++jj) {
        for (size_t rr = 0; rr < r; ++rr) {
          retRow[jj] += (diffusion_value * ansatzGradients[jj][rr]) * testGradients[ii][rr];
        }
      }
    }
  } // ... evaluate(...)

  /// \}
  /// \name Access to data functions (allows the evaluation to be used as traits handling multiple szenarios).
  /// \{

  const DiffusionFactorType& diffusion_factor() const
  {
    return diffusion_factor_.access();
  }

  const DiffusionTensorType& diffusion_tensor() const
  {
    return diffusion_tensor_.access();
  }

  /// \}

private:
  const DiffusionFactorProvider diffusion_factor_;
  const DiffusionTensorProvider diffusion_tensor_;
  const XT::Common::Parameter param_;
}; // class LocalEllipticIntegrand

} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCAL_INTEGRANDS_ELLIPTIC_HH


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__UTILS_HPP_
#define SMOOTH__UTILS_HPP_

#include <array>
#include <cstddef>
#include <functional>
#include <numeric>
#include <utility>

#include <Eigen/Core>

namespace smooth::utils {

/////////////////////
// STATIC FOR LOOP //
/////////////////////

/**
 * @brief Compile-time for loop implementation
 */
template<typename _F, std::size_t... _Idx>
inline static constexpr auto static_for_impl(_F && f, std::index_sequence<_Idx...>)
{
  return (std::invoke(f, std::integral_constant<std::size_t, _Idx>()), ...);
}

/**
 * @brief Compile-time for loop over 0, ..., _I-1
 */
template<std::size_t _I, typename _F>
inline static constexpr auto static_for(_F && f)
{
  return static_for_impl(std::forward<_F>(f), std::make_index_sequence<_I>{});
}

/////////////////
// ARRAY UTILS //
/////////////////

/**
 * @brief Prefix-sum an array starting at zero
 */
template<typename T, std::size_t L>
constexpr std::array<T, L + 1> array_psum(const std::array<T, L> & x)
{
  std::array<T, L + 1> ret;
  ret[0] = T(0);
  std::partial_sum(x.begin(), x.end(), ret.begin() + 1);
  return ret;
}

///////////////////////
// TUPLE STATE UTILS //
///////////////////////

template<typename T>
struct tuple_dof
{};

/**
 * @brief Compile-time size of a tuple of variables.
 *
 * If at least one variable is dynamically sized (size -1), this returns -1.
 */
template<typename... Wrt>
struct tuple_dof<std::tuple<Wrt...>>
{
  static constexpr int value = std::min<int>({std::decay_t<Wrt>::SizeAtCompileTime...}) == -1
                               ? std::min<int>({std::decay_t<Wrt>::SizeAtCompileTime...})
                               : (std::decay_t<Wrt>::SizeAtCompileTime + ...);
};

/**
 * @brief Cast a tuple of variables to a new scalar type.
 */
template<typename Scalar, typename... _Wrt>
auto tuple_cast(const std::tuple<_Wrt...> & wrt)
{
  std::tuple<
    typename std::decay_t<decltype(std::decay_t<_Wrt>{}.template cast<Scalar>())>::PlainObject...>
    ret;
  static_for<sizeof...(_Wrt)>(
    [&](auto i) { std::get<i>(ret) = std::get<i>(wrt).template cast<Scalar>(); });
  return ret;
}

/**
 * @brief Add a tangent vector to a tuple of variables.
 */
template<typename Derived, typename... _Wrt>
auto tuple_plus(const std::tuple<_Wrt...> & wrt, const Eigen::MatrixBase<Derived> & a)
{
  std::tuple<typename std::decay_t<_Wrt>::PlainObject...> ret;
  std::size_t i_beg = 0;
  static_for<sizeof...(_Wrt)>([&](auto i) {
    constexpr auto i_size =
      std::tuple_element_t<i, std::tuple<std::decay_t<_Wrt>...>>::SizeAtCompileTime;
    std::size_t i_len = std::get<i>(wrt).size();
    std::get<i>(ret)  = std::get<i>(wrt) + a.template segment<i_size>(i_beg, i_len);
    i_beg += i_len;
  });
  return ret;
}

/**
 * @brief Trait for removing const-ness from reference types.
 */
template<typename T>
struct remove_const_ref
{
  using type = T;
};

template<typename T>
struct remove_const_ref<const T &>
{
  using type = T;
};

/**
 * @brief Copy a tuple to make all elements modifiable.
 *
 * Copies are created form const & members, rest is forwarded.
 */
template<typename... T>
std::tuple<typename remove_const_ref<T>::type...> tuple_copy_if_const(std::tuple<T...> && in)
{
  return std::make_from_tuple<std::tuple<typename remove_const_ref<T>::type...>>(std::move(in));
}

/**
 * @brief Copy a tuple to make all elements modifiable.
 *
 * Copies are created form const & members, rest is forwarded.
 */
template<typename... T>
auto tuple_copy_if_const(const std::tuple<T...> & in)
{
  return std::make_from_tuple<std::tuple<typename remove_const_ref<T>::type...>>(in);
}

/////////////////////////////////
// COMPILE-TIME MATRIX ALGEBRA //
/////////////////////////////////

/**
 * @brief Elementary structure for compile-time matrix algebra
 */
template<typename _Scalar, std::size_t _Rows, std::size_t _Cols>
struct StaticMatrix : public std::array<std::array<_Scalar, _Cols>, _Rows>
{
  std::size_t Rows = _Rows;
  std::size_t Cols = _Cols;

  using std::array<std::array<_Scalar, _Cols>, _Rows>::operator[];

  /**
   * @brief Construct a matrix filled with zeros
   */
  constexpr StaticMatrix() : std::array<std::array<_Scalar, _Cols>, _Rows>{}
  {
    for (auto i = 0u; i != _Rows; ++i) { operator[](i).fill(_Scalar(0)); }
  }

  /**
   * @brief Add two matrices
   */
  constexpr StaticMatrix<_Scalar, _Rows, _Cols> operator+(
    StaticMatrix<_Scalar, _Rows, _Cols> o) const
  {
    StaticMatrix<_Scalar, _Rows, _Cols> ret;
    for (auto i = 0u; i < _Rows; ++i) {
      for (auto j = 0u; j < _Cols; ++j) { ret[i][j] = operator[](i)[j] + o[i][j]; }
    }
    return ret;
  }

  /**
   * @brief Return transpose of a matrix
   */
  constexpr StaticMatrix<_Scalar, _Rows, _Cols> transpose() const
  {
    StaticMatrix<_Scalar, _Rows, _Cols> ret;
    for (auto i = 0u; i < _Rows; ++i) {
      for (auto j = 0u; j < _Cols; ++j) { ret[j][i] = operator[](i)[j]; }
    }
    return ret;
  }

  /**
   * @brief Multiply two matrices
   */
  template<std::size_t _ColsNew>
  constexpr StaticMatrix<_Scalar, _Rows, _ColsNew> operator*(
    StaticMatrix<_Scalar, _Cols, _ColsNew> o) const
  {
    StaticMatrix<_Scalar, _Rows, _ColsNew> ret;
    for (auto i = 0u; i < _Rows; ++i) {
      for (auto j = 0u; j < _ColsNew; ++j) {
        for (auto k = 0u; k < _Cols; ++k) { ret[i][j] += operator[](i)[k] * o[k][j]; }
      }
    }
    return ret;
  }
};

}  // namespace smooth::utils

#endif  // SMOOTH__UTILS_HPP_


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2014 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 OWNER 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 <Eigen/Eigen>
#include <comma/base/exception.h>
#include "polytope.h"

namespace snark{ namespace geometry {

const Eigen::MatrixXd& convex_polytope::normals() const { return normals_; }

const Eigen::VectorXd& convex_polytope::offsets() const { return offsets_; }

convex_polytope::convex_polytope( const std::vector< Eigen::VectorXd >& normals, const std::vector< double >& offsets )
{
    if(!normals.size() || normals[0].size()) { COMMA_THROW( comma::exception, "normals size cannot be zero" ); }
    if(normals.size()!=offsets.size()) { COMMA_THROW( comma::exception, "normals and offsets should be of same size, got "<< normals.size()<<" and "<<offsets.size() ); }
    normals_.resize(normals.size(),normals[0].size());
    offsets_.resize(offsets.size());
    for(unsigned int i=0; i<normals.size(); ++i )
    {
        normals_.row(i)=normals[i].transpose();
        offsets_(i)=offsets[i];
    }
}

convex_polytope::convex_polytope( const Eigen::MatrixXd& normals, const Eigen::VectorXd& offsets )
    : normals_( normals )
    , offsets_( offsets )
{
    if(!normals.size()) { COMMA_THROW( comma::exception, "normals size cannot be zero" ); }
    if(normals.rows()!=offsets.rows()) { COMMA_THROW( comma::exception, "normals and offsets should be of same size, got "<< normals.rows()<<" and "<<offsets.rows() ); }
}

convex_polytope::convex_polytope( const Eigen::MatrixXd& planes )
{
    if(!planes.rows() || planes.cols() < 2) { COMMA_THROW( comma::exception, "planes rows cannot be zero and cols must be at least two, got "<<planes.rows()<<" and "<<planes.cols() ); }
    normals_=planes.leftCols( static_cast< unsigned int >( planes.cols() - 1 ) );
    offsets_=planes.rightCols(1);
}

//static Eigen::VectorXd symmat2vec(const Eigen::MatrixXd& A)
//{
//    unsigned int n = A.rows();
//    Eigen::VectorXd v( n * ( n + 1 ) / 2 );
//    unsigned int m = 0;
//    for( unsigned int i=0; i<n; ++i )
//    {
//        for( unsigned int j = 0; j <= i; v(m) = A(i,j), ++j, ++m );
//    }
//    return v;
//}

//static Eigen::VectorXd symmat2vec(const Eigen::MatrixXd& A)
//{
//    int n=A.rows();
//    Eigen::VectorXd v(n*(n+1)/2);

//    for(int i=0; i<n; i++)
//    {
//        for(int j=0; j<=i; j++)
//        {
//            v(i*(i+1)/2+j)=A(i,j);
//        }
//    }
//    return v;
//}
// This function uses the SDP solver provided by the DSDP library
// It reformulates the problem of finding whether a point is inside a convex polygon as an SDP (Semi-Definite Program)
// Problem:
// min ||y-x||^2 s.t. Ay<=b
// SDP Formulation:
// [I y-x; (y-xy)' t] is psd
// diag(b)-y1*A1-...-yn*An is psd

/// This function checks whether a point is inside a convex polytope: Ay<=b
bool convex_polytope::has(const Eigen::VectorXd &x)
{
    int dimx=x.size();
    if(dimx!=normals_.cols())
    {
        COMMA_THROW( comma::exception, "point should be of same dimension as polytope, polytope dimension: "<<normals_.cols()<<" point dimension: "<<dimx );
    }

    const Eigen::MatrixXd& A=normals_;
    const Eigen::VectorXd& b=offsets_;

    Eigen::VectorXd d=A*x-b;

    return((d.array()>=0).all());

//    int nofaces=A.rows();

//    tolerance=tolerance*tolerance; //square tolerance

//    DSDP solver;
//    DSDPCreate(dimx+1,&solver);

//    for(int cntr=1; cntr<=dimx; cntr++)
//    {
//        DSDPSetDualObjective(solver,cntr,0);
//    }
//    DSDPSetDualObjective(solver,dimx+1,-1); // objective is inverted

//    SDPCone cone;
//    DSDPCreateSDPCone(solver,2,&cone);

//    Eigen::MatrixXd A0(nofaces,nofaces);
//    Eigen::MatrixXd A1(dimx+1,dimx+1);

//    std::vector< Eigen::VectorXd > a0(dimx+2);
//    std::vector< Eigen::VectorXd > a1(dimx+2);

//    //constant terms
//    A0.setZero();
//    A1.setZero();
//    A0=b.asDiagonal();A0=-A0;
//    A1.block(0,0,dimx,dimx)=Eigen::MatrixXd::Identity(dimx,dimx);
//    A1.block(0,dimx,dimx,1)=-x;
//    A1.block(dimx,0,1,dimx)=-x.transpose();
//    a0[0] = Eigen::VectorXd(symmat2vec(A0));
//    a1[0] = Eigen::VectorXd(symmat2vec(A1));
//    SDPConeSetADenseVecMat(cone,0,0,nofaces,1.0,a0[0].data(),a0[0].size());
//    SDPConeSetADenseVecMat(cone,1,0,dimx+1,1.0,a1[0].data(),a1[0].size());

//    //yi's, A's are negated
//    for(int cntr=0; cntr<dimx; cntr++)
//    {
//        A0.setZero();
//        A1.setZero();
//        A0=A.col(cntr).asDiagonal();
//        A1(dimx,cntr)=1;
//        A1(cntr,dimx)=1;
//        a0[cntr+1] = Eigen::VectorXd(symmat2vec(-A0));
//        a1[cntr+1] = Eigen::VectorXd(symmat2vec(-A1));
//        SDPConeSetADenseVecMat(cone,0,cntr+1,nofaces,1.0,a0[cntr+1].data(),a0[cntr+1].size());
//        SDPConeSetADenseVecMat(cone,1,cntr+1,dimx+1,1.0,a1[cntr+1].data(),a1[cntr+1].size());
//    }

//    //t
//    A0.setZero();
//    A1.setZero();
//    A1(dimx,dimx)=1;
//    a0[dimx+1] = Eigen::VectorXd(symmat2vec(-A0));
//    a1[dimx+1] = Eigen::VectorXd(symmat2vec(-A1));
//    SDPConeSetADenseVecMat(cone,0,dimx+1,nofaces,1.0,a0[dimx+1].data(),a0[dimx+1].size());
//    SDPConeSetADenseVecMat(cone,1,dimx+1,dimx+1,1.0,a1[dimx+1].data(),a1[dimx+1].size());

// //    DSDPSetY0(solver,1,1);
// //    DSDPSetY0(solver,2,1);
// //    DSDPSetY0(solver,3,1);
// //    DSDPSetY0(solver,4,10);
// //    SDPConeViewDataMatrix(cone,1,1);
// //    std::cout<<std::endl;
// //    SDPConeViewDataMatrix(cone,1,2);
// //    std::cout<<std::endl;
// //    SDPConeViewDataMatrix(cone,1,3);
// //    std::cout<<std::endl;
// //    SDPConeViewDataMatrix(cone,1,4);
// //    std::cout<<std::endl;

//    DSDPSetup(solver);
//    DSDPSetGapTolerance(solver,1e-10);
// //    DSDPSetStandardMonitor(solver,1);
//    DSDPSolve(solver);

// //    DSDPTerminationReason reason;
// //    DSDPStopReason(solver,&reason);
// //    std::cout<<"reason: "<<reason<<std::endl;

//    Eigen::VectorXd y(dimx+1);

//    DSDPGetY(solver,y.data(),y.size());

// //    Eigen::VectorXd smat((dimx+1)*(dimx+2)/2);
// //    SDPConeComputeS(cone,1,1, y.data(), y.size(), 0, dimx+1, smat.data(), smat.size());
// //    Eigen::MatrixXd S=vec2symmat(smat);
// //    std::cout<<S<<std::endl;
// //    std::cout<<y<<std::endl;

//    double t=y(dimx);
//    return(t<tolerance);
}

}} // namespace snark{ { namespace geometry {


#ifndef N_BODY_SPACE_HPP
#define N_BODY_SPACE_HPP

#include "communication.hpp"
#include "data.hpp"
#include "logging.hpp"
#include <algorithm>
#include <boost/mpi.hpp>
#include <cstddef>
#include <limits>

namespace n_body::space {

template <typename T, std::size_t Dimension>
data::Space<T, Dimension> root_space(const boost::mpi::communicator &comm,
                                     const data::Bodies<T, Dimension> &bodies) {
  communication::Division division(comm, bodies.size());

  T min = std::numeric_limits<T>::max();
  T max = std::numeric_limits<T>::min();

  for (auto i = division.begin; i < division.end; ++i) {
    for (std::size_t d = 0; d < Dimension; ++d) {
      if (bodies[i].position[d] < min)
        min = bodies[i].position[d];
      if (bodies[i].position[d] > max)
        max = bodies[i].position[d];
    }
  }

  for (std::size_t d = 0; d < Dimension; ++d) {
    boost::mpi::all_reduce(comm, boost::mpi::inplace(min),
                           boost::mpi::minimum<T>());
    boost::mpi::all_reduce(comm, boost::mpi::inplace(max),
                           boost::mpi::maximum<T>());
  }
  data::Space<T, Dimension> space{};
  for (std::size_t d = 0; d < Dimension; ++d) {
    space.min[d] = min;
    space.max[d] = max;
    space.center[d] = (max + min) / 2;
  }
  return space;
}

// TODO infer T and Dimension
template <typename T, std::size_t Dimension, typename Iter>
data::Space<T, Dimension> root_space(Iter first, Iter last) {

  T min = std::numeric_limits<T>::max();
  T max = std::numeric_limits<T>::min();

  for (; first != last; ++first) {
    for (std::size_t d = 0; d < Dimension; ++d) {
      if (first->position[d] < min)
        min = first->position[d];
      if (first->position[d] > max)
        max = first->position[d];
    }
  }

  data::Space<T, Dimension> space{};
  for (std::size_t d = 0; d < Dimension; ++d) {
    space.min[d] = min;
    space.max[d] = max;
    space.center[d] = (max + min) / 2;
  }
  return space;
}

template <typename T, std::size_t Dimension>
data::Space<T, Dimension> subspace(const data::Space<T, Dimension> &space,
                                   std::size_t part) {
  data::Space<T, Dimension> result{};
  for (std::size_t d = 0; d < Dimension; ++d) {
    auto is_negative = part & 0b1u;
    part >>= 1u;

    if (is_negative == 0) {
      result.max[d] = space.max[d];
      result.min[d] = space.center[d];
    } else {
      result.max[d] = space.center[d];
      result.min[d] = space.min[d];
    }
    result.center[d] = (result.max[d] + result.min[d]) / 2;
  }

  return result;
}

template <typename T, std::size_t Dimension>
bool contains(const data::Space<T, Dimension> &space,
              data::Vector<T, Dimension> position) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    if (position[d] < space.min[d] || position[d] > space.max[d]) {
      return false;
    }
  }
  return true;
}

// determine which part of the space does the position belong to
// use binary to encode the part
template <typename T, std::size_t Dimension>
std::size_t part_of_space(const data::Space<T, Dimension> &space,
                          data::Vector<T, Dimension> position) {
  static_assert(
      std::numeric_limits<std::size_t>::radix == 2,
      "the radix of std::size_t must be 2 to make the algorithm working");
  static_assert(Dimension <= std::numeric_limits<std::size_t>::digits,
                "the dimension is too big for std::size_t to represent parts "
                "of the space");

  std::size_t result = 0;
  for (std::size_t i = Dimension; i > 0; --i) {
    result <<= 1u;
    auto d = i - 1;
    if (position[d] < space.center[d]) {
      result |= 0b1u;
    }
  }
  return result;
}

template <typename T, std::size_t Dimension>
std::size_t size_of_space(const data::Space<T, Dimension> &space) {
  return space.max[0] - space.min[0];
}

template <typename T, std::size_t Dimension>
void extend_to_contain(data::Space<T, Dimension> &space,
                       const data::Space<T, Dimension> &other) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    bool changed = false;
    if (other.max[d] > space.max[d]) {
      space.max[d] = other.max[d];
      changed = true;
    }
    if (other.min[d] < space.min[d]) {
      space.min[d] = other.min[d];
      changed = true;
    }
    if (changed) {
      space.center[d] = (space.max[d] + space.min[d]) / 2;
    }
  }
}

}; // namespace n_body::space

#endif