//==================================================================================================
/*!
  @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/LICENSbd:E_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_TRIG_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_GENERIC_TRIG_REDUCTION_HPP_INCLUDED

#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/rem_pio2_cephes.hpp>
#include <boost/simd/function/rem_pio2_straight.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/is_ngt.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/function/inrad.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/medium_pi.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

//#include <nt2/sdk/meta/as_logical.hpp>
//#include <boost/simd/sdk/meta/is_upgradable.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    template< class A0
            , class unit_tag
            , class style
            , class mode
            , class base_A0 = bd::scalar_of_t<A0>
    >
    struct trig_reduction;

    // This class exposes the public static member:
    // reduce:                to provide range reduction
    //
    // unit_tag allows to choose statically the scaling  among radian_tag, pi_tag, degree_tag
    // meaning that the cosa function will (for example) define respectively
    // x-->cos(x)          (radian_tag),
    // x-->cos(p*x)        (pi_tag)
    // x-->cos((pi/180)*x) (degree_tag)
    //

    // trigonometric reduction strategies in the [-pi/4, pi/4] range.
    // these reductions are used in the accurate and fast
    // trigonometric functions with different policies

    template<class A0, class style, class mode>
    struct trig_reduction < A0, tag::radian_tag, style, mode>
    {
      using i_t = bd::as_integer_t<A0, signed>;
      using l_t = bs::as_logical_t<A0>;
      using conversion_allowed_t = std::false_type; //bd::is_upgradable_on_ext_<A0>; // TODO

      static BOOST_FORCEINLINE auto is_0_pio4_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Pio_4<A0>()))
      {
        return is_ngt(a0, Pio_4<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_pio2_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Pio_2<A0>()))
      {
        return is_ngt(a0, Pio_2<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_20pi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()))
      {
        return is_ngt(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()); //20 pi;
      }
      static BOOST_FORCEINLINE auto is_0_mpi_reduced (const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Medium_pi<A0>()))
      {
        return is_ngt(a0, Medium_pi<A0>()); //2^6 pi
      }
      static BOOST_FORCEINLINE auto is_0_dmpi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0,Constant<A0,262144>()))
      {
        return is_ngt(a0, Ratio<A0,262144>()); //2^18 pi
      }

      static BOOST_FORCEINLINE l_t cot_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }
      static BOOST_FORCEINLINE l_t tan_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr)  BOOST_NOEXCEPT
      {
        return inner_reduce(x, xr);
      }

      static BOOST_FORCEINLINE i_t inner_reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xx =  preliminary<mode>::clip(x);
        return select_mode(xx, xr, boost::mpl::int_<mode::start>());
      }

      template < class Mode, bool clipped = Mode::clipped>
      struct preliminary
      {
        static BOOST_FORCEINLINE A0 const& clip(const A0& x) BOOST_NOEXCEPT  { return x; }
      };

      template < class Mode>
      struct preliminary<Mode, true>
      {
        static BOOST_FORCEINLINE A0 clip(const A0& x) BOOST_NOEXCEPT
        {
          return clipto(x, boost::mpl::int_<Mode::range>());
        }
      private :
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_pio4> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_pio4_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_20pi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_20pi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_mpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_mpi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_dmpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_dmpi_reduced(x), x);
        }
      };

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const&
                  ) BOOST_NOEXCEPT
      {
        xr = xx;
        return Zero<i_t>();
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_pio4_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_pio2>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio4> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_pio4>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio2> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_pio2_reduced(xx)))
          return rem_pio2_straight(xx, xr);
        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_20pi>());
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const&
                  ) BOOST_NOEXCEPT
      {
        return rem_pio2_cephes(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_20pi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_mpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_20pi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_20pi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const&
                  ) BOOST_NOEXCEPT
      {
        return rem_pio2_medium(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_mpi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_dmpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_mpi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_mpi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_dmpi> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_dmpi_reduced(xx)))
           return use_conversion(xx, xr, style(), conversion_allowed_t());
        return rem_pio2(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & xx,  A0& xr
                    ,  const style &, std::false_type) BOOST_NOEXCEPT
      {
        return rem_pio2(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & xx,  A0& xr
                    ,  const tag::not_simd_type &, std::true_type) BOOST_NOEXCEPT
      {
        // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
        using uA0 = bd::upgrade_t<A0>;
        using aux_reduction = trig_reduction< uA0,tag::radian_tag,tag::not_simd_type,mode,double>;
        uA0 ux = xx, uxr;
        i_t n = static_cast<i_t>(aux_reduction::reduce(ux, uxr));
        xr = static_cast<A0>(uxr);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & x,  A0& xr
                    , const tag::simd_type &,std::true_type) BOOST_NOEXCEPT
      {
        // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
        using uA0 = bd::upgrade_t<A0>;
        using aux_reduc_t = trig_reduction< uA0, tag::radian_tag,  tag::simd_type, mode, double>;
        uA0 ux1, ux2, uxr1, uxr2;
        split(x, ux1, ux2);
        auto n1 = aux_reduc_t::reduce(ux1, uxr1);
        auto n2 = aux_reduc_t::reduce(ux2, uxr2);
        xr = group(uxr1, uxr2);
        split(xr, ux1, ux2);
        return group(n1, n2);
      }
    };

    template<class A0, class style>
    struct trig_reduction<A0,tag::degree_tag, style, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x*Ratio<A0,1,180>()))
      {
        return is_nez(x)&&is_flint(x*Ratio<A0,1,180>());
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>()))
      {
        return is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>());
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Ratio<A0,1,90>());
        A0 x2 = x - xi * Ratio<A0,90>();

        xr =  inrad(x2);
        return bs::fast_(toint)(xi);
      }
    };

#ifdef BOOST_SIMD_HAS_X87
    template<class A0>
    struct trig_reduction<A0,degree_tag, tag::not_simd_type, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x*Ratio<A0,1,180>()))
      {
        return is_nez(x)&&is_flint(x/Constant<A0,180>());
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint((x- Ratio<A0,90>())/Constant<A0,180>()))
      {
        return is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>());
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Ratio<A0,1,90>());
        A0 x2 = x - xi * Ratio<A0,90>();

        xr =  inrad(x2);
        return bs::fast_(toint)(xi);
      }
    };
#endif

    template < class A0, class style>
    struct trig_reduction < A0, tag::pi_tag,  style, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x))
      {
        return is_nez(x)&&is_flint(x);
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint(x-Half<A0>()))
      {
        return is_flint(x-Half<A0>()) ;
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x,  A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Two<A0>());
        A0 x2 = x - xi * Half<A0>();
        xr = x2*Pi<A0>();
        return fast_(toint)(xi);
      }
    };
  }
} }

#endif


#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <dials/array_family/reflection_table.h>
#include <cctbx/miller.h>

namespace xfel {
namespace merging {
namespace error_model {
namespace sdfac_refine {

typedef
 scitbx::af::versa<cctbx::miller::index<>, scitbx::af::flex_grid<> > shared_miller;

using namespace dials::af;

static scitbx::af::shared<double>
compute_normalized_deviations(reflection_table ISIGI, shared_miller hkl_list) {
  /*
   * This formulation of the normalized deviations of a set of intensities and sigmas is similar to that
   * described in Evans 2011, but includes the nn term as currently implmented by aimless
   *
   */
  SCITBX_ASSERT(ISIGI.contains("scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("meanprime_scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("isigi"));
  SCITBX_ASSERT(ISIGI.contains("nn"));

  scitbx::af::shared<double>         result(ISIGI.size(), 0);
  scitbx::af::const_ref<double>      scaled_intensity = ISIGI["scaled_intensity"];
  scitbx::af::const_ref<double>      meanprime_scaled_intensity = ISIGI["meanprime_scaled_intensity"];
  scitbx::af::const_ref<double>      isigi = ISIGI["isigi"];
  scitbx::af::const_ref<double>      nn = ISIGI["nn"];

  double sigma;
  #pragma omp parallel for private(sigma)
  for (int i = 0; i < ISIGI.size(); i++) {
    if (isigi[i] == 0 || scaled_intensity[i] == 0) continue;
    sigma = scaled_intensity[i] / isigi[i];
    result[i] = std::sqrt(nn[i]) * (scaled_intensity[i] - meanprime_scaled_intensity[i]) / sigma;
  }
  return result;
}

void
apply_sd_error_params(reflection_table ISIGI, const double sdfac, const double sdb, const double sdadd, const bool squared_params) {
  /*
   * Apply a set of sd params (sdfac, sdb and sdd) to an ISIGI reflection table

   Squared not only uses the squared formulation of sigma', but also fixes 2 bugs:
   1) Use meanI not meanIprime
   2) When returning isigi, don't multiply by slope

   If using squared, it is assumed that sdfac, sdb and sdadd have already been squared
   */
  SCITBX_ASSERT(ISIGI.contains("scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("mean_scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("meanprime_scaled_intensity"));
  SCITBX_ASSERT(ISIGI.contains("isigi"));
  SCITBX_ASSERT(ISIGI.contains("slope"));
  SCITBX_ASSERT(ISIGI.contains("miller_id"));

  scitbx::af::const_ref<double>      scaled_intensity = ISIGI["scaled_intensity"];
  scitbx::af::const_ref<double>      mean_scaled_intensity = ISIGI["mean_scaled_intensity"];
  scitbx::af::const_ref<double>      meanprime_scaled_intensity = ISIGI["meanprime_scaled_intensity"];
  scitbx::af::ref<double>            isigi = ISIGI["isigi"];
  scitbx::af::const_ref<double>      slope = ISIGI["slope"];
  scitbx::af::shared<double>         sigmas(ISIGI.size(), 0);
  scitbx::af::const_ref<std::size_t> miller_id = ISIGI["miller_id"];

  std::size_t max_miller_id = scitbx::af::max(miller_id);
  scitbx::af::shared<std::size_t>    n_refl(max_miller_id+1, 0);
  scitbx::af::shared<double>         isum(max_miller_id+1, 0);

  double tmp = 0;
  double sigma_corrected = 0;
  double meanI, meanIprime, minimum;
  #pragma omp parallel for private(meanI, meanIprime, minimum)
  for (int i = 0; i < ISIGI.size(); i++) {
    // scaled intensity (iobs/slope)
    // corrected sigma (original sigma/slope)
    sigmas[i] = scaled_intensity[i] / isigi[i];

    // apply correction parameters
    if (squared_params) {
      // use meanI, which is the mean of all observations of this hkl
      meanI = mean_scaled_intensity[i];
      tmp = std::pow(sigmas[i],2) + sdb * meanI + sdadd * std::pow(meanI,2);
    }
    else {
      // use meanIprime, which for each observation, is the mean of all other observations of this hkl
      meanIprime = meanprime_scaled_intensity[i];
      tmp = std::pow(sigmas[i],2) + sdb * meanIprime + std::pow(sdadd*meanIprime,2);
    }

    // avoid rare negatives
    minimum = 0.1 * std::pow(sigmas[i],2);
    if (tmp < minimum)
      tmp = minimum;

    if (squared_params) {
      sigma_corrected = std::sqrt(sdfac * tmp);
      SCITBX_ASSERT(sigma_corrected != 0.0);
      isigi[i] = scaled_intensity[i] / sigma_corrected;
    }
    else {
      sigma_corrected = sdfac * std::sqrt(tmp);
      SCITBX_ASSERT(sigma_corrected != 0.0);
      isigi[i] = scaled_intensity[i] * slope[i]/ sigma_corrected;
    }
  }
}

scitbx::af::shared<double> df_dpsq(scitbx::af::shared<double>all_sigmas_normalized,
                                   scitbx::af::shared<double>sigma_prime,
                                   scitbx::af::shared<double>dsigmasq_dpsq,
                                   reflection_table ISIGI,
                                   scitbx::af::shared<int>bin_indices,
                                   size_t n_bins) {
  scitbx::af::shared<double> g(n_bins, 0);
  scitbx::af::shared<double> dsigmanormsq_dpsq(ISIGI.size());

  scitbx::af::const_ref<double> scaled_intensity = ISIGI["scaled_intensity"];
  scitbx::af::const_ref<double> meanprime_scaled_intensity = ISIGI["meanprime_scaled_intensity"];
  scitbx::af::const_ref<double> nn = ISIGI["nn"];

  scitbx::af::shared<int> counts(n_bins);
  scitbx::af::shared<double> bnssq(n_bins);
  scitbx::af::shared<double> t3(n_bins);

  double tmp, c; int b;
  // OpenMP doesn't work here. Need a reduction. Easy in OpenMP 4.5 which isn't available yet for my gcc
  for (int i = 0; i < ISIGI.size(); i++) {
    tmp = scaled_intensity[i]-meanprime_scaled_intensity[i];
    c = nn[i] * tmp * tmp;
    dsigmanormsq_dpsq[i] = -c / std::pow(sigma_prime[i], 4) * dsigmasq_dpsq[i];

    b = bin_indices[i];
    counts[b]++;
    bnssq[b] += all_sigmas_normalized[i] * all_sigmas_normalized[i];
    t3[b] += dsigmanormsq_dpsq[i];
  }

  int n; double t1, t2;
  #pragma omp parallel for private(n, t1, t2)
  for (int b = 0; b < n_bins; b++) {
    n = counts[b];

    if (!(n == 0 || bnssq[b] == 0.)) {
      bnssq[b] /= n;
      t1 = 2 * (1 - std::sqrt(bnssq[b]));
      t2 = -0.5 / std::sqrt(bnssq[b]);
      t3[b] /= n;
      g[b] = t1 * t2 * t3[b];
    }
  }
  return g;
}

/* Jiffy function to compute statistics needed downstream
  For every observation, computes:
  mean_scaled_intensity: mean of all observations of this miller index
  meanprime_scaled_intensity: mean of all observations of this miller index except this observation
  n_refl: count of observed reflections for this miller index
  nn: n_refl-1/n_refl
*/
void setup_isigi_stats(reflection_table ISIGI, scitbx::af::const_ref<cctbx::miller::index<> > indices) {
  scitbx::af::shared<double> sumI                = scitbx::af::shared<double>(indices.size(), 0);
  scitbx::af::shared<double> n_refl              = scitbx::af::shared<double>(indices.size(), 0);
  scitbx::af::const_ref<size_t> miller_id        = ISIGI["miller_id"];
  scitbx::af::const_ref<double> scaled_intensity = ISIGI["scaled_intensity"];

  for (size_t i = 0; i < ISIGI.size(); i++) {
    size_t hkl_id = miller_id[i];
    sumI[hkl_id] += scaled_intensity[i];
    n_refl[hkl_id] += 1;
  }

  scitbx::af::shared<double> all_meanI      = scitbx::af::shared<double>(ISIGI.size(), 0);
  scitbx::af::shared<double> all_n_refl     = scitbx::af::shared<double>(ISIGI.size(), 0);
  scitbx::af::shared<double> nn             = scitbx::af::shared<double>(ISIGI.size(), 0);
  scitbx::af::shared<double> all_imeanprime = scitbx::af::shared<double>(ISIGI.size(), 0);
  for (size_t i = 0; i < ISIGI.size(); i++) {
    size_t hkl_id = miller_id[i];
    all_meanI[i] = sumI[hkl_id]/n_refl[hkl_id];
    all_n_refl[i] = n_refl[hkl_id];
    nn[i] = (n_refl[hkl_id]-1)/n_refl[hkl_id];
    SCITBX_ASSERT(n_refl[hkl_id] > 0);
    if (n_refl[hkl_id] > 1)
      all_imeanprime[i] = (sumI[hkl_id]-scaled_intensity[i])/(n_refl[hkl_id]-1);
  }
  ISIGI["mean_scaled_intensity"] = all_meanI;
  ISIGI["n_refl"] = all_n_refl;
  ISIGI["nn"] = nn;
  ISIGI["meanprime_scaled_intensity"] = all_imeanprime ;
}

namespace boost_python { namespace {
  void
  init_module() {
    using namespace boost::python;
    def("compute_normalized_deviations", &compute_normalized_deviations);
    def("apply_sd_error_params", &apply_sd_error_params);
    def("df_dpsq", &df_dpsq);
    def("setup_isigi_stats", &setup_isigi_stats);
}
}}
}}}} // namespace

BOOST_PYTHON_MODULE(xfel_sdfac_refine_ext)
{
  xfel::merging::error_model::sdfac_refine::boost_python::init_module();

}


#include <boost/geometry/strategies/transform/matrix_transformers.hpp>


/*===========================================================================*\

author: Matthias W. Smith
email:  mwsmith2@uw.edu
file:   error_estimator_test_root.cxx

notes: Tests the techniques used to estimate the fit errors on the
      frequency extraction.

usage:

./error_estimator_test [<output_file>]

The parameters in brackets are optional.  The default output is
error_estimator_data.csv.

\*===========================================================================*/

//--- std includes ----------------------------------------------------------//
#include <iostream>
#include <fstream>
#include <random>
#include <string>

//--- other includes --------------------------------------------------------//
#include <boost/filesystem.hpp>
#include "TFile.h"
#include "TTree.h"

//--- project includes ------------------------------------------------------//
#include "fid.h"

#define FID_LEN 10000
#define FREQ_METHOD 6

using std::cout;
using std::endl;
using std::string;
using std::to_string;
using namespace fid;

int main(int argc, char **argv)
{
  // Necessary variables.
  int rounds = 10;
  std::vector<double> wf;
  std::vector<double> tm = time_vector(); // default time vector in fid::

  // Ouput filename.
  string outfile;

  // now get optional output file
  if (argc > 1) {

    outfile = string(argv[1]);

  } else {

    outfile = string("data/error_estimator_data.root");

  }

  fid_freq_t fid_data;
  Double_t freq;

  TFile pf(outfile.c_str(), "RECREATE");
  TTree pt("t", "FID Fits");
  pt.Branch("data", &fid_data, fid_freq_str);
  pt.Branch("seed", &freq, "freq/D");

  // Create random number engine/distribution.
  std::default_random_engine gen;
  std::uniform_real_distribution<double> rand_flat_dist(35.0, 40.0);

  FidFactory ff;
  ff.SetSignalToNoise(100*100);
  ff.SetMixdownPhi(0.0);

  for (int i = 0; i < rounds; ++i) {

    if (i % (rounds / 5) == 0) {
      cout << "Processing round " << i << " of " << rounds << "." << endl;
    }

    // Make ideal Fid waveform
    freq = rand_flat_dist(gen);

    ff.SetFidFreq(freq);
    ff.IdealFid(wf, tm);
    Fid myfid(wf, tm);

    myfid.CopyStruct(fid_data);

    pt.Fill();
  }

  pf.Write();
  pf.Close();
  return 0;
}


/* Copyright (c) 2012-2019 Big Ladder Software LLC. All rights reserved.
 * See the LICENSE file for additional terms and conditions. */

#ifndef Cell_HPP
#define Cell_HPP

#include "Algorithms.hpp"
#include "BoundaryConditions.hpp"
#include "Foundation.hpp"
#include "Functions.hpp"
#include "Mesher.hpp"
#include "libkiva_export.h"

#include <fstream>
#include <memory>
#include <numeric>

#include <Eigen/SparseCore>

namespace Kiva {

enum CellType {
  EXTERIOR_AIR,  // 0
  INTERIOR_AIR,  // 1
  NORMAL,        // 2
  BOUNDARY,      // 3
  ZERO_THICKNESS // 4
};

class LIBKIVA_EXPORT Cell {
public:
  Cell(const std::size_t &index, const CellType cellType, const std::size_t &i,
       const std::size_t &j, const std::size_t &k, std::size_t *stepsize,
       const Foundation &foundation, Surface *surfacePtr, Block *blockPtr, Mesher *mesh);

  virtual ~Cell(){};

  std::size_t coords[3], index;
  std::size_t *stepsize;
  std::size_t dims[3];

  // inherent properties
  double density;
  double specificHeat;
  double conductivity;
  double iHeatCapacity, iHeatCapacityADI; // used to minimize operations in caclulation loops

  double volume;
  double area, r;
  double heatGain;

  // derived properties
  double pde[3][2]{{0}}, pde_c[2]{0};
  double dist[3][2];
  double kcoeff[3][2];
  double *told_ptr;
  // organizational properties

  CellType cellType;

  Block *blockPtr;

  Surface *surfacePtr;
  Mesher *meshPtr;

  void setDistances(const double &dxp_in, const double &dxm_in, const double &dyp_in,
                    const double &dym_in, const double &dzp_in, const double &dzm_in);

  void setConductivities(const std::vector<std::shared_ptr<Cell>> &cell_v);

  void setPDEcoefficients(int ndims, bool cylindrical);
  double onePDEcoefficient(std::size_t dim, std::size_t dir);

  void setComputeDims(std::size_t (&dims)[3]);

  void setZeroThicknessCellProperties(std::vector<std::shared_ptr<Cell>> pointSet);

  virtual void calcCellADEUp(double timestep, const Foundation &foundation,
                             const BoundaryConditions &bcs, double &U);
  virtual void calcCellADEDown(double timestep, const Foundation &foundation,
                               const BoundaryConditions &bcs, double &V);

  virtual double calcCellExplicit(double timestep, const Foundation &foundation,
                                  const BoundaryConditions &bcs);

  virtual void calcCellMatrix(Foundation::NumericalScheme scheme, const double &timestep,
                              const Foundation &foundation, const BoundaryConditions &bcs,
                              double &A, double (&Alt)[3][2], double &bVal);

  void calcCellSteadyState(const Foundation &foundation, double &A, double (&Alt)[3][2],
                           double &bVal);

  virtual void calcCellADI(std::size_t dim, const double &timestep, const Foundation &foundation,
                           const BoundaryConditions &bcs, double &A, double (&Alt)[2],
                           double &bVal);
  void gatherCCoeffs(const double &theta, bool cylindrical, double (&C)[3][2]);

  virtual std::vector<double> calculateHeatFlux(int ndims, double &TNew, std::size_t nX,
                                                std::size_t nY, std::size_t nZ,
                                                const std::vector<std::shared_ptr<Cell>> &cell_v);

  void Assemble(const Foundation &foundation);

  inline void doOutdoorTemp(const BoundaryConditions &bcs, double &A, double &bVal);
  inline void doIndoorTemp(const BoundaryConditions &bcs, double &A, double &bVal);

  void ADImath(std::size_t dim, const double Q, const double f, const double multiplier,
               const double (&C)[3][2], double &A, double (&Alt)[2], double &bVal);
};

class ExteriorAirCell : public Cell {
public:
  ExteriorAirCell(const std::size_t &index, const CellType cellType, const std::size_t &i,
                  const std::size_t &j, const std::size_t &k, std::size_t *stepsize,
                  const Foundation &foundation, Surface *surfacePtr, Block *blockPtr,
                  Mesher *meshptr);

  void calcCellADEUp(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                     double &U) override;
  void calcCellADEDown(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                       double &v) override;
  double calcCellExplicit(double timestep, const Foundation &foundation,
                          const BoundaryConditions &bcs) override;
  void calcCellMatrix(Foundation::NumericalScheme scheme, const double &timestep,
                      const Foundation &foundation, const BoundaryConditions &bcs, double &A,
                      double (&Alt)[3][2], double &bVal) override;
  void calcCellADI(std::size_t dim, const double &timestep, const Foundation &foundation,
                   const BoundaryConditions &bcs, double &A, double (&Alt)[2],
                   double &bVal) override;
  std::vector<double> calculateHeatFlux(int ndims, double &TNew, std::size_t nX, std::size_t nY,
                                        std::size_t nZ,
                                        const std::vector<std::shared_ptr<Cell>> &cell_v) override;
};

class InteriorAirCell : public Cell {
public:
  InteriorAirCell(const std::size_t &index, const CellType cellType, const std::size_t &i,
                  const std::size_t &j, const std::size_t &k, std::size_t *stepsize,
                  const Foundation &foundation, Surface *surfacePtr, Block *blockPtr,
                  Mesher *meshptr);

  void calcCellADEUp(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                     double &U) override;
  void calcCellADEDown(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                       double &v) override;
  double calcCellExplicit(double timestep, const Foundation &foundation,
                          const BoundaryConditions &bcs) override;
  void calcCellMatrix(Foundation::NumericalScheme scheme, const double &timestep,
                      const Foundation &foundation, const BoundaryConditions &bcs, double &A,
                      double (&Alt)[3][2], double &bVal) override;
  void calcCellADI(std::size_t dim, const double &timestep, const Foundation &foundation,
                   const BoundaryConditions &bcs, double &A, double (&Alt)[2],
                   double &bVal) override;
  std::vector<double> calculateHeatFlux(int ndims, double &TNew, std::size_t nX, std::size_t nY,
                                        std::size_t nZ,
                                        const std::vector<std::shared_ptr<Cell>> &cell_v) override;
};

class BoundaryCell : public Cell {
public:
  BoundaryCell(const std::size_t &index, const CellType cellType, const std::size_t &i,
               const std::size_t &j, const std::size_t &k, std::size_t *stepsize,
               const Foundation &foundation, Surface *surfacePtr, Block *blockPtr, Mesher *meshptr);

  void calcCellADEUp(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                     double &U) override;
  void calcCellADEDown(double timestep, const Foundation &foundation, const BoundaryConditions &bcs,
                       double &V) override;
  double calcCellExplicit(double timestep, const Foundation &foundation,
                          const BoundaryConditions &bcs) override;
  void calcCellMatrix(Foundation::NumericalScheme scheme, const double &timestep,
                      const Foundation &foundation, const BoundaryConditions &bcs, double &A,
                      double (&Alt)[3][2], double &bVal) override;
  void calcCellADI(std::size_t dim, const double &timestep, const Foundation &foundation,
                   const BoundaryConditions &bcs, double &A, double (&Alt)[2],
                   double &bVal) override;
  std::vector<double> calculateHeatFlux(int ndims, double &TNew, std::size_t nX, std::size_t nY,
                                        std::size_t nZ,
                                        const std::vector<std::shared_ptr<Cell>> &cell_v) override;

private:
  inline void zfCellADI(const int &dim, const int &sdim, const int &sign, double &A, double &Alt,
                        double &bVal);
  inline void ifCellADI(const int &dim, const int &sdim, const int &dir, double &A, double &Alt, double &bVal);
  inline void efCellADI(const int &dim, const int &sdim, const int &sign, double &A, double &Alt, double &bVal);
  inline void zfCellMatrix(double &A, double &Alt, double &bVal);
  inline void ifCellMatrix(const int &dim, const int &dir, double &A, double &Alt, double &bVal);
  inline void efCellMatrix(const int &dim, const int &dir, double &A, double &Alt, double &bVal);
  inline void zfCellADEUp(const std::size_t &dim, const std::size_t &dir, double &U);
  inline void ifCellADEUp(const int &dim, const int &dir, double &U);
  inline void efCellADEUp(const int &dim, const int &dir, double &U);
  inline void zfCellADEDown(const std::size_t &dim, const std::size_t &dir, double &V);
  inline void ifCellADEDown(const int &dim, const int &dir, double &V);
  inline void efCellADEDown(const int &dim, const int &dir, double &V);
  inline double zfCellExplicit(const std::size_t &dim, const std::size_t &dir);
  inline double ifCellExplicit(const std::size_t &dim, const std::size_t &dir);
  inline double efCellExplicit(const std::size_t &dim, const std::size_t &dir);
};

class ZeroThicknessCell : public Cell {
public:
  ZeroThicknessCell(const std::size_t &index, const CellType cellType, const std::size_t &i,
                    const std::size_t &j, const std::size_t &k, std::size_t *stepsize,
                    const Foundation &foundation, Surface *surfacePtr, Block *blockPtr,
                    Mesher *meshptr);

  std::vector<double> calculateHeatFlux(int ndims, double &TNew, std::size_t nX, std::size_t nY,
                                        std::size_t nZ,
                                        const std::vector<std::shared_ptr<Cell>> &cell_v) override;
};

} // namespace Kiva

#endif


// Kratos Multi-Physics
//
// Copyright (c) 2015, Pooyan Dadvand, Riccardo Rossi, CIMNE (International Center for Numerical Methods in Engineering)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
// 	-	Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
// 	-	Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
// 		in the documentation and/or other materials provided with the distribution.
// 	-	All advertising materials mentioning features or use of this software must display the following acknowledgement:
// 			This product includes Kratos Multi-Physics technology.
// 	-	Neither the name of the CIMNE nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED ANDON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
// THE USE OF THISSOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//
//   Project Name:        Kratos
//   Last modified by:    $Author: rrossi $
//   Date:                $Date: 2008-10-13 08:17:41 $
//   Revision:            $Revision: 1.20 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "processes/process.h"
#include "custom_python/add_custom_edgebased_levelset_solver_to_python.h"
#include "custom_utilities/edge_data.h"
#include "custom_utilities/edgebased_levelset.h"
#include "custom_utilities/edgebased_levelset_substep.h"

#include "spaces/ublas_space.h"
#include "linear_solvers/linear_solver.h"
#include "custom_utilities/edge_data_c2c.h"

namespace Kratos
{

namespace Python
{
void  AddCustomEdgeBasedLevelSetToPython()
{
    using namespace boost::python;

    typedef UblasSpace<double, CompressedMatrix, Vector> SparseSpaceType;
    typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;
    typedef LinearSolver<SparseSpaceType, LocalSpaceType > LinearSolverType;

    class_< EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType>,  boost::noncopyable >
    ("EdgeBasedLevelSet2D", init< MatrixContainer< 2, SparseSpaceType>&, ModelPart&, const double, const double, const Vector,bool,double,double, double,double,bool >() )
    .def("Initialize",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::Initialize)
    .def("ComputeTimeStep",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTimeStep)
    .def("SolveStep1",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep1)
    .def("SolveStep2",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep2)
    .def("SolveStep3",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep3)
    .def("ExtrapolateValues",&EdgeBasedLevelSet< 2,MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ExtrapolateValues)
    .def("MarkExternalAndMixedNodes",&EdgeBasedLevelSet< 2,MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkExternalAndMixedNodes)
    .def("MarkInternalAndMixedNodes",&EdgeBasedLevelSet< 2,MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalAndMixedNodes)
    .def("MarkInternalNodes",&EdgeBasedLevelSet< 2,MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalNodes)
    .def("SaveScalarVariableToOldStep",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SaveScalarVariableToOldStep)
    .def("ChangeSignToDistance",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ChangeSignToDistance)
    .def("MarkNodesByDistance",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkNodesByDistance)
    .def("ConvectDistance",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ConvectDistance)
    .def("ReduceTimeStep",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ReduceTimeStep)
    .def("CheckDistanceConvection",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CheckDistanceConvection)
    .def("UpdateFixedVelocityValues",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::UpdateFixedVelocityValues)
    .def("ActivateWallResistance"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateWallResistance)
    .def("SetShockCapturingCoefficient"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetShockCapturingCoefficient)
    .def("ComputeVolumeVariation",&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeVolumeVariation)
    .def("ComputeWetVolume"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeWetVolume)
    .def("DiscreteVolumeCorrection"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::DiscreteVolumeCorrection)
    .def("CalculatePorousResistanceLaw"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CalculatePorousResistanceLaw)
    .def("PushFreeSurface"   ,&EdgeBasedLevelSet< 2, MatrixContainer< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::PushFreeSurface)
    ;

    class_< EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType>,  boost::noncopyable >
    ("EdgeBasedLevelSet3D", init< MatrixContainer< 3, SparseSpaceType>&, ModelPart&, const double, const double, const Vector,bool,double,double, double,double,bool >() )
    .def("Initialize",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::Initialize)
    .def("ComputeTimeStep",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTimeStep)
    .def("SolveStep1",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep1)
    .def("SolveStep2",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep2)
    .def("SolveStep3",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep3)
    .def("ExtrapolateValues",&EdgeBasedLevelSet< 3,MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ExtrapolateValues)
    .def("MarkExternalAndMixedNodes",&EdgeBasedLevelSet< 3,MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkExternalAndMixedNodes)
    .def("MarkInternalAndMixedNodes",&EdgeBasedLevelSet< 3,MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalAndMixedNodes)
    .def("MarkInternalNodes",&EdgeBasedLevelSet< 3,MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalNodes)
    .def("SaveScalarVariableToOldStep",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SaveScalarVariableToOldStep)
    .def("ChangeSignToDistance",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ChangeSignToDistance)
    .def("MarkNodesByDistance",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkNodesByDistance)
    .def("ConvectDistance",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ConvectDistance)
    .def("ReduceTimeStep",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ReduceTimeStep)
    .def("CheckDistanceConvection",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CheckDistanceConvection)
    .def("UpdateFixedVelocityValues",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::UpdateFixedVelocityValues)
    .def("ActivateWallResistance",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateWallResistance)
    .def("SetShockCapturingCoefficient"   ,&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetShockCapturingCoefficient)
    .def("ComputeVolumeVariation",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeVolumeVariation)
    .def("ComputeWetVolume"   ,&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeWetVolume)
    .def("DiscreteVolumeCorrection"   ,&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::DiscreteVolumeCorrection)
    .def("CalculatePorousResistanceLaw"   ,&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CalculatePorousResistanceLaw)
    .def("PushFreeSurface"   ,&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::PushFreeSurface)
    .def("ComputeBoundedTimeStep",&EdgeBasedLevelSet< 3, MatrixContainer< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeBoundedTimeStep)
    ;

    class_< MatrixContainerC2C < 2, SparseSpaceType>, boost::noncopyable > ("MatrixContainerC2C2D", init< >())
    .def("ConstructCSRVector", &MatrixContainerC2C < 2, SparseSpaceType >::ConstructCSRVector)
    .def("BuildCSRData", &MatrixContainerC2C < 2, SparseSpaceType >::BuildCSRData)
    .def("Clear", &MatrixContainerC2C < 2, SparseSpaceType >::Clear)
    ;

    class_< MatrixContainerC2C < 3, SparseSpaceType>, boost::noncopyable > ("MatrixContainerC2C3D", init< >())
    .def("ConstructCSRVector", &MatrixContainerC2C < 3, SparseSpaceType >::ConstructCSRVector)
    .def("BuildCSRData", &MatrixContainerC2C < 3, SparseSpaceType >::BuildCSRData)
    .def("Clear", &MatrixContainerC2C < 3, SparseSpaceType >::Clear)
    ;

    class_< EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType>,  boost::noncopyable >
    ("EdgeBasedLevelSetSubstep2D", init< MatrixContainerC2C< 2, SparseSpaceType>&, ModelPart&, const double, const double, const Vector,bool,double,double, double,double,bool >() )
    .def("Initialize",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::Initialize)
    .def("GatherValues",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::GatherValues)
    .def("ComputeTimeStep",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTimeStep)
    .def("SolveStep1",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep1)
    .def("SolveStep2",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep2)
    .def("SolveStep3",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep3)
    .def("ExtrapolateValues",&EdgeBasedLevelSetSubstep< 2,MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ExtrapolateValues)
    .def("MarkExternalAndMixedNodes",&EdgeBasedLevelSetSubstep< 2,MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkExternalAndMixedNodes)
    .def("MarkInternalAndMixedNodes",&EdgeBasedLevelSetSubstep< 2,MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalAndMixedNodes)
    .def("MarkInternalNodes",&EdgeBasedLevelSetSubstep< 2,MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalNodes)
    .def("SaveScalarVariableToOldStep",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SaveScalarVariableToOldStep)
    .def("ChangeSignToDistance",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ChangeSignToDistance)
    .def("MarkNodesByDistance",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkNodesByDistance)
    .def("ConvectDistance",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ConvectDistance)
    .def("ReduceTimeStep",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ReduceTimeStep)
    .def("CheckDistanceConvection",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CheckDistanceConvection)
    .def("UpdateFixedVelocityValues",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::UpdateFixedVelocityValues)
    .def("ActivateWallResistance"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateWallResistance)
    .def("ActivateClassicalWallResistance"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateClassicalWallResistance)
    .def("SetShockCapturingCoefficient"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetShockCapturingCoefficient)
    .def("ComputeVolumeVariation",&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeVolumeVariation)
    .def("ComputeWetVolume"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeWetVolume)
    .def("DiscreteVolumeCorrection"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::DiscreteVolumeCorrection)
    .def("ApplySmagorinsky"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ApplySmagorinsky)
	.def("ContinuousVolumeCorrection"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ContinuousVolumeCorrection)
	.def("SetWallReductionCoefficients"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetWallReductionCoefficients)
	.def("ComputeTotalVolume"   ,&EdgeBasedLevelSetSubstep< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTotalVolume)
//                           .def("CalculatePorousResistanceLaw"   ,&EdgeBasedLevelSet< 2, MatrixContainerC2C< 2, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CalculatePorousResistanceLaw)
    ;

    class_< EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType>,  boost::noncopyable >
    ("EdgeBasedLevelSetSubstep3D", init< MatrixContainerC2C< 3, SparseSpaceType>&, ModelPart&, const double, const double, const Vector,bool,double,double, double,double,bool >() )
    .def("Initialize",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::Initialize)
    .def("GatherValues",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::GatherValues)
    .def("ComputeTimeStep",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTimeStep)
    .def("SolveStep1",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep1)
    .def("SolveStep2",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep2)
    .def("SolveStep3",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SolveStep3)
    .def("ExtrapolateValues",&EdgeBasedLevelSetSubstep< 3,MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ExtrapolateValues)
    .def("MarkExternalAndMixedNodes",&EdgeBasedLevelSetSubstep< 3,MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkExternalAndMixedNodes)
    .def("MarkInternalAndMixedNodes",&EdgeBasedLevelSetSubstep< 3,MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalAndMixedNodes)
    .def("MarkInternalNodes",&EdgeBasedLevelSetSubstep< 3,MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkInternalNodes)
    .def("SaveScalarVariableToOldStep",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SaveScalarVariableToOldStep)
    .def("ChangeSignToDistance",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ChangeSignToDistance)
    .def("MarkNodesByDistance",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::MarkNodesByDistance)
    .def("ConvectDistance",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ConvectDistance)
    .def("ReduceTimeStep",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ReduceTimeStep)
    .def("CheckDistanceConvection",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CheckDistanceConvection)
    .def("UpdateFixedVelocityValues",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::UpdateFixedVelocityValues)
    .def("ActivateWallResistance"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateWallResistance)
    .def("ActivateClassicalWallResistance"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ActivateClassicalWallResistance)
    .def("SetShockCapturingCoefficient"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetShockCapturingCoefficient)
    .def("ComputeVolumeVariation",&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeVolumeVariation)
    .def("ComputeWetVolume"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeWetVolume)
    .def("DiscreteVolumeCorrection"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::DiscreteVolumeCorrection)
    .def("ApplySmagorinsky"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ApplySmagorinsky)
    .def("ContinuousVolumeCorrection"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ContinuousVolumeCorrection)
//     .def("FindBubbles"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::FindBubbles)
	.def("SetWallReductionCoefficients"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::SetWallReductionCoefficients)
	.def("ComputeTotalVolume"   ,&EdgeBasedLevelSetSubstep< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::ComputeTotalVolume)
//                           .def("CalculatePorousResistanceLaw"   ,&EdgeBasedLevelSet< 3, MatrixContainerC2C< 3, SparseSpaceType>, SparseSpaceType, LinearSolverType >::CalculatePorousResistanceLaw)
    ;

}

}  // namespace Python.

} // Namespace Kratos


#include <Eigen/Geometry>
#include <algorithm>
#include <elasty/cloth-sim-object.hpp>
#include <elasty/constraint.hpp>
#include <elasty/particle.hpp>
#include <elasty/utils.hpp>
#include <iostream>
#include <tiny_obj_loader.h>

elasty::ClothSimObject::ClothSimObject(const unsigned           resolution,
                                       const double             in_plane_stiffness,
                                       const double             in_plane_compliance,
                                       const double             out_of_plane_stiffness,
                                       const double             out_of_plane_compliance,
                                       const double             dt,
                                       const Eigen::Affine3d&   transform,
                                       const InPlaneStrategy    in_plane_strategy,
                                       const OutOfPlaneStrategy out_of_plane_strategy)
{
    std::istringstream obj_data_stream(generateClothMeshObjData(2.0, 2.0, resolution, resolution));

    tinyobj::attrib_t                attrib;
    std::vector<tinyobj::shape_t>    shapes;
    std::vector<tinyobj::material_t> materials;

    std::string warn;
    std::string err;
    const bool  return_value = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, &obj_data_stream);

    if (!warn.empty())
    {
        std::cerr << warn << std::endl;
    }
    if (!err.empty())
    {
        std::cerr << err << std::endl;
    }
    if (!return_value)
    {
        throw std::runtime_error("");
    }
    if (attrib.vertices.empty())
    {
        throw std::runtime_error("");
    }

    const auto& shape = shapes[0];

    assert(shapes.size() == 1);
    assert(attrib.vertices.size() % 3 == 0);
    assert(shape.mesh.indices.size() % 3 == 0);

    m_triangle_list.resize(shape.mesh.indices.size() / 3, 3);
    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++i)
    {
        m_triangle_list(i, 0) = shape.mesh.indices[i * 3 + 0].vertex_index;
        m_triangle_list(i, 1) = shape.mesh.indices[i * 3 + 1].vertex_index;
        m_triangle_list(i, 2) = shape.mesh.indices[i * 3 + 2].vertex_index;
    }

    if (!attrib.texcoords.empty())
    {
        m_uv_list.resize(shape.mesh.indices.size() / 3, 2 * 3);
        for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++i)
        {
            m_uv_list(i, 2 * 0 + 0) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 0].texcoord_index + 0];
            m_uv_list(i, 2 * 0 + 1) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 0].texcoord_index + 1];
            m_uv_list(i, 2 * 1 + 0) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 1].texcoord_index + 0];
            m_uv_list(i, 2 * 1 + 1) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 1].texcoord_index + 1];
            m_uv_list(i, 2 * 2 + 0) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 2].texcoord_index + 0];
            m_uv_list(i, 2 * 2 + 1) = attrib.texcoords[2 * shape.mesh.indices[i * 3 + 2].texcoord_index + 1];
        }
    }

    std::map<unsigned int, std::shared_ptr<Particle>> map_from_obj_vertex_index_to_particle;
    for (unsigned int i = 0; i < attrib.vertices.size() / 3; ++i)
    {
        const Eigen::Vector3d position{
            attrib.vertices[3 * i + 0], attrib.vertices[3 * i + 1], attrib.vertices[3 * i + 2]};

        const Eigen::Vector3d x = transform * position;
        const Eigen::Vector3d v = Eigen::Vector3d::Zero();
        const double          m = 1.0 / double(attrib.vertices.size());

        auto particle = std::make_shared<elasty::Particle>(x, v, m);

        map_from_obj_vertex_index_to_particle[i] = particle;

        m_particles.push_back(particle);
    }

    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++i)
    {
        const auto p_0 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 0].vertex_index];
        const auto p_1 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 1].vertex_index];
        const auto p_2 = map_from_obj_vertex_index_to_particle[shape.mesh.indices[i * 3 + 2].vertex_index];

        if (in_plane_strategy == InPlaneStrategy::EdgeDistance || in_plane_strategy == InPlaneStrategy::Both)
        {
            m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(
                p_0, p_1, in_plane_stiffness, in_plane_compliance, dt, (p_0->x - p_1->x).norm()));
            m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(
                p_0, p_2, in_plane_stiffness, in_plane_compliance, dt, (p_0->x - p_2->x).norm()));
            m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(
                p_1, p_2, in_plane_stiffness, in_plane_compliance, dt, (p_1->x - p_2->x).norm()));
        }

        if (in_plane_strategy == InPlaneStrategy::ContinuumTriangle || in_plane_strategy == InPlaneStrategy::Both)
        {
            m_constraints.push_back(std::make_shared<elasty::ContinuumTriangleConstraint>(
                p_0, p_1, p_2, in_plane_stiffness, in_plane_compliance, dt, 1000.0, 0.10));
        }
    }

    using vertex_t   = unsigned int;
    using triangle_t = unsigned int;
    using edge_t     = std::pair<vertex_t, vertex_t>;
    std::map<edge_t, std::vector<triangle_t>> edges_and_triangles;
    for (unsigned int i = 0; i < shape.mesh.indices.size() / 3; ++i)
    {
        const vertex_t index_0 = shape.mesh.indices[i * 3 + 0].vertex_index;
        const vertex_t index_1 = shape.mesh.indices[i * 3 + 1].vertex_index;
        const vertex_t index_2 = shape.mesh.indices[i * 3 + 2].vertex_index;

        const edge_t e_01 = std::make_pair(std::min(index_0, index_1), std::max(index_0, index_1));
        const edge_t e_02 = std::make_pair(std::min(index_0, index_2), std::max(index_0, index_2));
        const edge_t e_12 = std::make_pair(std::min(index_1, index_2), std::max(index_1, index_2));

        auto register_edge = [&](const edge_t& edge)
        {
            if (edges_and_triangles.find(edge) == edges_and_triangles.end())
            {
                edges_and_triangles[edge] = {i};
            }
            else
            {
                edges_and_triangles[edge].push_back(i);
            }
        };

        register_edge(e_01);
        register_edge(e_02);
        register_edge(e_12);
    }

    for (const auto& key_value : edges_and_triangles)
    {
        const edge_t&                  edge      = key_value.first;
        const std::vector<triangle_t>& triangles = key_value.second;

        assert(triangles.size() == 1 || triangles.size() == 2);

        // Boundary
        if (triangles.size() == 1)
        {
            continue;
        }

        auto obtain_another_vertex = [&](const triangle_t& triangle, const edge_t& edge)
        {
            const vertex_t vertex_0 = shape.mesh.indices[3 * triangle + 0].vertex_index;
            const vertex_t vertex_1 = shape.mesh.indices[3 * triangle + 1].vertex_index;
            const vertex_t vertex_2 = shape.mesh.indices[3 * triangle + 2].vertex_index;

            if (vertex_0 != edge.first && vertex_0 != edge.second)
            {
                return vertex_0;
            }
            else if (vertex_1 != edge.first && vertex_1 != edge.second)
            {
                return vertex_1;
            }
            else
            {
                assert(vertex_2 != edge.first && vertex_2 != edge.second);
                return vertex_2;
            }
        };

        const triangle_t another_vertex_0 = obtain_another_vertex(triangles[0], edge);
        const triangle_t another_vertex_1 = obtain_another_vertex(triangles[1], edge);

        switch (out_of_plane_strategy)
        {
            case OutOfPlaneStrategy::Bending:
            {
                const auto p_0 = map_from_obj_vertex_index_to_particle[edge.first];
                const auto p_1 = map_from_obj_vertex_index_to_particle[edge.second];
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                const Eigen::Vector3d& x_0 = p_0->x;
                const Eigen::Vector3d& x_1 = p_1->x;
                const Eigen::Vector3d& x_2 = p_2->x;
                const Eigen::Vector3d& x_3 = p_3->x;

                const Eigen::Vector3d p_10 = x_1 - x_0;
                const Eigen::Vector3d p_20 = x_2 - x_0;
                const Eigen::Vector3d p_30 = x_3 - x_0;

                const Eigen::Vector3d n_0 = p_10.cross(p_20).normalized();
                const Eigen::Vector3d n_1 = p_10.cross(p_30).normalized();

                assert(!n_0.hasNaN());
                assert(!n_1.hasNaN());

                // Typical value is 0.0 or pi
                const double dihedral_angle = std::acos(std::clamp(n_0.dot(n_1), -1.0, 1.0));

                assert(!std::isnan(dihedral_angle));

                m_constraints.push_back(std::make_shared<elasty::BendingConstraint>(
                    p_0, p_1, p_2, p_3, out_of_plane_stiffness, out_of_plane_compliance, dt, dihedral_angle));

                break;
            }
            case OutOfPlaneStrategy::IsometricBending:
            {
                const auto p_0 = map_from_obj_vertex_index_to_particle[edge.first];
                const auto p_1 = map_from_obj_vertex_index_to_particle[edge.second];
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                m_constraints.push_back(std::make_shared<elasty::IsometricBendingConstraint>(
                    p_0, p_1, p_2, p_3, out_of_plane_stiffness, out_of_plane_compliance, dt));

                break;
            }
            case OutOfPlaneStrategy::Cross:
            {
                const auto p_2 = map_from_obj_vertex_index_to_particle[another_vertex_0];
                const auto p_3 = map_from_obj_vertex_index_to_particle[another_vertex_1];

                const Eigen::Vector3d& x_2 = p_2->x;
                const Eigen::Vector3d& x_3 = p_3->x;

                m_constraints.push_back(std::make_shared<elasty::DistanceConstraint>(
                    p_2, p_3, out_of_plane_stiffness, out_of_plane_compliance, dt, (x_2 - x_3).norm()));

                break;
            }
        }
    }

    calculateAreas();
}

void elasty::ClothSimObject::applyAerodynamicForces(const Eigen::Vector3d& global_velocity,
                                                    const double           drag_coeff,
                                                    const double           lift_coeff)
{
    constexpr double rho = 1.225; // Taken from Wikipedia: https://en.wikipedia.org/wiki/Density_of_air

    assert(drag_coeff >= lift_coeff);

    const int num_triangles = m_triangle_list.rows();

    for (int i = 0; i < num_triangles; ++i)
    {
        const auto& x_0 = m_particles[m_triangle_list.row(i)(0)]->x;
        const auto& x_1 = m_particles[m_triangle_list.row(i)(1)]->x;
        const auto& x_2 = m_particles[m_triangle_list.row(i)(2)]->x;

        const auto& v_0 = m_particles[m_triangle_list.row(i)(0)]->v;
        const auto& v_1 = m_particles[m_triangle_list.row(i)(1)]->v;
        const auto& v_2 = m_particles[m_triangle_list.row(i)(2)]->v;

        const auto& m_0 = m_particles[m_triangle_list.row(i)(0)]->m;
        const auto& m_1 = m_particles[m_triangle_list.row(i)(1)]->m;
        const auto& m_2 = m_particles[m_triangle_list.row(i)(2)]->m;

        const double m_sum = m_0 + m_1 + m_2;

        // Calculate the weighted average of the particle velocities
        const Eigen::Vector3d v_triangle = (m_0 * v_0 + m_1 * v_1 + m_2 * v_2) / m_sum;

        // Calculate the relative velocity of the triangle
        const Eigen::Vector3d v_rel         = v_triangle - global_velocity;
        const double          v_rel_squared = v_rel.squaredNorm();

        const auto            cross         = (x_1 - x_0).cross(x_2 - x_0);
        const double          area          = 0.5 * cross.norm();
        const auto            n_either_side = cross.normalized();
        const Eigen::Vector3d n             = (n_either_side.dot(v_rel) > 0.0) ? n_either_side : -n_either_side;

        const double coeff = 0.5 * rho * area;

        // Note: This wind force model was proposed by [Wilson+14]
        const Eigen::Vector3d f =
            -coeff * ((drag_coeff - lift_coeff) * v_rel.dot(n) * v_rel + lift_coeff * v_rel_squared * n);

        m_particles[m_triangle_list.row(i)(0)]->f += (m_0 / m_sum) * f;
        m_particles[m_triangle_list.row(i)(1)]->f += (m_1 / m_sum) * f;
        m_particles[m_triangle_list.row(i)(2)]->f += (m_2 / m_sum) * f;
    }
}

void elasty::ClothSimObject::calculateAreas()
{
    const int num_triangles = m_triangle_list.rows();

    m_area_list = Eigen::VectorXd(num_triangles);

    for (int i = 0; i < num_triangles; ++i)
    {
        const auto& x_0 = m_particles[m_triangle_list.row(i)[0]]->x;
        const auto& x_1 = m_particles[m_triangle_list.row(i)[1]]->x;
        const auto& x_2 = m_particles[m_triangle_list.row(i)[2]]->x;

        const double area = 0.5 * (x_1 - x_0).cross(x_2 - x_0).norm();

        m_area_list(i) = area;
    }
}


#define print(a) std::cout<<a<<std::endl;
#define printr(a) for(auto it!=a.begin(); it!=a.end(); ++it){cout<a<endl;};
#include <mexplus.h>
#include <iostream>
#include <Eigen/Core>

using namespace std;
using namespace mexplus;
using namespace Eigen;
template<class T> using matrix = std::vector<std::vector<T>>;

namespace mexplus{
    // A nasty workaround
    /*
    template<>
    MatrixXd InputArguments::get(size_t index) const{
        MxArray raw_data(this->get(index));
        int Nr = raw_data.rows();
        int Nc = raw_data.cols();
        auto data = this->get<std::vector<double>>(index);
        MatrixXd Mdata = Eigen::Map<MatrixXd>(&data[0], Nr, Nc);
        return Mdata;
    }
    */

    /*
    //template<>// std::vector(nz, 1) of eigen::matrix(nx, ny)
    vector<MatrixXd> InputArguments::get_vecmat(size_t index,
            int nx, int ny, int nz) const{
        MxArray raw_data(this->get(index));
        int Nr = raw_data.rows();
        int Nc = raw_data.cols();
        auto tmp = this->get<std::vector<double>>(index);
        auto mat_data = Eigen::Map<MatrixXd>(&tmp[0], Nr, Nc);

        vector<MatrixXd> data(nz);
        for(auto it=data.begin(); it!=data.end(); ++it){
            int idx = it - data.begin();
            *it = mat_data.block<nx, ny>(nx*idx, 0);
        }
        return data;
    }
    */
}

// Hypothetical database class to be MEXed. This example is a proxy to C++ map.
class Monster {
public:
  // Database constructor. This is a stub.
  Monster(const string& filename):life(100), PTM(MatrixXd::Zero(2,2)){};

  // Database destructor.
  virtual ~Monster() {}
  // Query a record.
  void attack(double power){
      life -= power;
      cout<<"your remaining life is "<< life <<endl;
  }
  void get_life(double& life_){
      life_ = life;
  }

  void set_matrix(Eigen::MatrixXd PTM_){
      PTM = PTM_;
  }

  void set_vecmat(vector<MatrixXd> P__inpt){
      P__ = P__inpt;
      for(auto i=P__.begin(); i!=P__.end(); i++){
          print(*i);
      }
  }

  void set_matmat(matrix<MatrixXd> fuck_){
      fuck = fuck_;
      for(auto i=fuck.begin(); i!=fuck.end(); i++){
          for(auto j=i->begin(); j!=i->end(); j++){
              print(*j);
              cout<<endl;
          }
      }
  }

private:
  double life;
  MatrixXd PTM;
  vector<MatrixXd> P__;
  matrix<MatrixXd> fuck;

};

// Instance manager for Database.
template class mexplus::Session<Monster>;
namespace {

// Defines MEX API for new.
MEX_DEFINE(new) (int nlhs, mxArray* plhs[],
                 int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 1);
  OutputArguments output(nlhs, plhs, 1);
  output.set(0, Session<Monster>::create(new Monster(input.get<string>(0))));
}

// Defines MEX API for delete.
MEX_DEFINE(delete) (int nlhs, mxArray* plhs[],
                    int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 1);
  OutputArguments output(nlhs, plhs, 0);
  Session<Monster>::destroy(input.get(0));
}

// Defines MEX API for query (const method).
MEX_DEFINE(attack) (int nlhs, mxArray* plhs[],
                   int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 2);
  OutputArguments output(nlhs, plhs, 0);
  Monster* monster = Session<Monster>::get(input.get(0));
  monster->attack(input.get<double>(1));
}

MEX_DEFINE(get_life) (int nlhs, mxArray* plhs[],
                   int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 1);
  OutputArguments output(nlhs, plhs, 1);
  Monster* monster = Session<Monster>::get(input.get(0));
  double life_;
  monster->get_life(life_);
  output.set(0, life_);
}

MEX_DEFINE(set_matrix) (int nlhs, mxArray* plhs[],
                   int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 2);
  OutputArguments output(nlhs, plhs, 1);
  Monster* monster = Session<Monster>::get(input.get(0));
  auto inp_ = input.get(1);
  auto inp = input.get<std::vector<double>>(1);
  auto test = input.get<Eigen::MatrixXd>(1);
}

MEX_DEFINE(set_vecmat) (int nlhs, mxArray* plhs[],
                   int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 2);
  OutputArguments output(nlhs, plhs, 1);
  Monster* monster = Session<Monster>::get(input.get(0));
  auto a = input.get<vector<MatrixXd>>(1, 2, 1);
  monster->set_vecmat(a);
}

MEX_DEFINE(set_matmat) (int nlhs, mxArray* plhs[],
                   int nrhs, const mxArray* prhs[]) {
  InputArguments input(nrhs, prhs, 2);
  OutputArguments output(nlhs, plhs, 1);
  Monster* monster = Session<Monster>::get(input.get(0));
  auto fuck = input.get<matrix<MatrixXd>>(1, 1, 2);
  monster->set_matmat(fuck);
}

} // namespace

MEX_DISPATCH // Don't forget to add this if MEX_DEFINE() is used.


#include "cloud_flow.h"
#include "cloud_video.h"
#include "filters.h"
#include <algorithm>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#define LOG1(message)

namespace Cloud
{

class FlowEstimator
{
public:

  const PointSet & points;

  FlowEstimator (const PointSet & points, MatrixXf & flow);
  virtual ~FlowEstimator ();

  void solve (VideoSequence & seq, float tol, size_t max_em_steps = 8);

protected:

  enum { max_state_size = 64 };

  static const size_t s_bp_steps;

  struct Tile
  {
    Vector<int> m_head_idx;
    Vector<int> m_tail_idx;
    Vector<float> m_head;
    Vector<float> m_tail;

    static size_t s_max_steps;
    static size_t s_total_steps;
    static size_t s_total_solves;

  public:

    Tile (const VectorSf & head, const VectorSf & tail);

    void init (MatrixXf & accum);
    void solve (
        const MatrixXf & prior,
        MatrixXf & accum,
        float tol,
        float * work);

    static size_t pop_max_steps ();
    static float pop_mean_steps ();

  private:

    inline void solve_guts (float * restrict flow, float tol);
  };

  MatrixXf & m_flow;
  MatrixXf m_accum;
  std::vector<Tile *> m_tiles;

  void solve (float tol, size_t max_steps);
  void init_tiles ();
  void solve_tiles (float tol);
  void constrain (float tol);
};

void estimate_flow (
    const PointSet & points,
    MatrixXf & flow,
    VideoSequence & seq,
    float tol)
{
  FlowEstimator estimator(points, flow);
  estimator.solve(seq, tol);
}

const size_t FlowEstimator::s_bp_steps = 40;

FlowEstimator::FlowEstimator (const PointSet & p, MatrixXf & flow)
  : points(p),
    m_flow(flow),
    m_accum(flow.rows(), flow.cols())
{
  ASSERT_EQ(flow.cols(), int(points.size));
  ASSERT_EQ(flow.rows(), int(points.size));
}

FlowEstimator::~FlowEstimator ()
{}

void FlowEstimator::solve (
    VideoSequence & video_seq,
    float tol,
    size_t max_steps)
{
  LOG("Estimating pairwise probability density");
  LOG(" tol = " << tol << ", max_steps = " << max_steps);

  video_seq.sort();

  PointSequence seq(video_seq, points.dim);

  const size_t batch_size = PointSet::max_batch_size;
  Vector<float> likes_batch(points.size * batch_size);

  VectorXf dense(points.size);
  Vector<float> dense_vect = as_vector(dense);

  LOG(" building sparsity thresholds from " << seq.size << " observations");
  LOG("  observations per point = " << (float(seq.size) / points.size));

  float min_o_max_p_likes = INFINITY;
  Vector<float> max_o_likes(points.size);
  max_o_likes.set(-INFINITY);

  Filters::DebugStats<double> obs_density;

  for (PointSequence::BatchIterator i(seq, batch_size); i; i.next()) {
    const Point points_i = i.points();
    Vector<float> likes_i(points.size * i.buffer_size(), likes_batch);

    points.quantize_batch(points_i, likes_i);

    for (size_t j = 0; j < i.buffer_size(); ++j) {
      const Vector<float> likes = likes_i.block(points.size, j);

      imax(max_o_likes, likes);
      imin(min_o_max_p_likes, max(likes));

      obs_density.add(density(likes));
    }
  }

  float min_p_max_o_likes = min(max_o_likes);
  ASSERT_LT(0, min_p_max_o_likes);
  LOG("  min p:point. max o:obs. P(p|o) = "
      << (min_p_max_o_likes / points.size));
  ASSERT_LT(0, min_o_max_p_likes);
  LOG("  min o:obs. max p:point. P(p|o) = "
      << (min_o_max_p_likes / points.size));

  LOG("  obs density: " << obs_density);

  LOG(" building transition tiles from " << seq.size << " observations");

  VectorXf prior(points.size);
  prior.setZero();

  Vector<float> thresh(points.size);
  Vector<float> point_thresh(points.size);
  multiply(tol, max_o_likes, point_thresh);
  ASSERT_LT(0, min(point_thresh));

  size_t num_cropped_states = 0;
  Filters::DebugStats<double> state_density;
  Filters::DebugStats<double> state_entropy;
  Filters::DebugStats<double> state_size;
  Filters::DebugStats<double> state_loss;
  Filters::DebugStats<double> tile_area;

  VectorSf state;
  VectorSf prev_state;

  m_tiles.reserve(seq.size);

  for (PointSequence::BatchIterator i(seq, batch_size); i; i.next()) {
    const Point points_i = i.points();
    Vector<float> likes_i(points.size * i.buffer_size(), likes_batch);

    points.quantize_batch(points_i, likes_i);

    for (size_t j = 0; j < i.buffer_size(); ++j) {
      const Vector<float> likes = likes_i.block(points.size, j);
      dense_vect = likes;

      std::swap(prev_state, state);

      float obs_thresh = tol * dense.maxCoeff();
      ASSERT_LT(0, obs_thresh);
      minimum(obs_thresh, point_thresh, thresh);

      float loss = sparsify_absolute(dense, state, thresh);
      if (state.nonZeros() > max_state_size) {
        loss += sparsify_size(state, max_state_size);
        ++num_cropped_states;
      }
      state_loss.add(loss / points.size);
      state_size.add(state.nonZeros());

      normalize_l1(state, state.size());
      ASSERT_LT(0, state.sum());
      prior += state;

      state_density.add(density(state));
      state_entropy.add(likelihood_entropy(state));
      state_size.add(state.nonZeros());

      if (i.has_prev(j)) {

        m_tiles.push_back(new Tile(prev_state, state));

        tile_area.add(prev_state.nonZeros() * state.nonZeros());
      }
    }
  }

  float portion_cropped = float(num_cropped_states) / seq.size;
  LOG("  cropped " << (100 * portion_cropped)
      << "% of states to " << max_state_size);
  LOG("  state size: " << state_size);
  LOG("  state loss: " << state_loss);
  LOG("  state density: " << state_density);
  LOG("  state entropy: " << state_entropy);
  LOG("  tile area: " << tile_area);

  float density = state_size.mean() / points.size;
  float loss = state_loss.mean();
  LOG("  sparsifying states to density " << density
      << " loses " << (100 * loss) << "% of mass");

  normalize_l1(prior, prior.size());
  ASSERT_LT(0, prior.minCoeff()); // XXX error here
  Vector<float> prior_sparse = as_vector(prior);
  const Vector<float> & prior_dense = points.get_prior();
  float relentropy_d_s = relentropy(prior_dense, prior_sparse, true);
  LOG("  relentropy(dense prior, sparse prior) = " << relentropy_d_s);

  solve(tol, max_steps);

  delete_all(m_tiles.begin(), m_tiles.end());
  m_tiles.clear();

  constrain(tol);
}

void FlowEstimator::solve (float tol, size_t max_em_steps)
{
  LOG(" estimating flow via EM + local BP");

  Timer timer;

  init_tiles();

  for (size_t step = 0; step < max_em_steps; ++step) {

    cout << "  step " << (1+step) << "/" << max_em_steps << flush;

    solve_tiles(tol);

    cout << ", tile steps mean = " << Tile::pop_mean_steps()
      << ", max = " << Tile::pop_max_steps() << endl;
  }

  LOG(" EM + local BP took " << timer.elapsed() << " sec");
}

void FlowEstimator::init_tiles ()
{
  const size_t num_tiles = m_tiles.size();
  Tile ** restrict tiles = & m_tiles[0];

  m_flow.setZero();

  #pragma omp parallel for schedule(dynamic)
  for (size_t t = 0; t < num_tiles; ++t) {
    tiles[t]->init(m_flow);
  }
}

void FlowEstimator::solve_tiles (float tol)
{
  const size_t num_tiles = m_tiles.size();
  Tile ** restrict tiles = & m_tiles[0];

  m_accum.setZero();

  float * work;
  #pragma omp parallel private(work)
  {
    work = malloc_float(max_state_size * max_state_size);

    #pragma omp for schedule(dynamic)
    for (size_t t = 0; t < num_tiles; ++t) {
      tiles[t]->solve(m_flow, m_accum, tol, work);
    }

    free_float(work);
  }

  m_flow = m_accum;
}

void FlowEstimator::constrain (float tol)
{
  VectorXf prior(int(points.size));
  VectorXf marginals(int(points.size));

  points.get_prior(prior);

  constrain_marginals_bp(
      m_flow,
      prior,
      prior,
      marginals,
      marginals,
      tol);
}

//----( tiles )----

size_t FlowEstimator::Tile::s_max_steps = 0;
size_t FlowEstimator::Tile::s_total_steps = 0;
size_t FlowEstimator::Tile::s_total_solves = 0;

size_t FlowEstimator::Tile::pop_max_steps ()
{
  size_t max_steps = s_max_steps;

  s_max_steps = 0;

  return max_steps;
}

float FlowEstimator::Tile::pop_mean_steps ()
{
  ASSERT_LT(0, s_total_solves);

  float mean_steps = float(s_total_steps) / float(s_total_solves);

  s_total_steps = 0;
  s_total_solves = 0;

  return mean_steps;
}

FlowEstimator::Tile::Tile (
    const VectorSf & head,
    const VectorSf & tail)
  : m_head_idx(head.nonZeros()),
    m_tail_idx(tail.nonZeros()),
    m_head(head.nonZeros()),
    m_tail(tail.nonZeros())
{
  {
    size_t h = 0;
    for (VectorSf::InnerIterator iter(head); iter; ++iter) {

      m_head_idx[h] = iter.index();
      m_head[h] = iter.value();
      ++h;
    }
  }
  {
    size_t t = 0;
    for (VectorSf::InnerIterator iter(tail); iter; ++iter) {

      m_tail_idx[t] = iter.index();
      m_tail[t] = iter.value();
      ++t;
    }
  }

  // we expect head & tail to be l1-normalized
}

void FlowEstimator::Tile::init (MatrixXf & accum)
{
  const size_t H = m_head.size;
  const size_t T = m_tail.size;

  const float * restrict head = m_head;
  const float * restrict tail = m_tail;

  for (size_t h = 0; h < H; ++h) { int i = m_head_idx[h];
  for (size_t t = 0; t < T; ++t) { int j = m_tail_idx[t];

    #pragma omp atomic
    accum(j,i) += head[h] * tail[t];
  }}
}

void FlowEstimator::Tile::solve (
    const MatrixXf & prior,
    MatrixXf & accum,
    float tol,
    float * work)
{
  const size_t H = m_head.size;
  const size_t T = m_tail.size;

  Vector<float> flow(H * T, work);

  for (size_t h = 0; h < H; ++h) { int i = m_head_idx[h];
  for (size_t t = 0; t < T; ++t) { int j = m_tail_idx[t];
    flow[H * t + h] = prior(j,i);
  }}

  solve_guts(flow, tol);

  for (size_t h = 0; h < H; ++h) { int i = m_head_idx[h];
  for (size_t t = 0; t < T; ++t) { int j = m_tail_idx[t];

    #pragma omp atomic
    accum(j,i) += flow[T * h + t];
  }}
}

inline void FlowEstimator::Tile::solve_guts (float * restrict flow, float tol)
{
  const size_t H = m_head.size;
  const size_t T = m_tail.size;

  const float tol2 = tol / 100;
  const float tol2_over_H = tol2 / H;
  const float tol2_over_T = tol2 / T;

  const float * restrict head = m_head;
  const float * restrict tail = m_tail;

  size_t steps = 0;
  while (steps < s_bp_steps) {
    ++steps;

    float stepsize = 0;

    for (size_t h = 0; h < H; ++h) {

      float flow_h = 0;
      for (size_t t = 0; t < T; ++t) {
        flow_h += flow[T * h + t];
      }
      ASSERT_LT(0, flow_h);

      imax(stepsize, fabsf(head[h] - flow_h));

      const float scale = head[h] / (flow_h + tol2);
      for (size_t t = 0; t < T; ++t) {
        float & restrict flow_ht = flow[T * h + t];
        flow_ht = scale * (flow_ht + tol2_over_T);
      }
    }

    for (size_t t = 0; t < T; ++t) {

      float flow_t = 0;
      for (size_t h = 0; h < H; ++h) {
        flow_t += flow[T * h + t];
      }
      ASSERT_LT(0, flow_t);

      imax(stepsize, fabsf(tail[t] - flow_t));

      const float scale = tail[t] / (flow_t + tol2);
      for (size_t h = 0; h < H; ++h) {
        float & restrict flow_ht = flow[T * h + t];
        flow_ht = scale * (flow_ht + tol2_over_H);
      }
    }

    if (stepsize < tol) break;
  }

  imax(s_max_steps, steps);
  s_total_steps += H * T * steps;
  s_total_solves += H * T;
}

} // namespace Cloud


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */
#include <map>

#include <functional>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Propagators/variationalEquations.h"
#include "Tudat/Astrodynamics/Propagators/rotationalMotionQuaternionsStateDerivative.h"

#include "Tudat/Astrodynamics/OrbitDetermination/AccelerationPartials/accelerationPartial.h"

namespace tudat
{

namespace propagators
{

template< typename StateScalarType >
void VariationalEquations::getBodyInitialStatePartialMatrix(
        const Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic >& stateTransitionAndSensitivityMatrices,
        Eigen::Block< Eigen::Matrix< StateScalarType, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative )
{
    setBodyStatePartialMatrix( );

    // Add partials of body positions and velocities.
    currentMatrixDerivative.block( 0, 0, totalDynamicalStateSize_, numberOfParameterValues_ ) =
            ( variationalMatrix_.template cast< StateScalarType >( ) * stateTransitionAndSensitivityMatrices );
}

//! Calculates matrix containing partial derivatives of state derivatives w.r.t. body state.
void VariationalEquations::setBodyStatePartialMatrix( )
{
    // Initialize partial matrix
    variationalMatrix_.setZero( );

    if( dynamicalStatesToEstimate_.count( propagators::translational_state ) > 0 )
    {
        int startIndex = stateTypeStartIndices_.at( propagators::translational_state );
        for( unsigned int i = 0; i < dynamicalStatesToEstimate_.at( propagators::translational_state ).size( ); i++ )
        {
            variationalMatrix_.block( startIndex + i * 6, startIndex + i * 6 + 3, 3, 3 ).setIdentity( );
        }
    }

    if( dynamicalStatesToEstimate_.count( propagators::rotational_state ) > 0 )
    {
         Eigen::VectorXd rotationalStates = currentStatesPerTypeInConventionalRepresentation_.at(
                     propagators::rotational_state );

        int startIndex = stateTypeStartIndices_.at( propagators::rotational_state );
        for( unsigned int i = 0; i < dynamicalStatesToEstimate_.at( propagators::rotational_state ).size( ); i++ )
        {
            variationalMatrix_.block( startIndex + i * 7, startIndex + i * 7 , 4, 4 ) =
                    getQuaterionToQuaternionRateMatrix( rotationalStates.segment( 7 * i + 4, 3 ) );
            variationalMatrix_.block( startIndex + i * 7, startIndex + i * 7 + 4, 4, 3 ) =
                    getAngularVelocityToQuaternionRateMatrix( rotationalStates.segment( 7 * i, 4 ) );
        }
    }

    // Iterate over all bodies undergoing accelerations for which initial condition is to be estimated.
    for( std::map< IntegratedStateType, std::vector< std::multimap< std::pair< int, int >,
         std::function< void( Eigen::Block< Eigen::MatrixXd > ) > > > >::iterator
         typeIterator = statePartialList_.begin( ); typeIterator != statePartialList_.end( ); typeIterator++ )
    {
        int startIndex = stateTypeStartIndices_.at( typeIterator->first );
        int currentStateSize = getSingleIntegrationSize( typeIterator->first );
        int entriesToSkipPerEntry = currentStateSize - getGeneralizedAccelerationSize( typeIterator->first );

        for( unsigned int i = 0; i < typeIterator->second.size( ); i++ )
        {
            // Iterate over all bodies exerting an acceleration on this body.
            for( statePartialIterator_ = typeIterator->second.at( i ).begin( );
                 statePartialIterator_ != typeIterator->second.at( i ).end( );
                 statePartialIterator_++ )
            {
                statePartialIterator_->second(
                            variationalMatrix_.block(
                                startIndex + entriesToSkipPerEntry + i* currentStateSize, statePartialIterator_->first.first,
                                currentStateSize - entriesToSkipPerEntry, statePartialIterator_->first.second ) );

            }
        }
    }

   for( unsigned int i = 0; i < statePartialAdditionIndices_.size( ); i++ )
   {
       variationalMatrix_.block( 0, statePartialAdditionIndices_.at( i ).second, totalDynamicalStateSize_, 3 ) +=
               variationalMatrix_.block( 0, statePartialAdditionIndices_.at( i ).first, totalDynamicalStateSize_, 3 );
   }

   for( unsigned int i = 0; i < inertiaTensorsForMultiplication_.size( ); i++ )
   {
       variationalMatrix_.block( inertiaTensorsForMultiplication_.at( i ).first, 0, 3, totalDynamicalStateSize_ ) =
               ( inertiaTensorsForMultiplication_.at( i ).second( ).inverse( ) ) *
               variationalMatrix_.block( inertiaTensorsForMultiplication_.at( i ).first, 0, 3, totalDynamicalStateSize_ ).eval( );
   }

}

//! Function to clear reference/cached values of state derivative partials.
void VariationalEquations::clearPartials( )
{
    for( stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( );
         stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( );
         stateDerivativeTypeIterator_++ )
    {
        for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ )
        {
            for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ )
            {
                stateDerivativeTypeIterator_->second.at( i ).at( j )->resetTime( TUDAT_NAN );
            }

        }
    }
}

//! Function (called by constructor) to set up the statePartialList_ member from the state derivative partials
void VariationalEquations::setStatePartialFunctionList( )
{
    std::pair< std::function< void( Eigen::Block< Eigen::MatrixXd > ) >, int > currentDerivativeFunction;

    // Iterate over all state types
    for( std::map< propagators::IntegratedStateType,
         orbit_determination::StateDerivativePartialsMap >::iterator
         stateDerivativeTypeIterator_ = stateDerivativePartialList_.begin( );
         stateDerivativeTypeIterator_ != stateDerivativePartialList_.end( );
         stateDerivativeTypeIterator_++ )
    {
        // Iterate over all bodies undergoing 'accelerations' for which initial state is to be estimated.
        for( unsigned int i = 0; i < stateDerivativeTypeIterator_->second.size( ); i++ )
        {
            std::multimap< std::pair< int, int >, std::function< void( Eigen::Block< Eigen::MatrixXd > ) > >
                    currentBodyPartialList;

            // Iterate over all 'accelerations' from single body on other single body
            for( unsigned int j = 0; j < stateDerivativeTypeIterator_->second.at( i ).size( ); j++ )
            {
                for( std::map< propagators::IntegratedStateType,
                     std::vector< std::pair< std::string, std::string > > >::iterator
                     estimatedStateIterator = dynamicalStatesToEstimate_.begin( );
                     estimatedStateIterator != dynamicalStatesToEstimate_.end( );
                     estimatedStateIterator++ )
                {
                    // Iterate over all bodies to see if body exerting acceleration is also to be estimated (cross-terms)
                    for( unsigned int k = 0; k < estimatedStateIterator->second.size( ); k++ )
                    {
                        currentDerivativeFunction = stateDerivativeTypeIterator_->second.at( i ).at( j )->
                                getDerivativeFunctionWrtStateOfIntegratedBody(
                                    estimatedStateIterator->second.at( k ), estimatedStateIterator->first );

                        // If function is not-empty: add to list.
                        if( currentDerivativeFunction.second != 0 )
                        {
                            currentBodyPartialList.insert(
                                        std::make_pair(
                                            std::make_pair( k * getSingleIntegrationSize( estimatedStateIterator->first ) +
                                                            stateTypeStartIndices_.at( estimatedStateIterator->first ),
                                                            getSingleIntegrationSize( estimatedStateIterator->first ) ),
                                            currentDerivativeFunction.first ) );
                        }
                    }
                }
            }
            statePartialList_[ stateDerivativeTypeIterator_->first ].push_back( currentBodyPartialList );
        }
    }
}

template void VariationalEquations::getBodyInitialStatePartialMatrix< double >(
        const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& stateTransitionAndSensitivityMatrices,
        Eigen::Block< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative );

//#if( BUILD_EXTENDED_PRECISION_PROPAGATION_TOOLS )
template void VariationalEquations::getBodyInitialStatePartialMatrix< long double >(
        const Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic >& stateTransitionAndSensitivityMatrices,
        Eigen::Block< Eigen::Matrix< long double, Eigen::Dynamic, Eigen::Dynamic > > currentMatrixDerivative );
//#endif

} // namespace propagators

} // namespace tudat


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/maptbx/accessors/c_grid_padded_p1.h>
#include <cctbx/maptbx/statistics.h>
#include <boost/python/def.hpp>

#if PY_MAJOR_VERSION >= 3
#define IS_PY3K

void destroy_pyobject(PyObject* obj_ptr) {
  free(obj_ptr);
}

#endif

namespace cctbx { namespace maptbx { namespace boost_python {

namespace {

  // Based on code by N.K. Sauter
  template <typename InpFloatType,
            typename OutFloatType>
  struct as_CObjectZYX
  {
    typedef c_grid_padded_p1<3>::index_type index_type;

    static boost::python::object
    convert(
      af::const_ref<InpFloatType, c_grid_padded_p1<3> > const& map_unit_cell,
      index_type const& first,
      index_type const& last,
      bool apply_sigma_scaling)
    {
      InpFloatType mean = 0;
      InpFloatType sigma = 0;
      if (apply_sigma_scaling) {
        statistics<InpFloatType>
          map_statistics(af::const_ref<InpFloatType, af::flex_grid<> >(
            map_unit_cell.begin(),
            map_unit_cell.accessor().as_flex_grid()));
        mean = map_statistics.mean();
        sigma = map_statistics.sigma();
        if (sigma == 0) sigma = 1;
      }
      OutFloatType* out_mem = reinterpret_cast<OutFloatType*>(
        malloc(out_size(first, last) * sizeof(OutFloatType)));
      OutFloatType* out_ptr = out_mem;
      index_type out_pt;
      for (out_pt[2] = first[2]; out_pt[2] <= last[2]; out_pt[2]++) {
      for (out_pt[1] = first[1]; out_pt[1] <= last[1]; out_pt[1]++) {
      for (out_pt[0] = first[0]; out_pt[0] <= last[0]; out_pt[0]++) {
        InpFloatType val = map_unit_cell(out_pt);
        if (apply_sigma_scaling) val = (val - mean) / sigma;
        *out_ptr++ = static_cast<OutFloatType>(val);
      }}}
      return boost::python::object(boost::python::handle<>(
#ifdef IS_PY3K
        PyCapsule_New(out_mem, NULL, destroy_pyobject)));
#else
        PyCObject_FromVoidPtr(out_mem, free)));
#endif
    }

    static std::size_t
    out_size(index_type const& first, index_type const& last)
    {
      std::size_t result = 1;
      for(std::size_t i=0;i<3;i++) {
        CCTBX_ASSERT(last[i] >= first[i]);
        result *= (last[i] - first[i] + 1);
      }
      return result;
    }
  };

} // namespace <anoymous>

  void wrap_pymol_interface()
  {
    using namespace boost::python;
    def("as_CObjectZYX", as_CObjectZYX<float, float>::convert);
    def("as_CObjectZYX", as_CObjectZYX<double, float>::convert);
  }

}}} // namespace cctbx::maptbx::boost_python


#ifndef LBP_CSSILTP_HPP
#define LBP_CSSILTP_HPP

#include <lbp/defs.hpp>
#include <lbp/utils.hpp>
#include <lbp/detail/neighborhoods.hpp>
#include <lbp/detail/sampling.hpp>

#include <opencv2/core.hpp>

#include <boost/hana/integral_constant.hpp>
#include <boost/hana/functional/demux.hpp>

#include <boost/circular_buffer.hpp>
#include <boost/integer.hpp>

//
// @article{wu2014real,
//   title={Real-time background subtraction-based video surveillance of people by integrating local texture patterns},
//   author={Wu, Hefeng and Liu, Ning and Luo, Xiaonan and Su, Jiawei and Chen, Liangshi},
//   journal={Signal, Image and Video Processing},
//   volume={8},
//   number={4},
//   pages={665--676},
//   year={2014},
//   publisher={Springer}
// }
//

namespace lbp {
namespace cssiltp_detail {

template< typename T >
auto cssiltp = [](auto N, auto S) {
    return [=](const cv::Mat& lhs, const cv::Mat& rhs,
               size_t i, size_t j, const T& tau) {
        using namespace cv;
        using namespace hana::literals;

        return boost::hana::fold_left (
            N, 0, [&, shift = 0](auto accum, auto x) mutable {
                const auto a = S (lhs, i + x [0_c], j + x [1_c]);
                const auto b = S (rhs, i - x [0_c], j - x [1_c]);

                const auto lhs = saturate_cast< T > ((1. - tau) * a);
                const auto rhs = saturate_cast< T > ((1. + tau) * a);

                return accum | (
                    (b > rhs ? 1 : (b < lhs ? 2 : 0)) << (shift++ * 2));
            });
    };
};

} // namespace cssiltp_detail

template< typename T, size_t R, size_t P >
auto cssiltp = [](const T& tau = T { }) {
    //
    // The operator operates on center-symmetric pixels from
    // center-symmetric frames (see 2.1:CS-SILTP descriptor):
    //
    boost::circular_buffer< cv::Mat > framebuf (R << 1);

    return [=](const cv::Mat& src) mutable {
        LBP_STATIC_ASSERT_MSG (0 == (P % 2), "odd-sized neighborhood");

        framebuf.push_back (src.clone ());

        if (framebuf.size () < R * 2)
            return src;

        //
        // Limited by the largest integral type of cv::Mat:
        //
        using value_type = typename boost::uint_t< R * P >::least;

        cv::Mat dst (src.size (), opencv_type< value_type >, cv::Scalar (0));

        auto op = cssiltp_detail::cssiltp< T > (
            detail::semicircular_neighborhood< R, P >,
            detail::nearest_sampler< T >);

#pragma omp parallel for
        for (size_t i = R; i < src.rows - R; ++i) {
            for (size_t j = R; j < src.cols - R; ++j) {
                size_t n = 0;

                for (size_t r = 0; r < 2 * R; ++r) {
                    n <<= P/2;

                    const auto& lhs = framebuf [r];
                    const auto& rhs = framebuf [2 * R - r - 1];

                    n |= op (lhs, rhs, i, j, tau);
                }

                dst.at< value_type > (i, j) = n;
            }
        }

        return dst;
    };
};

} // namespace lbp

#endif // LBP_CSSILTP_HPP