////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  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 <mdspan>

#include <vector>
#include <tuple>

#include <boost/core/lightweight_test.hpp>

using std::vector;
using std::tuple;

using std::experimental::dyn;
using std::experimental::dimensions;
using std::experimental::layout_mapping_left;

template <std::size_t X>
void test_1d_static()
{ // {{{
    layout_mapping_left<
        dimensions<X>, dimensions<1>, dimensions<0>
    > const l{};

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);

    BOOST_TEST_EQ((l.stride(0)), 1);

    BOOST_TEST_EQ((l.size()), X);
    BOOST_TEST_EQ((l.span()), X);

    int dptr[X];

    // Set all elements to a unique value.
    for (auto i = 0; i < l[0]; ++i)
    {
        BOOST_TEST_EQ((l.index(i)), i);

        BOOST_TEST_EQ(&(dptr[l.index(i)]), &(dptr[i]));

        dptr[l.index(i)] = i;

        BOOST_TEST_EQ((dptr[l.index(i)]), i);
    }
} // }}}

template <std::size_t X>
void test_1d_dynamic()
{ // {{{
    layout_mapping_left<
        dimensions<dyn>, dimensions<1>, dimensions<0>
    > const l(X);

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);

    BOOST_TEST_EQ((l.stride(0)), 1);

    BOOST_TEST_EQ((l.size()), X);
    BOOST_TEST_EQ((l.span()), X);

    vector<int> data(l[0]);
    int* dptr = data.data();

    // Set all elements to a unique value.
    for (auto i = 0; i < l[0]; ++i)
    {
        BOOST_TEST_EQ((l.index(i)), i);

        BOOST_TEST_EQ(&(dptr[l.index(i)]), &(dptr[i]));

        dptr[l.index(i)] = i;

        BOOST_TEST_EQ((dptr[l.index(i)]), i);

        // Bounds-checking.
        BOOST_TEST_EQ((data.at(l.index(i))), i);
    }
} // }}}

template <std::size_t X, std::size_t Y>
void test_2d_static()
{ // {{{
    layout_mapping_left<
        dimensions<X, Y>, dimensions<1, 1>, dimensions<0, 0>
    > const l{};

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), false);

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);

    BOOST_TEST_EQ((l.size()), X * Y);
    BOOST_TEST_EQ((l.span()), X * Y);

    tuple<int, int> dptr[X * Y];

    // Set all elements to a unique value.
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j);

        BOOST_TEST_EQ((l.index(i, j)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j)]) = i;
        std::get<1>(dptr[l.index(i, j)]) = j;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j)])), j);
    }
} // }}}

template <std::size_t X, std::size_t Y>
void test_2d_dynamic()
{ // {{{
    layout_mapping_left<
        dimensions<dyn, dyn>, dimensions<1, 1>, dimensions<0, 0>
    > const l(X, Y);

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), true);

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);

    BOOST_TEST_EQ((l.size()), X * Y);
    BOOST_TEST_EQ((l.span()), X * Y);

    vector<tuple<int, int> > data(l[0] * l[1]);
    tuple<int, int>* dptr = data.data();

    // Set all elements to a unique value.
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j);

        BOOST_TEST_EQ((l.index(i, j)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j)]) = i;
        std::get<1>(dptr[l.index(i, j)]) = j;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j)])), j);

        // Bounds-checking.
        BOOST_TEST_EQ((std::get<0>(data.at(l.index(i, j)))), i);
        BOOST_TEST_EQ((std::get<1>(data.at(l.index(i, j)))), j);
    }
} // }}}

template <std::size_t X, std::size_t Y>
void test_2d_mixed_DS()
{ // {{{
    layout_mapping_left<
        dimensions<dyn, Y>, dimensions<1, 1>, dimensions<0, 0>
    > const l(X);

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), true); // Depends on l[0].

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);

    BOOST_TEST_EQ((l.size()), X * Y);
    BOOST_TEST_EQ((l.span()), X * Y);

    vector<tuple<int, int> > data(l[0] * l[1]);
    tuple<int, int>* dptr = data.data();

    // Set all elements to a unique value.
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j);

        BOOST_TEST_EQ((l.index(i, j)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j)]) = i;
        std::get<1>(dptr[l.index(i, j)]) = j;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j)])), j);

        // Bounds-checking.
        BOOST_TEST_EQ((std::get<0>(data.at(l.index(i, j)))), i);
        BOOST_TEST_EQ((std::get<1>(data.at(l.index(i, j)))), j);
    }
} // }}}

template <std::size_t X, std::size_t Y>
void test_2d_mixed_SD()
{ // {{{
    layout_mapping_left<
        dimensions<X, dyn>, dimensions<1, 1>, dimensions<0, 0>
    > const l(Y);

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), false);

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);

    BOOST_TEST_EQ((l.size()), X * Y);
    BOOST_TEST_EQ((l.span()), X * Y);

    vector<tuple<int, int> > data(l[0] * l[1]);
    tuple<int, int>* dptr = data.data();

    // Set all elements to a unique value.
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j);

        BOOST_TEST_EQ((l.index(i, j)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j)]) = i;
        std::get<1>(dptr[l.index(i, j)]) = j;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j)])), j);

        // Bounds-checking.
        BOOST_TEST_EQ((std::get<0>(data.at(l.index(i, j)))), i);
        BOOST_TEST_EQ((std::get<1>(data.at(l.index(i, j)))), j);
    }
} // }}}

template <std::size_t X, std::size_t Y, std::size_t Z>
void test_3d_static()
{ // {{{
    layout_mapping_left<
        dimensions<X, Y, Z>, dimensions<1, 1, 1>, dimensions<0, 0, 0>
    >  const l{};

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(2)), false);

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);
    BOOST_TEST_EQ((l.stride(2)), X * Y);

    BOOST_TEST_EQ((l.size()), X * Y * Z);
    BOOST_TEST_EQ((l.span()), X * Y * Z);

    tuple<int, int, int> dptr[X * Y * Z];

    // Set all elements to a unique value.
    for (auto k = 0; k < l[2]; ++k)
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j) + (l[0]) * (l[1]) * (k);

        BOOST_TEST_EQ((l.index(i, j, k)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j, k)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j, k)]) = i;
        std::get<1>(dptr[l.index(i, j, k)]) = j;
        std::get<2>(dptr[l.index(i, j, k)]) = k;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j, k)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j, k)])), j);
        BOOST_TEST_EQ((std::get<2>(dptr[l.index(i, j, k)])), k);
    }
} // }}}

template <std::size_t X, std::size_t Y, std::size_t Z>
void test_3d_dynamic()
{ // {{{
    layout_mapping_left<
        dimensions<dyn, dyn, dyn>, dimensions<1, 1, 1>, dimensions<0, 0, 0>
    > const l(X, Y, Z);

    BOOST_TEST_EQ((l.is_regular()), true);

    BOOST_TEST_EQ((l.is_dynamic_stride(0)), false);
    BOOST_TEST_EQ((l.is_dynamic_stride(1)), true);
    BOOST_TEST_EQ((l.is_dynamic_stride(2)), true);

    BOOST_TEST_EQ((l.stride(0)), 1);
    BOOST_TEST_EQ((l.stride(1)), X);
    BOOST_TEST_EQ((l.stride(2)), X * Y);

    BOOST_TEST_EQ((l.size()), l[0] * l[1] * l[2]);
    BOOST_TEST_EQ((l.span()), l[0] * l[1] * l[2]);

    vector<tuple<int, int, int> > data(l[0] * l[1] * l[2]);
    tuple<int, int, int>* dptr = data.data();

    // Set all elements to a unique value.
    for (auto k = 0; k < l[2]; ++k)
    for (auto j = 0; j < l[1]; ++j)
    for (auto i = 0; i < l[0]; ++i)
    {
        auto const true_idx = (i) + (l[0]) * (j) + (l[0]) * (l[1]) * (k);

        BOOST_TEST_EQ((l.index(i, j, k)), true_idx);

        BOOST_TEST_EQ(&(dptr[l.index(i, j, k)]), &(dptr[true_idx]));

        std::get<0>(dptr[l.index(i, j, k)]) = i;
        std::get<1>(dptr[l.index(i, j, k)]) = j;
        std::get<2>(dptr[l.index(i, j, k)]) = k;

        BOOST_TEST_EQ((std::get<0>(dptr[l.index(i, j, k)])), i);
        BOOST_TEST_EQ((std::get<1>(dptr[l.index(i, j, k)])), j);
        BOOST_TEST_EQ((std::get<2>(dptr[l.index(i, j, k)])), k);

        // Bounds-checking.
        BOOST_TEST_EQ((std::get<0>(data.at(l.index(i, j, k)))), i);
        BOOST_TEST_EQ((std::get<1>(data.at(l.index(i, j, k)))), j);
        BOOST_TEST_EQ((std::get<2>(data.at(l.index(i, j, k)))), k);
    }
} // }}}

int main()
{
    // Empty
    {
        layout_mapping_left<dimensions<>, dimensions<>, dimensions<> > const l{};

        BOOST_TEST_EQ((l.is_regular()), true);

        BOOST_TEST_EQ((l.size()), 1);
        BOOST_TEST_EQ((l.span()), 1);

        int data = 42;
        int* dptr = &data;

        BOOST_TEST_EQ((l.index()), 0);

        BOOST_TEST_EQ((dptr[l.index()]), 42);

        BOOST_TEST_EQ(&(dptr[l.index()]), dptr);

        dptr[l.index()] = 17;

        BOOST_TEST_EQ((dptr[l.index()]), 17);
    }

    // 1D Static
    test_1d_static<1 >();
    test_1d_static<30>();

    // 1D Dynamic
    test_1d_dynamic<1 >();
    test_1d_dynamic<30>();

    // 2D Static
    test_2d_static<1,  1 >();

    test_2d_static<30, 1 >();
    test_2d_static<1,  30>();

    test_2d_static<30, 15>();
    test_2d_static<15, 30>();

    test_2d_static<30, 30>();

    // 2D Dynamic
    test_2d_dynamic<1,  1 >();

    test_2d_dynamic<30, 1 >();
    test_2d_dynamic<1,  30>();

    test_2d_dynamic<30, 15>();
    test_2d_dynamic<15, 30>();

    test_2d_dynamic<30, 30>();

    // 2D Mixed - A[dynamic][static]
    test_2d_mixed_SD<1,  1 >();

    test_2d_mixed_SD<30, 1 >();
    test_2d_mixed_SD<1,  30>();

    test_2d_mixed_SD<30, 15>();
    test_2d_mixed_SD<15, 30>();

    test_2d_mixed_SD<30, 30>();

    // 2D Mixed - A[static][dynamic]
    test_2d_mixed_SD<1,  1 >();

    test_2d_mixed_SD<30, 1 >();
    test_2d_mixed_SD<1,  30>();

    test_2d_mixed_SD<30, 15>();
    test_2d_mixed_SD<15, 30>();

    test_2d_mixed_SD<30, 30>();

    // 3D Static
    test_3d_static<1,  1,  1 >();

    test_3d_static<30, 1,  1 >();
    test_3d_static<1,  30, 1 >();
    test_3d_static<1,  1,  30>();

    test_3d_static<1,  30, 30>();
    test_3d_static<30, 1,  30>();
    test_3d_static<30, 30, 1 >();

    test_3d_static<30, 15, 15>();
    test_3d_static<15, 30, 15>();
    test_3d_static<15, 15, 30>();

    test_3d_static<15, 30, 30>();
    test_3d_static<30, 15, 30>();
    test_3d_static<30, 30, 15>();

    test_3d_static<30, 30, 30>();

    // 3D Dynamic
    test_3d_dynamic<1,  1,  1 >();

    test_3d_dynamic<30, 1,  1 >();
    test_3d_dynamic<1,  30, 1 >();
    test_3d_dynamic<1,  1,  30>();

    test_3d_dynamic<1,  30, 30>();
    test_3d_dynamic<30, 1,  30>();
    test_3d_dynamic<30, 30, 1 >();

    test_3d_dynamic<30, 15, 15>();
    test_3d_dynamic<15, 30, 15>();
    test_3d_dynamic<15, 15, 30>();

    test_3d_dynamic<15, 30, 30>();
    test_3d_dynamic<30, 15, 30>();
    test_3d_dynamic<30, 30, 15>();

    test_3d_dynamic<30, 30, 30>();

    return boost::report_errors();
}


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

#include <boost/simd/arithmetic/functions/iround2even.hpp>
#include <boost/simd/include/functions/scalar/fast_iround2even.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( iround2even_, tag::cpu_
                                   , (A0)
                                   , ((scalar_<single_<A0> >))
                                   )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
    #ifndef BOOST_SIMD_NO_NANS
      if (boost::simd::is_nan(a0))       return Zero<result_type>();
    #endif
      if (a0 >= Valmax<result_type>())   return Valmax<result_type>();
      if (a0 <= Valmin<result_type>())   return Valmin<result_type>();
      return fast_iround2even(a0);
    }
  };
} } }

#endif


/*=============================================================================
Copyright 2018 Pranam Lashkari <plashkari628@gmail.com>
Copyright 2019 Sarthak Singhal <singhalsarthak2007@gmail.com>
Copyright 2020 Rohit Ranjan    <rohitrjn629@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_COORDINATE_SPHERICAL_EQUATORIAL_REPRESENTATION_HPP
#define BOOST_ASTRONOMY_COORDINATE_SPHERICAL_EQUATORIAL_REPRESENTATION_HPP

#include <tuple>
#include <cstddef>

#include <boost/geometry/strategies/strategy_transform.hpp>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/units/systems/si/dimensionless.hpp>

#include <boost/astronomy/detail/is_base_template_of.hpp>
#include <boost/astronomy/coordinate/rep/base_representation.hpp>
#include <boost/astronomy/coordinate/rep/cartesian_representation.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

//!Represents the coordinate in spherical equatorial representation
//!Uses three components to represent a point/vector (latitude, longitude, distance)
/*!
\brief Spherical equatorial coordinate system, in degree or in radian
\details This one resembles the geographic coordinate system, and has latitude
    up from zero at the equator, to 90 at the pole
    (opposite to the spherical(polar) coordinate system).
    Used in astronomy and in GIS (but there is also the geographic)

\see http://en.wikipedia.org/wiki/Spherical_coordinates
\ingroup cs
*/
template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename DistQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>
>
struct spherical_equatorial_representation : public base_representation
    <3, bg::cs::spherical_equatorial<radian>, CoordinateType>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<LatQuantity>::type,
            bu::plane_angle_dimension>::value) &&
            (std::is_same<typename bu::get_dimension<LonQuantity>::type,
            bu::plane_angle_dimension>::value)),
        "Latitude and Longitude must be of plane angle type");
    BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
        "CoordinateType must be a floating-point type");
    ///@endcond

public:
    typedef LatQuantity quantity1;
    typedef LonQuantity quantity2;
    typedef DistQuantity quantity3;

    //default constructor no initialization
    spherical_equatorial_representation(){}

    //!constructs object from provided value of coordinates (latitude, longitude, distance)
    spherical_equatorial_representation
    (
        LatQuantity const& lat,
        LonQuantity const& lon,
        DistQuantity const& distance
    )
    {
        this->set_lat_lon_dist(lat, lon, distance);
    }

    //!constructs object from boost::geometry::model::point object
    template
    <
        std::size_t OtherDimensionCount,
        typename OtherCoordinateSystem,
        typename OtherCoordinateType
    >
    spherical_equatorial_representation
    (
        bg::model::point
        <
            OtherCoordinateType,
            OtherDimensionCount,
            OtherCoordinateSystem
        > const& pointObject
    )
    {
        bg::model::point<OtherCoordinateType, 3, bg::cs::cartesian> temp;
        bg::transform(pointObject, temp);
        bg::transform(temp, this->point);
    }

    //copy constructor
    spherical_equatorial_representation
    (
        spherical_equatorial_representation
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        > const& object
    )
    {
        this->point = object.get_point();
    }

    //!constructs object from any type of representation
    template <typename Representation>
    spherical_equatorial_representation(Representation const& other)
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, Representation>::value),
            "No constructor found with given argument type");

        BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<DistQuantity>::type,
        typename bu::get_dimension<typename Representation::quantity3>::type>::value)),
        "Two representations must have same dimensions");

        auto tempRep = make_spherical_equatorial_representation(other);
        bg::model::point
        <
            typename std::conditional
            <
                sizeof(CoordinateType) >= sizeof(typename Representation::type),
                CoordinateType,
                typename Representation::type
            >::type,
            3,
            bg::cs::spherical_equatorial<radian>
        > tempPoint;

        bg::set<0>(tempPoint,
            static_cast<
            bu::quantity<bu::si::plane_angle, CoordinateType>
            >(tempRep.get_lat()).value());
        bg::set<1>(tempPoint,
            static_cast<
            bu::quantity<bu::si::plane_angle, CoordinateType>
            >(tempRep.get_lon()).value());
        bg::set<2>(tempPoint,
            static_cast<DistQuantity>(tempRep.get_dist()).value());

        this->point = tempPoint;

    }
    //! returns the (lat, lon, distance) in the form of tuple
    std::tuple<LatQuantity, LonQuantity, DistQuantity> get_lat_lon_dist() const
    {
        return std::make_tuple(this->get_lat(), this->get_lon(), this->get_dist());
    }

    //!returns the lat component of point
    LatQuantity get_lat() const
    {
        return static_cast<LatQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<0>(this->point))
            );
    }

    //!returns the lon component of point
    LonQuantity get_lon() const
    {
        return static_cast<LonQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<1>(this->point))
            );
    }

    //!returns the distance component of point
    DistQuantity get_dist() const
    {
        return DistQuantity::from_value(bg::get<2>(this->point));
    }

    //!set value of (lat, lon, distance) in current object
    void set_lat_lon_dist
    (
        LatQuantity const& lat,
        LonQuantity const& lon,
        DistQuantity const& distance
    )
    {
        this->set_lat(lat);
        this->set_lon(lon);
        this->set_dist(distance);
    }

    //!set value of lat component of point
    void set_lat(LatQuantity const& lat)
    {
        bg::set<0>
            (
            this->point,
            static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(lat).value()
            );
    }

    //!set value of lon component of point
    void set_lon(LonQuantity const& lon)
    {
        bg::set<1>
            (
            this->point,
            static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(lon).value()
            );
    }

    //!set value of distance component of point
    void set_dist(DistQuantity const& distance)
    {
        bg::set<2>(this->point, distance.value());
    }

    //!operator for addition of representation
    template<typename Addend>
    spherical_equatorial_representation
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    >
    operator +(Addend const& addend) const
    {

        auto cartesian1 = make_cartesian_representation
            <CoordinateType, DistQuantity, DistQuantity, DistQuantity>(this->point);
        auto cartesian2 = make_cartesian_representation(addend);

        auto temp = cartesian1 + cartesian2;

        spherical_equatorial_representation
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        > result = make_spherical_equatorial_representation(temp);

        return result;
    }

}; //spherical_equatorial_representation

//!Constructs object from provided quantities
//!Each quantity can have different units but with same datatypes
template
<
    typename CoordinateType,
    template <typename Unit1, typename CoordinateType_> class LatQuantity,
    template <typename Unit2, typename CoordinateType_> class LonQuantity,
    template <typename Unit3, typename CoordinateType_> class DistQuantity,
    typename Unit1,
    typename Unit2,
    typename Unit3
>
spherical_equatorial_representation
<
    CoordinateType,
    LatQuantity<Unit1, CoordinateType>,
    LonQuantity<Unit2, CoordinateType>,
    DistQuantity<Unit3, CoordinateType>
>
make_spherical_equatorial_representation
(
    LatQuantity<Unit1, CoordinateType> const& lat,
    LonQuantity<Unit2, CoordinateType> const& lon,
    DistQuantity<Unit3, CoordinateType> const& dist
)
{
    return spherical_equatorial_representation
        <
            CoordinateType,
            LatQuantity<Unit1, CoordinateType>,
            LonQuantity<Unit2, CoordinateType>,
            DistQuantity<Unit3, CoordinateType>
        >(lat, lon, dist);
}

//!constructs object from provided components of representation with different units
template
<
    typename ReturnCoordinateType,
    typename ReturnLatQuantity,
    typename ReturnLonQuantity,
    typename ReturnDistQuantity,
    typename CoordinateType,
    typename LatQuantity,
    typename LonQuantity,
    typename DistQuantity
>
spherical_equatorial_representation
<
    ReturnCoordinateType,
    ReturnLatQuantity,
    ReturnLonQuantity,
    ReturnDistQuantity
>
make_spherical_equatorial_representation
(
    spherical_equatorial_representation
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    > const& other
)
{
    return make_spherical_equatorial_representation(
        static_cast<ReturnLatQuantity>(other.get_lat()),
        static_cast<ReturnLonQuantity>(other.get_lon()),
        static_cast<ReturnDistQuantity>(other.get_dist())
    );
}

//!constructs object from provided representation
template
<
    typename CoordinateType,
    typename LatQuantity,
    typename LonQuantity,
    typename DistQuantity
>
spherical_equatorial_representation
<
    CoordinateType,
    LatQuantity,
    LonQuantity,
    DistQuantity
>
make_spherical_equatorial_representation
(
    spherical_equatorial_representation
    <
        CoordinateType,
        LatQuantity,
        LonQuantity,
        DistQuantity
    > const& other
)
{
    return spherical_equatorial_representation
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        >(other);
}

//!constructs object from boost::geometry::model::point object
template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename DistQuantity = bu::quantity<bu::si::dimensionless, CoordinateType>,
    std::size_t OtherDimensionCount,
    typename OtherCoordinateSystem,
    typename OtherCoordinateType
>
spherical_equatorial_representation
<
    CoordinateType,
    LatQuantity,
    LonQuantity,
    DistQuantity
>
make_spherical_equatorial_representation
(
    bg::model::point
    <
        OtherCoordinateType,
        OtherDimensionCount,
        OtherCoordinateSystem
    > const& pointObject
)
{
    return spherical_equatorial_representation
        <
            CoordinateType,
            LatQuantity,
            LonQuantity,
            DistQuantity
        >(pointObject);
}

//!constructs object from any type of representation
template
<
    typename OtherRepresentation
>
auto make_spherical_equatorial_representation
(
    OtherRepresentation const& other
)
{
    auto temp = make_cartesian_representation(other);
    typedef decltype(temp) cartesian_type;

    bg::model::point
        <
            typename cartesian_type::type,
            3,
            bg::cs::cartesian
        > tempPoint;

    bg::set<0>(tempPoint, temp.get_x().value());
    bg::set<1>(tempPoint, static_cast<typename cartesian_type::quantity1>
        (temp.get_y()).value());
    bg::set<2>(tempPoint, static_cast<typename cartesian_type::quantity1>
        (temp.get_z()).value());

    bg::model::point<typename cartesian_type::type, 3, bg::cs::spherical_equatorial
        <radian>> result(0.0, 0.0, 0.0);
    bg::transform(tempPoint, result);

    return spherical_equatorial_representation
        <
            typename cartesian_type::type,
            bu::quantity<bu::si::plane_angle, typename cartesian_type::type>,
            bu::quantity<bu::si::plane_angle, typename cartesian_type::type>,
            typename cartesian_type::quantity1
        >(result);
}

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_SPHERICAL_EQUATORIAL__REPRESENTATION_HPP


/*  */

#include <cassert>
#include <iostream> // std::cout, std::endl
#include <memory>   // std::shared_ptr
#include <thread>   // std::this_thread
#include <stack>    // std::stack
#include <vector>
#include <map>
#include <fstream>
#include <sstream>

#include <Python.h>
#include <boost/python.hpp>

#include <GL/glut.h>
#include <glm/glm.hpp>
#include <glm/gtx/component_wise.hpp>

using namespace std;
using namespace glm;

struct Vertex
{
    vec3 position;
    vec3 normal;
};

vector< Vertex > LoadM( istream& in )
{
    vector< Vertex > verts;

    map< int, vec3 > positions;

    string lineStr;
    while( getline( in, lineStr ) )
    {
        istringstream lineSS( lineStr );
        string lineType;
        lineSS >> lineType;

        // parse vertex line
        if( lineType == "Vertex" )
        {
            int idx = 0;
            float x = 0, y = 0, z = 0;
            lineSS >> idx >> x >> y >> z;
            positions.insert( make_pair( idx, vec3( x, y, z ) ) );
        }

        // parse face line
        if( lineType == "Face" )
        {
            int indexes[ 3 ] = { 0 };
            int idx = 0;
            lineSS >> idx >> indexes[0] >> indexes[1] >> indexes[2];

            // http://www.opengl.org/wiki/Calculating_a_Surface_Normal
            vec3 U( positions[ indexes[1] ] - positions[ indexes[0] ] );
            vec3 V( positions[ indexes[2] ] - positions[ indexes[0] ] );
            vec3 faceNormal = normalize( cross( U, V ) );

            for( size_t j = 0; j < 3; ++j )
            {
                Vertex vert;
                vert.position = vec3( positions[ indexes[j] ] );
                vert.normal   = faceNormal;
                verts.push_back( vert );
            }
        }
    }

    return verts;
}

// for rotation and translation
ivec2 curRot;
ivec2 curTrans;

// for zoom in/out
float gfScaling = 0.5;
float gfScalingAcc = 0.03;
float curScaling = 1.0;
void PassMotion(int iX, int iY)
{
    glutPostRedisplay();
}
void vMotion(int iSX1, int iSY1, int iSX2, int iSY2, int iFlag, int iStart)
{
    ivec2 PosRos(iSX1, iSY1);
    ivec2 PosTrans(iSX2, iSY2);
    ivec2 PosStart(iSX1, iSY1);
    ivec2 PosDiff(iSX2-iSX1, iSY2-iSY1);
    if(iFlag == 0 && iStart==0)
    {
        curRot = PosRos;
        curTrans = PosTrans;
    }
    else if(iStart==1)
    {
        curScaling = gfScaling + (float)( iSX2-iSX1 ) / 800.0f;
    }
    glutPostRedisplay();
}

vector< Vertex > model;
void display()
{
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    double w = glutGet( GLUT_WINDOW_WIDTH );
    double h = glutGet( GLUT_WINDOW_HEIGHT );
    double ar = w / h;
    // "pan"
    glTranslatef( curTrans.x / w * 2, curTrans.y / h * 2, 0 );
    gluPerspective( 60, ar, 0.1, 20 );

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();
    glTranslatef( 0, 0, -10 );

    glPushMatrix();
    // apply mouse rotation
    glRotatef( curRot.x % 360, 0, 1, 0 );
    glRotatef( -curRot.y % 360, 1, 0, 0 );

    // apply zoom in/out
    glScalef(curScaling, curScaling, curScaling);

    //glColor3ub( 255, 0, 0 );
    glColor3ub( 128, 128, 128 );

    // draw model
    glEnableClientState( GL_VERTEX_ARRAY );
    glEnableClientState( GL_NORMAL_ARRAY );
    glVertexPointer( 3, GL_FLOAT, sizeof(Vertex), &model[0].position );
    glNormalPointer( GL_FLOAT, sizeof(Vertex), &model[0].normal );
    glDrawArrays( GL_TRIANGLES, 0, model.size() );
    glDisableClientState( GL_VERTEX_ARRAY );
    glDisableClientState( GL_NORMAL_ARRAY );

    // draw bounding cube
    glDisable( GL_LIGHTING );
    glColor3ub( 255, 255, 255 );
    glutWireCube( 7 );
    glEnable( GL_LIGHTING );

    glPopMatrix();

    glutSwapBuffers();
}

// return the x/y/z min/max of some geometry
template< typename Vec >
pair< Vec, Vec > GetExtents
    (
    const Vec* pts,
    size_t stride,
    size_t count
    )
{
    typedef typename Vec::value_type Scalar;
    Vec pmin( std::numeric_limits< Scalar >::max() );
    Vec pmax( std::min( std::numeric_limits< Scalar >::min(),
                        (Scalar)-std::numeric_limits< Scalar >::max() ) );

    // find extents
    unsigned char* base = (unsigned char*)pts;
    for( size_t i = 0; i < count; ++i )
    {
        const Vec& pt = *(Vec*)base;
        pmin = glm::min( pmin, pt );
        pmax = glm::max( pmax, pt );
        base += stride;
    }

    return make_pair( pmin, pmax );
}

// centers geometry around the origin
// and scales it to fit in a size^3 box
template< typename Vec >
void CenterAndScale
    (
    Vec* pts,
    size_t stride,
    size_t count,
    const typename Vec::value_type& size
    )
{
    typedef typename Vec::value_type Scalar;

    // get min/max extents
    pair< Vec, Vec > exts = GetExtents( pts, stride, count );

    // center and scale
    const Vec center = ( exts.first * Scalar( 0.5 ) ) + ( exts.second * Scalar( 0.5f ) );

    const Scalar factor = size / glm::compMax( exts.second - exts.first );
    unsigned char* base = (unsigned char*)pts;
    for( size_t i = 0; i < count; ++i )
    {
        Vec& pt = *(Vec*)base;
        pt = ((pt - center) * factor);
        base += stride;
    }
}

/// @brief Guard that will acquire the GIL upon construction, and
///        restore its state upon destruction.
class with_gil
{
public:
    with_gil()  { state_ = PyGILState_Ensure(); }
    ~with_gil() { PyGILState_Release(state_);   }

    with_gil(const with_gil&)            = delete;
    with_gil& operator=(const with_gil&) = delete;
private:
    PyGILState_STATE state_;
};

/// @brief Guard that will unlock the GIL upon construction, and
///        restore its staet upon destruction.
class without_gil
{
public:
    without_gil()  { state_ = PyEval_SaveThread(); }
    ~without_gil() { PyEval_RestoreThread(state_); }

    without_gil(const without_gil&)            = delete;
    without_gil& operator=(const without_gil&) = delete;
private:
    PyThreadState* state_;
};

/// @brief Guard that provides higher-level GIL controls.
class gil_guard
{
public:
    struct no_acquire_t {} // tag type used for gil acquire strategy
    static no_acquire;

    gil_guard()             { acquire(); }
    gil_guard(no_acquire_t) { release(); }
    ~gil_guard()            { while (!stack_.empty()) { restore(); } }

    void acquire()          { stack_.emplace(new with_gil); }
    void release()          { stack_.emplace(new without_gil); }
    void restore()          { stack_.pop(); }

    static bool owns_gil()
    {
    // For Python 3.4+, one can use `PyGILState_Check()`.
    return _PyThreadState_Current == PyGILState_GetThisThreadState();
    }

    gil_guard(const gil_guard&)            = delete;
    gil_guard& operator=(const gil_guard&) = delete;

private:
    // Use std::shared_ptr<void> for type erasure.
    std::stack<std::shared_ptr<void>> stack_;
};

struct stShowModel
{
    void vReflashModel()
    {
        assert(gil_guard::owns_gil());
        gil_guard gil(gil_guard::no_acquire);
        assert(!gil.owns_gil());

        ifstream ifile( "/home/jxgu/github/CamControl_3D_Objects/gargoyle.m" );
        model = LoadM( ifile );
        if( model.empty() )
        {
            cerr << "Empty model!" << endl;
        }

        CenterAndScale( &model[0].position, sizeof( Vertex ), model.size(), 7 );

        int iArgc = 1;
        char *pcArgv = "Window";
        glutInit( &iArgc, &pcArgv );
        glutInitDisplayMode( GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE );
        glutInitWindowSize( 640, 480 );
        glutCreateWindow( "GLUT" );
        glutDisplayFunc( display );

        glutPassiveMotionFunc(PassMotion);

        glEnable( GL_DEPTH_TEST );

        // set up "headlamp"-like light
        glShadeModel( GL_SMOOTH );
        glEnable( GL_COLOR_MATERIAL );
        glColorMaterial( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE ) ;
        glEnable( GL_LIGHTING );
        glEnable( GL_LIGHT0 );
        glMatrixMode( GL_MODELVIEW );
        glLoadIdentity();
        GLfloat position[] = { 0, 0, 1, 0 };
        glLightfv( GL_LIGHT0, GL_POSITION, position );

        glPolygonMode( GL_FRONT, GL_FILL );
        glPolygonMode( GL_BACK, GL_LINE );
        glutMainLoop();
    };
    void vCameraControl(int iSX1, int iSY1, int iSX2, int iSY2, int iSteadyFlag, int iStart)
    {
        int iFlagRot = 0;
        if(iSteadyFlag == 1)
        {
            this->iSteadyReady = 1;
        }
        else if(iSteadyFlag == 2)
        {
            this->iSteadyReady = 0;
        }
        if(iSteadyReady == 1)
        {
            vMotion(iSX1, iSY1, iSX2, iSY2, iFlagRot, iStart);
        }
    }
    int iSteadyReady = 0;
};

BOOST_PYTHON_MODULE(LibShowModel)
{
    // Force the GIL to be created and initialized.  The current caller will
    // own the GIL.
    PyEval_InitThreads();

    using namespace boost::python;
    class_<stShowModel>("stShowModel")
        .def("vReflashModel", &stShowModel::vReflashModel)
        .def("vCameraControl", &stShowModel::vCameraControl)
        ;
}

/*
BOOST_PYTHON_MODULE(LibShowModel)
{
    // Force the GIL to be created and initialized.  The current caller will
    // own the GIL.
    PyEval_InitThreads();

    namespace python = boost::python;
    python::def("f", +[] {
        // For exposition, assert caller owns GIL before and after
        // invoking function `f()`.
        assert(gil_guard::owns_gil());
        f();
        assert(gil_guard::owns_gil());
    });
}
*/


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <Eigen/Eigen>
#include "compute_smd_tlsph_strain.h"
#include "atom.h"
#include "update.h"
#include "modify.h"
#include "comm.h"
#include "force.h"
#include "memory.h"
#include "error.h"
#include "pair.h"

using namespace Eigen;
using namespace std;
using namespace LAMMPS_NS;

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

ComputeSMDTLSPHstrain::ComputeSMDTLSPHstrain(LAMMPS *lmp, int narg, char **arg) :
                Compute(lmp, narg, arg) {
        if (narg != 3)
                error->all(FLERR, "Illegal compute smd/tlsph_strain command");

        peratom_flag = 1;
        size_peratom_cols = 6;

        nmax = 0;
        strainVector = NULL;
}

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

ComputeSMDTLSPHstrain::~ComputeSMDTLSPHstrain() {
        memory->sfree(strainVector);
}

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

void ComputeSMDTLSPHstrain::init() {

        int count = 0;
        for (int i = 0; i < modify->ncompute; i++)
                if (strcmp(modify->compute[i]->style, "smd/tlsph_strain") == 0)
                        count++;
        if (count > 1 && comm->me == 0)
                error->warning(FLERR, "More than one compute smd/tlsph_strain");
}

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

void ComputeSMDTLSPHstrain::compute_peratom() {
        double **defgrad0 = atom->smd_data_9;

        invoked_peratom = update->ntimestep;

        // grow vector array if necessary

        if (atom->nmax > nmax) {
                memory->destroy(strainVector);
                nmax = atom->nmax;
                memory->create(strainVector, nmax, size_peratom_cols, "strainVector");
                array_atom = strainVector;
        }

        // copy data to output array
        int itmp = 0;
        Matrix3d *Fincr = (Matrix3d *) force->pair->extract("smd/tlsph/Fincr_ptr", itmp);
        if (Fincr == NULL) {
                error->all(FLERR, "compute smd/tlsph_strain failed to access Fincr array");
        }

        int *mask = atom->mask;
        int nlocal = atom->nlocal;
        Matrix3d E, eye, Ftotal, F0;
        eye.setIdentity();

        for (int i = 0; i < nlocal; i++) {
                if (mask[i] & groupbit) {

                        // old deformation gradient
                        F0(0, 0) = defgrad0[i][0];
                        F0(0, 1) = defgrad0[i][1];
                        F0(0, 2) = defgrad0[i][2];
                        F0(1, 0) = defgrad0[i][3];
                        F0(1, 1) = defgrad0[i][4];
                        F0(1, 2) = defgrad0[i][5];
                        F0(2, 0) = defgrad0[i][6];
                        F0(2, 1) = defgrad0[i][7];
                        F0(2, 2) = defgrad0[i][8];

                        // compute current total deformation gradient
                        Ftotal = F0 * Fincr[i]; // this is the total deformation gradient: reference deformation times incremental deformation

                        E = 0.5 * (Ftotal.transpose() * Ftotal - eye); // Green-Lagrange strain
                        strainVector[i][0] = E(0, 0);
                        strainVector[i][1] = E(1, 1);
                        strainVector[i][2] = E(2, 2);
                        strainVector[i][3] = E(0, 1);
                        strainVector[i][4] = E(0, 2);
                        strainVector[i][5] = E(1, 2);
                } else {
                        for (int j = 0; j < size_peratom_cols; j++) {
                                strainVector[i][j] = 0.0;
                        }
                }
        }
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based array
 ------------------------------------------------------------------------- */

double ComputeSMDTLSPHstrain::memory_usage() {
        double bytes = size_peratom_cols * nmax * sizeof(double);
        return bytes;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DISTS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DISTS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing dists capabilities

    Computes the (saturated) absolute value of the difference of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = dists(x, y);
    @endcode

    is similar to:

    @code
    T r = abss(x-y);
    @endcode

    @par Note

    The result is never stricly negative moreover it satisfies the predicate

     @code  is_positive(dist(x, y)) || is_unord(x, y) @endcode

    For floating, it can of course be @ref Nan.

    @see  abss, ulpdist

  **/
  const boost::dispatch::functor<tag::dists_> dists = {};
} }
#endif

#include <boost/simd/function/scalar/dists.hpp>
#include <boost/simd/function/simd/dists.hpp>

#endif


#pragma once

// deal.II includes -------------------------------------------------------------
#include <deal.II/base/index_set.h>
#include <deal.II/base/point.h>
#include <deal.II/base/utilities.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/trilinos_vector.h>
#include <deal.II/numerics/data_out.h>

// system includes --------------------------------------------------------------
#include <boost/static_assert.hpp>
#include <iomanip>
#include <iostream>
#include <vector>
// own includes -----------------------------------------------------------------
#include "aux/filtered_data_out.hpp"
#include "export/export_vtk.hpp"
#include "post_processing/compute_mean_value.hpp"
#include "post_processing/energy.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"

namespace boltzmann {
/**
 *
 *
 * @param domain
 *
 * @return
 */
template <int dim>
class Moments2VTK
{
 private:
  typedef dealii::TrilinosWrappers::Vector vector_t;  // this class is deprecated
  typedef dealii::TrilinosWrappers::MPI::Vector mpi_vector_t;
  typedef dealii::DoFHandler<dim> dh_t;

 public:
  /**
   *
   * @param dh DoFHandler
   * @param sb spectral basis
   * @param epetra_map_phys local dofs (physical domain)
   * @param local_dofs  local dofs (phase space domain)
   * @param local_relevant_dofs local relevant dofs (phase space domain)
   */
  template <typename SB>
  Moments2VTK(const dh_t& dh, const SB& sb)
      : dof_handler(dh)
      , data_out(dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD))
  {
    BOOST_STATIC_ASSERT(dim == 2);
    energy.init(sb);
    momentum.init(sb);
    mass.init(sb);
    pid = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
    this->locally_relevant_phys_dofs =
        dealii::DoFTools::dof_indices_with_subdomain_association(dh, pid);
    // initialize vectors with ghost cells
    v_mass.reinit(locally_relevant_phys_dofs);
    v_energy.reinit(locally_relevant_phys_dofs);
    v_momentumX.reinit(locally_relevant_phys_dofs);
    v_momentumY.reinit(locally_relevant_phys_dofs);

    if (pid == 0) {
      // clear file
      std::ofstream fout(fxdmf, std::ios_base::out);
      fout << "<!-- Problems reading this file? Encasing XDMF tags might be missing ... -->";
      fout.close();
    }
  }

  ~Moments2VTK();

  /**
   * @brief write results to parallel filesystem in HDF5-Format
   *
   * @param src           solution vector with ghost elements
   * @param timestep_no
   * @param present_time
   * @param indexer
   */
  template <typename VEC_IN, typename INDEXER>
  void run(const VEC_IN& src,
           const size_t timestep_no,
           const double present_time,
           const INDEXER& indexer);

  template <typename VEC_IN, typename INDEXER>
  void run_debug(const VEC_IN& src,
                 const size_t timestep_no,
                 const double present_time,
                 const INDEXER& indexer,
                 const mpi_vector_t& scores);

  const vector_t& get_mass() const { return v_mass; }

 private:
  const dh_t& dof_handler;
  /// tasks
  Energy energy;
  Momentum momentum;
  Mass mass;
  // internal storage
  mpi_vector_t v_mass;
  mpi_vector_t v_energy;
  mpi_vector_t v_momentumX;
  mpi_vector_t v_momentumY;
  /// locally relevant *phys* DoFs
  dealii::IndexSet locally_relevant_phys_dofs;
  unsigned int pid;
  // IO
  dealii::FilteredDataOut<dim> data_out;
  std::vector<dealii::XDMFEntry> xdmf_entries;
  std::string fxdmf = "solution.xdmf";
};

// ----------------------------------------------------------------------
template <int dim>
template <typename VEC_IN, typename INDEXER>
void
Moments2VTK<dim>::run(const VEC_IN& src,
                      const size_t timestep_no,
                      const double present_time,
                      const INDEXER& indexer)
{
  // collect ghost entries
  auto write_to_stdout = [&](std::string tag, double value) {
    std::cout << "moments::monitor\t" << std::setw(15) << tag << std::setw(15) << timestep_no
              << std::scientific << std::setprecision(20) << std::setw(30) << value << std::endl;
  };

  dealii::FilteredDataOut<dim> data_out(pid);
  dealii::DataOutBase::VtkFlags flags = dealii::DataOutBase::VtkFlags();
  const bool filter_duplicate_vertices = true;
  const bool xdmf_hdf5_output = true;
  typedef dealii::DataOutBase::DataOutFilterFlags data_out_filter_flags;
  dealii::DataOutBase::DataOutFilter data_out_filter(
      data_out_filter_flags(filter_duplicate_vertices, xdmf_hdf5_output));
  data_out.set_flags(flags);
  data_out.attach_dof_handler(dof_handler);
  energy.compute(v_energy, src, locally_relevant_phys_dofs, indexer);
  data_out.add_data_vector(v_energy, "E");
  double e = mpi::compute_mean_value(dof_handler, v_energy);
  if (pid == 0) write_to_stdout("Energy", e);
  // mass
  mass.compute(v_mass, src, locally_relevant_phys_dofs, indexer);
  data_out.add_data_vector(v_mass, "M");
  double rho = mpi::compute_mean_value(dof_handler, v_mass);
  if (pid == 0) write_to_stdout("Mass", rho);
  // momentum
  momentum.compute(v_momentumX, v_momentumY, src, locally_relevant_phys_dofs, indexer);
  double ux = mpi::compute_mean_value(dof_handler, v_momentumX);
  double uy = mpi::compute_mean_value(dof_handler, v_momentumY);
  if (pid == 0) {
    write_to_stdout("ux", ux);
    write_to_stdout("uy", uy);
  }
  // ------------------------------------------------------------
  // VTK output
  data_out.add_data_vector(v_momentumX, "Ux");
  data_out.add_data_vector(v_momentumY, "Uy");
  data_out.build_patches();

  std::string filename = "solution-" + dealii::Utilities::int_to_string(timestep_no, 6) + ".hdf5";
  data_out.write_filtered_data(data_out_filter);
  data_out.write_hdf5_parallel(
      data_out_filter, timestep_no == 0, "mesh.hdf5", filename, MPI_COMM_WORLD);

  // xdmf entry
  auto xdmf_entry = data_out.create_xdmf_entry(
      data_out_filter, "mesh.hdf5", filename, present_time, MPI_COMM_WORLD);
  // xdmf_entries.push_back(xdmf_entry);

  // TODO: for 20K timesteps takes > 0.5sec to write...
  if (pid == 0) {
    std::string xdmf_str = xdmf_entry.get_xdmf_content(1 /* indent level */);
    std::ofstream fout(fxdmf, std::ios_base::out | std::ios_base::app);
    fout << xdmf_str << std::endl;
    fout.close();
  }

  //  data_out.write_xdmf_file(xdmf_entries, "solution.xdmf", MPI_COMM_WORLD);
}

// ----------------------------------------------------------------------
template <int dim>
template <typename VEC_IN, typename INDEXER>
void
Moments2VTK<dim>::run_debug(const VEC_IN& src,
                            const size_t timestep_no,
                            const double present_time,
                            const INDEXER& indexer,
                            const mpi_vector_t& scores)
{
  // collect ghost entries
  auto write_to_stdout = [&](std::string tag, double value) {
    std::cout << "moments::monitor\t" << std::setw(15) << tag << std::setw(15) << timestep_no
              << std::scientific << std::setprecision(20) << std::setw(30) << value << std::endl;
  };

  dealii::FilteredDataOut<dim> data_out(pid);
  dealii::DataOutBase::VtkFlags flags = dealii::DataOutBase::VtkFlags();
  const bool filter_duplicate_vertices = true;
  const bool xdmf_hdf5_output = true;
  typedef dealii::DataOutBase::DataOutFilterFlags data_out_filter_flags;
  dealii::DataOutBase::DataOutFilter data_out_filter(
      data_out_filter_flags(filter_duplicate_vertices, xdmf_hdf5_output));
  data_out.set_flags(flags);
  data_out.attach_dof_handler(dof_handler);
  energy.compute(v_energy, src, locally_relevant_phys_dofs, indexer);
  data_out.add_data_vector(v_energy, "E");
  double e = mpi::compute_mean_value(dof_handler, v_energy);
  if (pid == 0) write_to_stdout("Energy", e);
  // mass
  mass.compute(v_mass, src, locally_relevant_phys_dofs, indexer);
  data_out.add_data_vector(v_mass, "M");
  double rho = mpi::compute_mean_value(dof_handler, v_mass);
  if (pid == 0) write_to_stdout("Mass", rho);
  // momentum
  momentum.compute(v_momentumX, v_momentumY, src, locally_relevant_phys_dofs, indexer);
  double ux = mpi::compute_mean_value(dof_handler, v_momentumX);
  double uy = mpi::compute_mean_value(dof_handler, v_momentumY);
  if (pid == 0) {
    write_to_stdout("ux", ux);
    write_to_stdout("uy", uy);
  }
  // ------------------------------------------------------------
  // VTK output
  data_out.add_data_vector(v_momentumX, "Ux");
  data_out.add_data_vector(v_momentumY, "Uy");
  data_out.add_data_vector(scores, "Scores");
  data_out.build_patches();

  std::string filename = "solution-" + dealii::Utilities::int_to_string(timestep_no, 6) + ".hdf5";
  data_out.write_filtered_data(data_out_filter);
  data_out.write_hdf5_parallel(
      data_out_filter, timestep_no == 0, "mesh.hdf5", filename, MPI_COMM_WORLD);

  // xdmf entry
  auto xdmf_entry = data_out.create_xdmf_entry(
      data_out_filter, "mesh.hdf5", filename, present_time, MPI_COMM_WORLD);
  // xdmf_entries.push_back(xdmf_entry);

  // TODO: for 20K timesteps takes > 0.5sec to write...
  if (pid == 0) {
    std::string xdmf_str = xdmf_entry.get_xdmf_content(1 /* indent level */);
    std::ofstream fout(fxdmf, std::ios_base::out | std::ios_base::app);
    fout << xdmf_str << std::endl;
    fout.close();
  }

  //  data_out.write_xdmf_file(xdmf_entries, "solution.xdmf", MPI_COMM_WORLD);
}

// ----------------------------------------------------------------------
template <int dim>
Moments2VTK<dim>::~Moments2VTK()
{
  //  data_out.write_xdmf_file(xdmf_entries, "solution.xdmf", MPI_COMM_WORLD);
}
}


// 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__LIE_GROUP_BASE_HPP_
#define SMOOTH__LIE_GROUP_BASE_HPP_

#include <Eigen/Core>

namespace smooth {

/**
 * @brief Type trait that maps a type to Lie group operations.
 *
 * Required members:
 *  - `typename Impl`: Lie group operations
 *  - `typename Scalar`: Scalar type (\p float, \p double, ...)
 *  - `typename template<NewScalar> PlainObject`: Default return type
 */
template<typename T>
struct lie_traits;

/**
 * @brief Base class for Lie group types
 */
template<typename Derived>
class LieGroupBase
{
  Derived & derived() { return static_cast<Derived &>(*this); }

  const Derived & cderived() const { return static_cast<const Derived &>(*this); }

protected:
  LieGroupBase() = default;

  //! CRTP traits
  using traits = lie_traits<Derived>;

  //! Group-specific Lie group implementation
  using Impl = typename traits::Impl;

  //! Plain return type
  template<typename NewScalar>
  using PlainObjectCast = typename traits::template PlainObject<NewScalar>;

  //! True if underlying storage supports modification
  static constexpr bool is_mutable = traits::is_mutable;

public:
  //! Number of scalars in internal representation.
  static constexpr Eigen::Index RepSize = Impl::RepSize;
  //! Degrees of freedom of manifold (equal to tangent space dimension).
  static constexpr Eigen::Index Dof = Impl::Dof;
  //! Side of Lie group matrix representation.
  static constexpr Eigen::Index Dim = Impl::Dim;

  //! Scalar type
  using Scalar = typename traits::Scalar;
  //! Lie group matrix type
  using Matrix = Eigen::Matrix<Scalar, Dim, Dim>;
  //! Lie group parameterized tangent type
  using Tangent = Eigen::Matrix<Scalar, Dof, 1>;
  //! Matrix representing map between tangent elements
  using TangentMap = Eigen::Matrix<Scalar, Dof, Dof>;
  //! Plain return type
  using PlainObject = PlainObjectCast<Scalar>;

  /**
   * Assignment operation from other storage type.
   */
  template<typename OtherDerived>
  // \cond
  requires is_mutable && std::is_same_v<Impl, typename lie_traits<OtherDerived>::Impl>
  // \endcond
  Derived & operator=(const LieGroupBase<OtherDerived> & o)
  {
    derived().coeffs() = static_cast<const OtherDerived &>(o).coeffs();
    return derived();
  }

  // Group API

  /**
   * @brief Static size (degrees of freedom).
   */
  static constexpr Eigen::Index SizeAtCompileTime = Dof;

  /**
   * @brief Dynamic size (degrees of freedom).
   */
  Eigen::Index size() const { return Dof; }

  /**
   * @brief Set to group identity element.
   */
  void setIdentity() { Impl::setIdentity(derived().coeffs()); }

  /**
   * @brief Set to a random element.
   *
   * Set the seed with \p std::srand(unsigned).
   */
  void setRandom() { Impl::setRandom(derived().coeffs()); }

  /**
   * @brief Construct the identity element.
   */
  static PlainObject Identity()
  {
    PlainObject ret;
    ret.setIdentity();
    return ret;
  }

  /**
   * @brief Construct a random element.
   *
   * Set the seed with \p std::srand(unsigned).
   */
  static PlainObject Random()
  {
    PlainObject ret;
    ret.setRandom();
    return ret;
  }

  /**
   * @brief Return as matrix Lie group element in \f$ \mathbb{R}^{\mathtt{dim} \times \mathtt{dim}}
   * \f$.
   */
  Matrix matrix() const
  {
    Matrix ret;
    Impl::matrix(cderived().coeffs(), ret);
    return ret;
  }

  /**
   * @brief Check if (approximately) equal to other element `o`.
   */
  template<typename OtherDerived>
  // \cond
  requires std::is_same_v<Impl, typename lie_traits<OtherDerived>::Impl>
  // \endcond
  bool isApprox(const LieGroupBase<OtherDerived> & o,
    const Scalar & eps = Eigen::NumTraits<Scalar>::dummy_precision()) const
  {
    return cderived().coeffs().isApprox(static_cast<const OtherDerived &>(o).coeffs(), eps);
  }

  /**
   * @brief Cast to different scalar type.
   */
  template<typename NewScalar>
  PlainObjectCast<NewScalar> cast() const
  {
    PlainObjectCast<NewScalar> ret;
    ret.coeffs() = cderived().coeffs().template cast<NewScalar>();
    return ret;
  }

  /**
   * @brief Group binary composition operation.
   */
  template<typename OtherDerived>
  // \cond
  requires std::is_same_v<Impl, typename lie_traits<OtherDerived>::Impl>
  // \endcond
  PlainObject operator*(const LieGroupBase<OtherDerived> & o) const
  {
    PlainObject ret;
    Impl::composition(
      cderived().coeffs(), static_cast<const OtherDerived &>(o).coeffs(), ret.coeffs());
    return ret;
  }

  /**
   * @brief Inplace group binary composition operation.
   */
  template<typename OtherDerived>
  // \cond
  requires is_mutable && std::is_same_v<Impl, typename lie_traits<OtherDerived>::Impl>
  // \endcond
  Derived & operator*=(const LieGroupBase<OtherDerived> & o)
  {
    derived().coeffs() = (*this * o).coeffs();
    return derived();
  }

  /**
   * @brief Group inverse operation.
   */
  PlainObject inverse() const
  {
    PlainObject ret;
    Impl::inverse(cderived().coeffs(), ret.coeffs());
    return ret;
  }

  /**
   * @brief Lie group logarithm.
   *
   * @return tangent logarithm element.
   */
  Tangent log() const
  {
    Tangent ret;
    Impl::log(cderived().coeffs(), ret);
    return ret;
  }

  /**
   * @brief Lie group adjoint.
   *
   * @return Matrix \f$ \mathbf{Ad}_\mathbf{X} \f$ s.t.  \f$ \mathbf{Ad_X} \mathbf{a} = ( \mathbf{X}
   * \mathbf{a}^\wedge \mathbf{X}^{-1} )^\vee \f$
   */
  TangentMap Ad() const
  {
    TangentMap ret;
    Impl::Ad(cderived().coeffs(), ret);
    return ret;
  }

  /**
   * @brief Right-plus.
   *
   * @return \f$ \mathbf{x} \oplus \mathbf{a} = \mathbf{x} \circ \exp(\mathbf{a}) \f$
   */
  template<typename TangentDerived>
  PlainObject operator+(const Eigen::MatrixBase<TangentDerived> & a) const
  {
    return *this * exp(a);
  }

  /**
   * @brief Inplace right-plus: \f$ \mathbf{x} \leftarrow \mathbf{x} \circ \exp(\mathbf{a}) \f$.
   *
   * @return Reference to this
   */
  template<typename TangentDerived>
  // \cond
  requires is_mutable
  // \endcond
  Derived & operator+=(const Eigen::MatrixBase<TangentDerived> & a)
  {
    *this *= exp(a);
    return derived();
  }

  /**
   * @brief Right-minus.
   *
   * @return Tangent space element \f$ \mathbf{a} \f$ such that \f$ \mathbf{this} = \mathbf{xo}
   * \oplus \mathbf{a} \f$.
   *
   * \f[
   *   \mathbf{x}_1 \ominus \mathbf{x}_2 := \log(\mathbf{x}_2^{-1} \circ \mathbf{x}_1)
   * \f]
   */
  template<typename OtherDerived>
  // \cond
  requires std::is_same_v<Impl, typename lie_traits<OtherDerived>::Impl>
  // \endcond
  Tangent operator-(const LieGroupBase<OtherDerived> & xo) const
  {
    return (xo.inverse() * *this).log();
  }

  // Tangent API

  /**
   * @brief Lie group exponential map.
   */
  template<typename TangentDerived>
  static PlainObject exp(const Eigen::MatrixBase<TangentDerived> & a)
  {
    PlainObject ret;
    Impl::exp(a, ret.coeffs());
    return ret;
  }

  /**
   * @brief Lie algebra hat map.
   *
   * Maps a parameterization \f$ a \in \mathbb{R}^\mathtt{dof} \f$ a matrix Lie algebra element
   * to the corresponding matrix Lie algebra element \f$ A \in \mathbb{R}^{\mathtt{dim} \times
   * \mathtt{dim}} \f$.
   *
   * @see vee for the inverse of hat
   */
  template<typename TangentDerived>
  static Matrix hat(const Eigen::MatrixBase<TangentDerived> & a)
  {
    Matrix ret;
    Impl::hat(a, ret);
    return ret;
  }

  /**
   * @brief Lie alebra vee map.
   *
   * Maps a matrix Lie algebra element \f$ A \in \mathbb{R}^{\texttt{dim} \times \mathtt{dim}} \f$
   * to the corresponding parameterization \f$ a \in \mathbb{R}^\mathtt{dof} \f$.
   *
   * @see hat for the inverse of vee
   */
  template<typename MatrixDerived>
  static Tangent vee(const Eigen::MatrixBase<MatrixDerived> & A)
  {
    Tangent ret;
    Impl::vee(A, ret);
    return ret;
  }

  /**
   * @brief Lie algebra adjoint.
   *
   * @return Matrix \f$ \mathbf{ad}_\mathbf{a} \f$ s.t. \f$ \mathbf{ad}_\mathbf{a} \mathbf{b} =
   * [\mathbf{a}, \mathbf{b}] \f$
   */
  template<typename TangentDerived>
  static TangentMap ad(const Eigen::MatrixBase<TangentDerived> & a)
  {
    TangentMap ret;
    Impl::ad(a, ret);
    return ret;
  }

  /**
   * @brief Lie algebra bracket.
   *
   * \f[
   * [ \mathbf{a}, \mathbf{b}] = \left( \mathbf{a}^\wedge \mathbf{b}^\wedge - \mathbf{b}^\wedge
   * \mathbf{a}^\wedge \right)^\vee. \f]
   */
  template<typename TangentDerived1, typename TangentDerived2>
  static Tangent lie_bracket(
    const Eigen::MatrixBase<TangentDerived1> & a, const Eigen::MatrixBase<TangentDerived2> & b)
  {
    return ad(a) * b;
  }
  /**
   * @brief Right jacobian of the exponential map.
   *
   * @return \f$ \mathrm{d}^r \exp_a \f$
   */
  template<typename TangentDerived>
  static TangentMap dr_exp(const Eigen::MatrixBase<TangentDerived> & a)
  {
    TangentMap ret;
    Impl::dr_exp(a, ret);
    return ret;
  }

  /**
   * @brief Inverse of right jacobian of the exponential map.
   *
   * @return \f$ \left( \mathrm{d}^r \exp_a \right)^{-1} \f$
   */
  template<typename TangentDerived>
  static TangentMap dr_expinv(const Eigen::MatrixBase<TangentDerived> & a)
  {
    TangentMap ret;
    Impl::dr_expinv(a, ret);
    return ret;
  }

  /**
   * @brief Left jacobian of the exponential map.
   *
   * @return \f$ \mathrm{d}^l \exp_a \f$
   */
  template<typename TangentDerived>
  static TangentMap dl_exp(const Eigen::MatrixBase<TangentDerived> & a)
  {
    return exp(a).Ad() * dr_exp(a);
  }

  /**
   * @brief Inverse of left jacobian of the exponential map.
   *
   * @return \f$ \left( \mathrm{d}^l \exp_a \right)^{-1} \f$
   */
  template<typename TangentDerived>
  static TangentMap dl_expinv(const Eigen::MatrixBase<TangentDerived> & a)
  {
    return -ad(a) + dr_expinv(a);
  }
};

/**
 * @brief Stream operator for Lie groups that ouputs the coefficients.
 */
template<typename Stream, typename Derived>
Stream & operator<<(Stream & s, const LieGroupBase<Derived> & g)
{
  s << static_cast<const Derived &>(g).coeffs().transpose();
  return s;
}

}  // namespace smooth

#endif  // SMOOTH__LIE_GROUP_BASE_HPP_


#ifndef PSP_TIME_H_IS_INCLUDED
#define PSP_TIME_H_IS_INCLUDED

#include <psp/cpuid.hh>
#include <psp/logging.hh>

#include <asm/ops.h>
#include <boost/chrono.hpp>
#undef assert
#include <base/assert.h>
#include <base/compiler.h>

extern float cycles_per_ns;

using hr_clock = boost::chrono::steady_clock;
typedef hr_clock::time_point tp;

uint64_t since_epoch(const tp &time);
uint64_t ns_diff(const tp &start, const tp &end);
tp take_time();

static const auto system_start_time = take_time();

static inline uint64_t take_start_time() {
    cpu_serialize();
    return rdtsc();
}

static inline void take_end_time(uint64_t *time) {
    *time = rdtscp(NULL);
    cpu_serialize();
}

inline int get_system_freq(void) {
    CPUID cpuID(0x16, 0x0);
    cycles_per_ns = cpuID.EAX() / 1000.0;
    printf("Processor Base Frequency:  %04d MHz (%.03f cycles per ns)\n", cpuID.EAX(), cycles_per_ns);
    return 0;
};

// From Adam's base OS
inline int time_calibrate_tsc(void) {
    struct timespec sleeptime;
    sleeptime.tv_nsec = 5E8; /* 1/2 second */
    struct timespec t_start, t_end;

    cpu_serialize();
    if (clock_gettime(CLOCK_MONOTONIC_RAW, &t_start) == 0) {
        uint64_t ns, end, start;
        double secs;

        start = rdtsc();
        nanosleep(&sleeptime, NULL);
        clock_gettime(CLOCK_MONOTONIC_RAW, &t_end);
        end = rdtscp(NULL);
        ns = ((t_end.tv_sec - t_start.tv_sec) * 1E9);
        ns += (t_end.tv_nsec - t_start.tv_nsec);

        secs = (double)ns / 1000;
        cycles_per_ns = ((uint64_t)((end - start) / secs)) / 1000.0;
        printf("time: detected %.03f ticks / us\n", cycles_per_ns);

        return 0;
    }

    return -1;
}

#endif /* PSP_TIME_H_IS_INCLUDED */


/*
 * scan.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <string>
#include <scitbx/constants.h>
#include <dxtbx/model/scan_helpers.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::deg_as_rad;
  using scitbx::rad_as_deg;

  static
  vec2<double> deg_as_rad(vec2<double> angles) {
    angles[0] = deg_as_rad(angles[0]);
    angles[1] = deg_as_rad(angles[1]);
    return angles;
  }

  static
  vec2<double> rad_as_deg(vec2<double> angles) {
    angles[0] = rad_as_deg(angles[0]);
    angles[1] = rad_as_deg(angles[1]);
    return angles;
  }

  static
  bool is_angle_in_range_wrapper(vec2<double> range, double angle,
      bool deg) {
    return is_angle_in_range(
      deg ? deg_as_rad(range) : range,
      deg ? deg_as_rad(angle) : angle);
  }

  static vec2 <double>
  get_range_of_mod2pi_angles_wrapper(vec2<double> range, double angle,
      bool deg) {
    return rad_as_deg(get_range_of_mod2pi_angles(
      deg ? deg_as_rad(range) : range,
      deg ? deg_as_rad(angle) : angle));
  }

  static scitbx::af::shared<double>
  get_mod2pi_angles_in_range_wrapper(vec2 <double> range, double angle,
      bool deg) {
    scitbx::af::shared<double> result = get_mod2pi_angles_in_range(
      deg ? deg_as_rad(range) : range,
      deg ? deg_as_rad(angle) : angle);
    if (deg) {
      for (std::size_t i = 0; i < result.size(); ++i) {
        result[i] = rad_as_deg(result[i]);
      }
    }
    return result;
  }

  void export_scan_helpers()
  {
    def("is_angle_in_range", &is_angle_in_range_wrapper, (
        arg("range"), arg("angle"), arg("deg") = false));

    def("get_range_of_mod2pi_angles", &get_range_of_mod2pi_angles_wrapper, (
        arg("range"), arg("angle"), arg("deg") = false));

    def("get_mod2pi_angles_in_range", &get_mod2pi_angles_in_range_wrapper, (
        arg("range"), arg("angle"), arg("deg") = false));
  }

}}} // namespace = dxtbx::model::boost_python


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void initParameters();
void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setPoissonRatio(const void *value, void *clientData);
void TW_CALL getPoissonRatio(void *value, void *clientData);
void TW_CALL setNormalizeStretch(const void *value, void *clientData);
void TW_CALL getNormalizeStretch(void *value, void *clientData);
void TW_CALL setNormalizeShear(const void *value, void *clientData);
void TW_CALL getNormalizeShear(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);

DemoBase *base;
const unsigned int width = 30;
const unsigned int depth = 5;
const unsigned int height = 5;
short simulationMethod = 2;

// main
int main( int argc, char **argv )
{
	REPORT_MEMORY_LEAKS

	base = new DemoBase();
	base->init(argc, argv, "Bar demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	initParameters();

	Simulation::getCurrent()->setSimulationMethodChangedCallback([&]() { reset(); initParameters(); base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep()); });

	// OpenGL
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
			" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "PoissonRatio", TW_TYPE_REAL, setPoissonRatio, getPoissonRatio, model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Strain based dynamics' ");

	glutMainLoop ();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void initParameters()
{
	TwRemoveAllVars(MiniGL::getTweakBar());
	TweakBarParameters::cleanup();

	MiniGL::initTweakBarParameters();

	TweakBarParameters::createParameterGUI();
	TweakBarParameters::createParameterObjectGUI(base);
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getModel());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getTimeStep());
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();

	Simulation::getCurrent()->getModel()->cleanup();
	buildModel();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTetModels().size(); i++)
	{
		model->getTetModels()[i]->updateMeshNormals(model->getParticles());
	}
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));

	createMesh();
}

void render ()
{
	base->render();
}

void createMesh()
{
	Vector3r points[width*height*depth];
	for (unsigned int i = 0; i < width; i++)
	{
		for (unsigned int j = 0; j < height; j++)
		{
			for (unsigned int k = 0; k < depth; k++)
			{
				points[i*height*depth + j*depth + k] = 0.3*Vector3r((Real)i, (Real)j, (Real)k);
			}
		}
	}

	vector<unsigned int> indices;
	for (unsigned int i = 0; i < width - 1; i++)
	{
		for (unsigned int j = 0; j < height - 1; j++)
		{
			for (unsigned int k = 0; k < depth - 1; k++)
			{
				// For each block, the 8 corners are numerated as:
				//     4*-----*7
				//     /|    /|
				//    / |   / |
				//  5*-----*6 |
				//   | 0*--|--*3
				//   | /   | /
				//   |/    |/
				//  1*-----*2
				unsigned int p0 = i*height*depth + j*depth + k;
				unsigned int p1 = p0 + 1;
				unsigned int p3 = (i + 1)*height*depth + j*depth + k;
				unsigned int p2 = p3 + 1;
				unsigned int p7 = (i + 1)*height*depth + (j + 1)*depth + k;
				unsigned int p6 = p7 + 1;
				unsigned int p4 = i*height*depth + (j + 1)*depth + k;
				unsigned int p5 = p4 + 1;

				// Ensure that neighboring tetras are sharing faces
				if ((i + j + k) % 2 == 1)
				{
					indices.push_back(p2); indices.push_back(p1); indices.push_back(p6); indices.push_back(p3);
					indices.push_back(p6); indices.push_back(p3); indices.push_back(p4); indices.push_back(p7);
					indices.push_back(p4); indices.push_back(p1); indices.push_back(p6); indices.push_back(p5);
					indices.push_back(p3); indices.push_back(p1); indices.push_back(p4); indices.push_back(p0);
					indices.push_back(p6); indices.push_back(p1); indices.push_back(p4); indices.push_back(p3);
				}
				else
				{
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p5); indices.push_back(p1);
					indices.push_back(p7); indices.push_back(p2); indices.push_back(p0); indices.push_back(p3);
					indices.push_back(p5); indices.push_back(p2); indices.push_back(p7); indices.push_back(p6);
					indices.push_back(p7); indices.push_back(p0); indices.push_back(p5); indices.push_back(p4);
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p7); indices.push_back(p5);
				}
			}
		}
	}
	SimulationModel *model = Simulation::getCurrent()->getModel();
	model->addTetModel(width*height*depth, (unsigned int)indices.size() / 4u, points, indices.data());

	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}
	for (unsigned int i = 0; i < 1; i++)
	{
		for (unsigned int j = 0; j < height; j++)
		{
			for (unsigned int k = 0; k < depth; k++)
				pd.setMass(i*height*depth + j*depth + k, 0.0);
		}
	}

	// init constraints
	for (unsigned int cm = 0; cm < model->getTetModels().size(); cm++)
	{
		const unsigned int nTets = model->getTetModels()[cm]->getParticleMesh().numTets();
		const unsigned int *tets = model->getTetModels()[cm]->getParticleMesh().getTets().data();
		const IndexedTetMesh::VertexTets *vTets = model->getTetModels()[cm]->getParticleMesh().getVertexTets().data();
		if (simulationMethod == 1)
		{
			const unsigned int offset = model->getTetModels()[cm]->getIndexOffset();
			const unsigned int nEdges = model->getTetModels()[cm]->getParticleMesh().numEdges();
			const IndexedTetMesh::Edge *edges = model->getTetModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model->addDistanceConstraint(v1, v2);
			}

			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addVolumeConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 2)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addFEMTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 3)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addStrainTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 4)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v[4] = { tets[4 * i], tets[4 * i + 1], tets[4 * i + 2], tets[4 * i + 3] };
				// Important: Divide position correction by the number of clusters
				// which contain the vertex.
				const unsigned int nc[4] = { vTets[v[0]].m_numTets, vTets[v[1]].m_numTets, vTets[v[2]].m_numTets, vTets[v[3]].m_numTets };
				model->addShapeMatchingConstraint(4, v, nc);
			}
		}
		model->getTetModels()[cm]->updateMeshNormals(pd);
	}

	LOG_INFO << "Number of tets: " << indices.size() / 4;
	LOG_INFO << "Number of vertices: " << width*height*depth;

}

void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*) clientData)->setValue<Real>(SimulationModel::SOLID_STIFFNESS, val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_STIFFNESS);
}

void TW_CALL setPoissonRatio(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_POISSON_RATIO, val);
}

void TW_CALL getPoissonRatio(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_POISSON_RATIO);
}

void TW_CALL setNormalizeStretch(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_NORMALIZE_STRETCH, val);
}

void TW_CALL getNormalizeStretch(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_NORMALIZE_STRETCH);
}

void TW_CALL setNormalizeShear(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_NORMALIZE_SHEAR, val);
}

void TW_CALL getNormalizeShear(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_NORMALIZE_SHEAR);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}


#include <boost/simd/memory/input_iterator.hpp>

#include <algorithm>
#include <iterator>
#include <iostream>
#include <vector>

int main()
{
  using boost::simd::pack;
  using boost::simd::input_end;
  using boost::simd::input_begin;
  using boost::simd::input_iterator;

  std::vector<float> data(16);
  typedef std::vector<float>::iterator it_t;

  for(std::size_t i=0;i<data.size();++i)
    data[i]  = 1 + i;

  input_iterator<it_t,4> vb = input_begin<4>(data.begin());
  input_iterator<it_t,4> ve = input_end<4>(data.end());

  std::copy(vb, ve, std::ostream_iterator< pack<float,4> >(std::cout, " "));
  std::cout << std::endl;
}


/*
 * Copyright 2014-2019, CNRS
 * Copyright 2018-2019, INRIA
 */

#include "eigenpy/eigenpy.hpp"
#include "eigenpy/geometry.hpp"
#include "eigenpy/solvers/solvers.hpp"
#include "eigenpy/solvers/preconditioners.hpp"

#include <iostream>
#include <boost/python/scope.hpp>

using namespace eigenpy;

BOOST_PYTHON_MODULE(eigenpy)
{
  enableEigenPy();
  exposeAngleAxis();
  exposeQuaternion();
  exposeGeometryConversion();

  {
    boost::python::scope solvers = boost::python::class_<SolversScope>("solvers");
    exposeSolvers();
    exposePreconditioners();
  }

}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_PIO_3_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_PIO_3_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\frac\pi{3}\f$.

    @par Semantic:

    For type T:

    @code
    T r = Pio_3<T>();
    @endcode

    is similar to:

    @code
    T r = Pi<T>()/Three<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Pio_3();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\frac\pi{3}\f$.

      Generate the  constant pio_3.

      @return The Pio_3 constant for the proper type
    **/
    const boost::dispatch::functor<tag::pio_3_> pio_3 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/pio_3.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*
 * File:   FitsHeader_tests.cc
 * Author: ts337
 *
 * Created on 07 February 2011, 12:41
 */
#include <UnitTest++/UnitTest++.h>
#include <stdexcept>
#include "../image_noise_sim.h"
#include "coela_utility/src/histogram_container.h"
//#include <tbb/tick_count.h>

#include <boost/filesystem.hpp>
#include <iostream>

using namespace coela;
using namespace std;

//SLOW!
///*
SUITE(Image_Noise_Sim)
{
    string test_suite_output_dir = "image_noise_sim_tests/";

    TEST(Notify_Suite_Has_Been_Run) {
        cout << "*** \"image_noise_sim_tests\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(Init_Unuran_Seed) {
        //Need to do this within a function, hence declaration within a TEST macro.
        std::vector < unsigned long > seed;

        seed.push_back(111);
        seed.push_back(222);
        seed.push_back(333);
        seed.push_back(444);
        seed.push_back(555);
        seed.push_back(666);

        //NB changing the seed may cause some tests to fail due to overly tight / crude statistical checks.

        if (! unuran::
                StreamWrapper::unuran_package_has_been_seeded()) {

            unuran::
            StreamWrapper::set_unuran_package_seed(seed);
        }

    }

    TEST(uniform_random_pixel_variate) {
        PixelArray2d<int> img(5, 5, 0);

        unuran::StreamWrapper rns;

        image_noise_simulation::uniform_random_pixel_variate pixel_picker(
            img.range(), rns);

        size_t n_picks=1e5;
        for (size_t i=0; i!=n_picks; ++i) {
            img(pixel_picker())+=1;
        }

        double prob = 1.0/(double)img.range().n_pix();
        double expected_val = (double)n_picks * prob;
        double expected_SD = sqrt(expected_val*(1.0 - prob));
        for (PixelIterator pix(img.range()); pix!=pix.end; ++pix) {
            CHECK_CLOSE(expected_val, img(pix), expected_SD*3);
        }

//        img.write_to_file(test_suite_output_dir+"uniformly_picked_pixels.fits");

    }
//

    TEST(intensity_map_random_pixel_variate_setup_time) {

    }

    TEST(intensity_map_random_pixel_variate_pick_time) {

    }

    TEST(Photon_Shot_Noise) { //Slow!
        PixelArray2d<double> intensity_map(50,50, 0.5);

        intensity_map(2,2)=3.14;
        intensity_map(3,3)=15.555;
        intensity_map(4,4)=5.14;

        vector< HistogramContainer14bit > hists;

        unuran::StreamWrapper rns;

        size_t n_its =2e3;

        PixelArray2d<double> avg_img(intensity_map);
        avg_img.assign(0);

//        tbb::tick_count t0;
//        t0 = tbb::tick_count::now();

        for (size_t i=0; i!=n_its; ++i) {
            PixelArray2d<int> photon_img =
                image_noise_simulation::generate_photon_arrival_map(
                    intensity_map,
                    intensity_map.sum(),
                    0.0,
                    rns);
            avg_img +=photon_img;
        }

//        tbb::tick_count t1 = tbb::tick_count::now();
//        double time =(t1-t0).seconds();
//
//        cout<<"Performed photon_shot_noise sim in "<<time<<" seconds "<<endl;

//        double expected_phots_per_it = intensity_map.sum();
//        cout <<time / n_its / expected_phots_per_it *1e9<< " nanoseconds per photon" <<endl;
//        cout<<"( Expected phots per frame: " << expected_phots_per_it<< " )" << endl;

        intensity_map.write_to_file(test_suite_output_dir+"photon_shot_test_intensity_map.fits");
        image_noise_simulation::generate_photon_arrival_map(
            intensity_map,
            intensity_map.sum(),
            0.0,
            rns
        ).write_to_file(
            test_suite_output_dir+"photon_shot_test_sample_instance.fits");

        avg_img/=n_its;
        avg_img.write_to_file(test_suite_output_dir+"photon_shot_test_avg_map.fits");

        for (PixelIterator p_it(avg_img.range()); p_it!=p_it.end; ++p_it) {
            CHECK_CLOSE(intensity_map(p_it), avg_img(p_it), sqrt(intensity_map(p_it))*0.5);
        }
    }

///*

//    TEST(EMCCD_image_sim_dark_image_zero_cic){
//        image<int> photon_map(5,5, 0);
//
//        CCD_models::EMCCD_model_parameters model_pars;
//        model_pars.photon_gain=100;
//        model_pars.serial_CIC_gain=10;
//        model_pars.CIC_rate_per_pix=0.0;
//        model_pars.readout_sigma=10.5;
//        model_pars.bias_pedestal=500;
//
//
////        photon_map(2,2)=1;
////        photon_map(3,3)=2;
////        photon_map(4,4)=3;
//
//        HistogramContainer14bit  hist;
//
//                std::vector < unsigned long > seed;
//
//        seed.push_back(111);
//        seed.push_back(222);
//        seed.push_back(333);
//        seed.push_back(444);
//        seed.push_back(555);
//        seed.push_back(666);
//
//        unuran::
//        StreamWrapper::set_unuran_package_seed(seed, false);
//
//
//
//
//        unuran::StreamWrapper rns;
//
//        CCD_models::EMCCD_model model(model_pars, rns);
//
//
//        size_t n_its =1e5;
//
//        image<double> avg_img(photon_map.range().x_dim(), photon_map.range().y_dim(), 0);
//
//        for (size_t i=0; i!=n_its;++i){
//            image<int> dark_frame =
//                image_noise_simulation::EMCCD_simulated_image(
//                    photon_map, model, rns);
//            avg_img +=dark_frame;
//
//            for (PixelIterator pix(dark_frame.range()); pix!=pix.end; ++pix){
//                hist.count_int_value(dark_frame(pix));
//            }
//        }
//
//        avg_img/=(double)n_its;
//
//        for (PixelIterator pix(avg_img.range()); pix!=pix.end; ++pix){
//            CHECK_CLOSE( model_pars.bias_pedestal, avg_img(pix), 3 * model_pars.readout_sigma / sqrt(n_its) );
//        }
//
//        avg_img.write_to_file(test_suite_output_dir+"EMCCD_dark_run_sim_CIC_0.fits");
//
//        hist.write_to_file(test_suite_output_dir+"EMCCD_dark_run_sim_hist_CIC_0.txt");
//
//
//    }

//    TEST(EMCCD_image_sim_dark_image_nonzero_CIC){
//        image<int> photon_map(5,5, 0);
//
//        CCD_models::EMCCD_model_parameters model_pars;
//        model_pars.photon_gain=100;
//        model_pars.serial_CIC_gain=10;
//        model_pars.CIC_rate_per_pix=0.04;
//        model_pars.readout_sigma=10.5;
//        model_pars.bias_pedestal=500;
//
//
////        photon_map(2,2)=1;
////        photon_map(3,3)=2;
////        photon_map(4,4)=3;
//
//        HistogramContainer14bit  hist;
//
//                std::vector < unsigned long > seed;
//
//        seed.push_back(111);
//        seed.push_back(222);
//        seed.push_back(333);
//        seed.push_back(444);
//        seed.push_back(555);
//        seed.push_back(666);
//
//        unuran::
//        StreamWrapper::set_unuran_package_seed(seed, false);
//
//
//
//
//        unuran::StreamWrapper rns;
//
//        CCD_models::EMCCD_model model(model_pars, rns);
//
//
//        size_t n_its =1e5;
//
//        image<double> avg_img(photon_map.range().x_dim(), photon_map.range().y_dim(), 0);
//
//        for (size_t i=0; i!=n_its;++i){
//            image<int> dark_frame =
//                image_noise_simulation::EMCCD_simulated_image(
//                    photon_map, model, rns);
//            avg_img +=dark_frame;
//
//            for (PixelIterator pix(dark_frame.range()); pix!=pix.end; ++pix){
//                hist.count_int_value(dark_frame(pix));
//            }
//        }
//
//        avg_img/=(double)n_its;
//
//        for (PixelIterator pix(avg_img.range()); pix!=pix.end; ++pix){
//            CHECK_CLOSE( model_pars.bias_pedestal + model_pars.CIC_rate_per_pix*model_pars.serial_CIC_gain,
//                    avg_img(pix), 4 * model_pars.readout_sigma / sqrt(n_its) );
//        }
//
//        avg_img.write_to_file(
//            test_suite_output_dir+"EMCCD_dark_run_sim_CIC_"+string_utils::ftoa(model_pars.CIC_rate_per_pix)+ ".fits");
//
//        hist.write_to_file(test_suite_output_dir+"EMCCD_dark_run_sim_hist_CIC_" +string_utils::ftoa(model_pars.CIC_rate_per_pix)+  ".txt");
//    }

    /*
        TEST(EMCCD_image_sim_uniform_photon_image_zero_CIC){
            int uniform_n_photons =1;
            image<int> photon_map(5,5, uniform_n_photons);

            CCD_models::EMCCD_model_parameters model_pars;
            model_pars.photon_gain=100;
            model_pars.serial_CIC_gain=10;
            model_pars.CIC_rate_per_pix=0.00;
            model_pars.readout_sigma=10.5;
            model_pars.bias_pedestal=500;

    //        photon_map(2,2)=1;
    //        photon_map(3,3)=2;
    //        photon_map(4,4)=3;

            HistogramContainer14bit  hist;

                    std::vector < unsigned long > seed;

            seed.push_back(111);
            seed.push_back(222);
            seed.push_back(333);
            seed.push_back(444);
            seed.push_back(555);
            seed.push_back(666);

            unuran::
            StreamWrapper::set_unuran_package_seed(seed, false);

            unuran::StreamWrapper rns;

            CCD_models::EMCCD_model model(model_pars, rns);

            size_t n_its =1e5;

            image<double> avg_img(photon_map.range().x_dim(), photon_map.range().y_dim(), 0);

            for (size_t i=0; i!=n_its;++i){
                image<int> dark_frame =
                    image_noise_simulation::EMCCD_simulated_image(
                        photon_map, model, rns);
                avg_img +=dark_frame;

                for (PixelIterator pix(dark_frame.range()); pix!=pix.end; ++pix){
                    hist.count_int_value(dark_frame(pix));
                }
            }

            avg_img/=(double)n_its;

            for (PixelIterator pix(avg_img.range()); pix!=pix.end; ++pix){
                CHECK_CLOSE( model_pars.bias_pedestal + uniform_n_photons*model_pars.photon_gain,
                        avg_img(pix),
                        3 * sqrt(model_pars.readout_sigma*model_pars.readout_sigma + uniform_n_photons*model_pars.photon_gain*model_pars.photon_gain )/ sqrt(n_its) );
            }

            avg_img.write_to_file(
                test_suite_output_dir+"EMCCD_uniform_photon_image_zero_CIC.fits");

            hist.write_to_file(test_suite_output_dir+"EMCCD_uniform_photon_image_zero_CIC_hist.txt");
        }

        TEST(EMCCD_image_sim_varying_photon_image_nonzero_CIC){

            image<int> photon_map(5,5, 0.0);

            CCD_models::EMCCD_model_parameters model_pars;
            model_pars.photon_gain=100;
            model_pars.serial_CIC_gain=10;
            model_pars.CIC_rate_per_pix=0.04;
            model_pars.readout_sigma=10.5;
            model_pars.bias_pedestal=500;

            photon_map(2,2)=1;
            photon_map(3,3)=2;
            photon_map(4,4)=3;

            HistogramContainer14bit  hist;

                    std::vector < unsigned long > seed;

            seed.push_back(111);
            seed.push_back(222);
            seed.push_back(333);
            seed.push_back(444);
            seed.push_back(555);
            seed.push_back(666);

            unuran::
            StreamWrapper::set_unuran_package_seed(seed, false);

            unuran::StreamWrapper rns;

            CCD_models::EMCCD_model model(model_pars, rns);

            size_t n_its =1e5;

            image<double> avg_img(photon_map.range().x_dim(), photon_map.range().y_dim(), 0);

            for (size_t i=0; i!=n_its;++i){
                image<int> dark_frame =
                    image_noise_simulation::EMCCD_simulated_image(
                        photon_map, model, rns);
                avg_img +=dark_frame;

                for (PixelIterator pix(dark_frame.range()); pix!=pix.end; ++pix){
                    hist.count_int_value(dark_frame(pix));
                }
            }

            avg_img/=(double)n_its;

            avg_img.write_to_file(
                test_suite_output_dir+"EMCCD_image_sim_varying_photon_image_nonzero_CIC.fits");

            hist.write_to_file(test_suite_output_dir+"EMCCD_image_sim_varying_photon_image_nonzero_CIC_hist.txt");
        }

        TEST(EMCCD_image_sim_stochastic_photon_image_nonzero_CIC){
            double uniform_prob_photons =0.1;
            image<float> intensity_map(5,5, uniform_prob_photons);

            CCD_models::EMCCD_model_parameters model_pars;
            model_pars.photon_gain=100;
            model_pars.serial_CIC_gain=10;
            model_pars.CIC_rate_per_pix=0.1;
            model_pars.readout_sigma=10.5;
            model_pars.bias_pedestal=500;

    //        photon_map(2,2)=1;
    //        photon_map(3,3)=2;
    //        photon_map(4,4)=3;

            HistogramContainer14bit  hist;

                    std::vector < unsigned long > seed;

            seed.push_back(111);
            seed.push_back(222);
            seed.push_back(333);
            seed.push_back(444);
            seed.push_back(555);
            seed.push_back(666);

            unuran::
            StreamWrapper::set_unuran_package_seed(seed, false);

            unuran::StreamWrapper rns;

            CCD_models::EMCCD_model model(model_pars, rns);

            size_t n_its =1e5;

            image<double> avg_img(intensity_map.range().x_dim(), intensity_map.range().y_dim(), 0);

            for (size_t i=0; i!=n_its;++i){
                image<int> photon_map = image_noise_simulation::photon_arrival_map(intensity_map, 1.0, rns);
                image<int> gain_cal_frame =
                    image_noise_simulation::EMCCD_simulated_image(
                        photon_map, model, rns);
                avg_img +=gain_cal_frame;

                for (PixelIterator pix(gain_cal_frame.range()); pix!=pix.end; ++pix){
                    hist.count_int_value(gain_cal_frame(pix));
                }
            }

            avg_img/=(double)n_its;

        avg_img.write_to_file(
                test_suite_output_dir+ string(UnitTest::CurrentTest::Details()->testName) +".fits");

            hist.write_to_file(test_suite_output_dir+string(UnitTest::CurrentTest::Details()->testName)+"_hist.txt");
        }

    */
}