//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/predecessor.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/mthree.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/bitincrement.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/four.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;
  using iT  = bd::as_integer_t<T>;
  using i_t =  bs::pack<iT, N>;

  T a1[N],  b[N];
  iT a2[N];

  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(-i/2) : T(i);
     a2[i] = i%4;
     b[i] = bs::predecessor(a1[i], a2[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  i_t aa2(&a2[0], &a2[0]+N);
  p_t bb(&b[0], &b[0]+N);

  STF_EQUAL(bs::predecessor(aa1, aa2), bb);
}

STF_CASE_TPL("Check predecessor on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" predecessor real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::predecessor;
  using p_t = bs::pack<T>;

  using r_t = decltype(predecessor(p_t()));

  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(predecessor(bs::Inf<p_t>()), bs::Valmax<r_t>());
  STF_IEEE_EQUAL(predecessor(bs::Minf<p_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(predecessor(bs::Nan<p_t>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(predecessor(bs::Mone<p_t>()), bs::Mone<r_t>()-bs::Eps<r_t>());
  STF_EQUAL(predecessor(bs::One<p_t>()), bs::One<r_t>()-bs::Halfeps<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>()), bs::Minf<r_t>());
#if !defined(BOOST_SIMD_NO_DENORMALS)
  STF_EQUAL(predecessor(bs::Zero<p_t>()), -bs::Bitincrement<p_t>());
#endif
} // end of test for floating_

STF_CASE_TPL (" predecessor ui ",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::predecessor;
  using p_t = bs::pack<T>;

  using r_t = decltype(predecessor(p_t()));

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

  // specific values tests
  STF_EQUAL(predecessor(bs::One<p_t>()), bs::Zero<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>()), bs::Valmin<r_t>());
  STF_EQUAL(predecessor(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" predecessor si",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::predecessor;
  using p_t = bs::pack<T>;

  using r_t = decltype(predecessor(p_t()));

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

  // specific values tests
  STF_EQUAL(predecessor(bs::Mone<p_t>()), -bs::Two<r_t>());
  STF_EQUAL(predecessor(bs::One<p_t>()), bs::Zero<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>()), bs::Valmin<r_t>());
  STF_EQUAL(predecessor(bs::Zero<p_t>()), bs::Mone<r_t>());
} // end of test for signed_int_

STF_CASE_TPL (" predecessor real 2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::predecessor;
  using p_t = bs::pack<T>;

  using ip_t = bd::as_integer_t<p_t>;
  using r_t = decltype(predecessor(p_t(), ip_t()));

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

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_IEEE_EQUAL(predecessor(bs::Minf<p_t>(), bs::Two<ip_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(predecessor(bs::Nan<p_t>(), bs::Two<ip_t>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(predecessor(bs::Mone<p_t>(), bs::Two<ip_t>()), bs::Mone<r_t>()-bs::Eps<r_t>()-bs::Eps<r_t>());
  STF_EQUAL(predecessor(bs::One<p_t>(), bs::Two<ip_t>()), bs::One<r_t>()-bs::Eps<r_t>());
  STF_IEEE_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Two<ip_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Four<ip_t>()), bs::Nan<r_t>());
#if !defined(BOOST_SIMD_NO_DENORMALS)
  STF_EQUAL(predecessor(bs::Zero<p_t>(), bs::Two<ip_t>()), -bs::Bitincrement<r_t>()-bs::Bitincrement<r_t>());
#endif
}

STF_CASE_TPL (" predecessorui_2",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::predecessor;
  using p_t = bs::pack<T>;

  using ip_t = bd::as_integer_t<p_t>;
  using r_t = decltype(predecessor(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);
  STF_EQUAL(predecessor(bs::Four<p_t>(), bs::Two<ip_t>()), bs::Two<r_t>());
  STF_EQUAL(predecessor(bs::One<p_t>(), bs::Two<ip_t>()), bs::Zero<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Two<ip_t>()), bs::Valmin<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Four<ip_t>()), bs::Valmin<r_t>());
}

STF_CASE_TPL (" predecessorsi_2",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::predecessor;
  using p_t = bs::pack<T>;

  using ip_t = bd::as_integer_t<p_t>;
  using r_t = decltype(predecessor(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);
  STF_EQUAL(predecessor(bs::Mone<p_t>(), bs::Two<ip_t>()), bs::Mthree<r_t>());
  STF_EQUAL(predecessor(bs::One<p_t>(), bs::Two<ip_t>()), bs::Mone<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Two<ip_t>()), bs::Valmin<r_t>());
  STF_EQUAL(predecessor(bs::Valmin<p_t>(), bs::Four<ip_t>()), bs::Valmin<r_t>());
} // end of test for signed_int_


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [rem_pio2_medium]
#include <boost/simd/trigonometric.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 4>;

int main()
{
  pack_ft pf = {1.0f, 2.0f, -1.0f, 0.5f};
  pack_ft pe, pm;
  std::tie(pe, pm) = bs::rem_pio2_medium(pf);
  std::cout
    << "---- simd" << '\n'
    << "<- pf =           " << pf << '\n'
    << "-> std::tie(pe, pm) = bs::rem_pio2_medium(pf) = " << '\n'
    << "-> pe = " << pe << '\n'
    << "-> pm = " << pm << '\n';

  float xf = 2.0f;
  float e, m;
  std::tie(e, m) = bs::rem_pio2_medium(xf);
  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =           " << xf << '\n'
    << "-> std::tie(pe, pm) =  bs::rem_pio2_medium(xf) = " << '\n'
    << "-> e =  " << e <<  '\n'
    << "-> m =  " << m <<  '\n';
  return 0;
}
//! [rem_pio2_medium]


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

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

 * See LICENSE for the license information

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

/**
 *  @file  PreintegrationBase.h
 *  @author Luca Carlone
 *  @author Stephen Williams
 *  @author Richard Roberts
 *  @author Vadim Indelman
 *  @author David Jensen
 *  @author Frank Dellaert
 *  @author Varun Agrawal
 **/

#include "PreintegrationBase.h"
#include <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>

using namespace std;

namespace gtsam {

//------------------------------------------------------------------------------
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
                                       const Bias& biasHat)
    : p_(p), biasHat_(biasHat), deltaTij_(0.0) {
}

//------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
  os << "    deltaTij = " << pim.deltaTij_ << endl;
  os << "    deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
  os << "    deltaPij = " << pim.deltaPij().transpose() << endl;
  os << "    deltaVij = " << pim.deltaVij().transpose() << endl;
  os << "    gyrobias = " << pim.biasHat_.gyroscope().transpose() << endl;
  os << "    acc_bias = " << pim.biasHat_.accelerometer().transpose() << endl;
  return os;
}

//------------------------------------------------------------------------------
void PreintegrationBase::print(const string& s) const {
  cout << (s.empty() ? s : s + "\n") << *this << endl;
}

//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
	biasHat_ = biasHat;
	resetIntegration();
}

//------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
    const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
    OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc,
    OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega,
    OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const {
  assert(p().body_P_sensor);

  // Compensate for sensor-body displacement if needed: we express the quantities
  // (originally in the IMU frame) into the body frame
  // Equations below assume the "body" frame is the CG

  // Get sensor to body rotation matrix
  const Matrix3 bRs = p().body_P_sensor->rotation().matrix();

  // Convert angular velocity and acceleration from sensor to body frame
  Vector3 correctedAcc = bRs * unbiasedAcc;
  const Vector3 correctedOmega = bRs * unbiasedOmega;

  // Jacobians
  if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs;
  if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3;
  if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs;

  // Centrifugal acceleration
  const Vector3 b_arm = p().body_P_sensor->translation();
  if (!b_arm.isZero()) {
    // Subtract out the the centripetal acceleration from the unbiased one
    // to get linear acceleration vector in the body frame:
    const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
    const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
    correctedAcc -= body_Omega_body * b_velocity_bs;

    // Update derivative: centrifugal causes the correlation between acc and omega!!!
    if (correctedAcc_H_unbiasedOmega) {
      double wdp = correctedOmega.dot(b_arm);
      const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
      *correctedAcc_H_unbiasedOmega = -( diag_wdp
          + correctedOmega * b_arm.transpose()) * bRs.matrix()
          + 2 * b_arm * unbiasedOmega.transpose();
    }
  }

  return make_pair(correctedAcc, correctedOmega);
}

//------------------------------------------------------------------------------
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
    const Vector3& measuredOmega, double dt) {
  // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
  // even when not of interest to the caller. Provide scratch space here.
  Matrix9 A;
  Matrix93 B, C;
  update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}

//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
    OptionalJacobian<9, 6> H2) const {
  Matrix96 D_biasCorrected_bias;
  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
                                             H2 ? &D_biasCorrected_bias : nullptr);

  // Correct for initial velocity and gravity
  Matrix9 D_delta_state, D_delta_biasCorrected;
  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
                                  p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
                                  H2 ? &D_delta_biasCorrected : nullptr);

  // Use retract to get back to NavState manifold
  Matrix9 D_predict_state, D_predict_delta;
  NavState state_j = state_i.retract(xi,
                                     H1 ? &D_predict_state : nullptr,
                                     H2 || H2 ? &D_predict_delta : nullptr);
  if (H1)
    *H1 = D_predict_state + D_predict_delta * D_delta_state;
  if (H2)
    *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
  return state_j;
}

//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeError(const NavState& state_i,
                                         const NavState& state_j,
                                         const imuBias::ConstantBias& bias_i,
                                         OptionalJacobian<9, 9> H1,
                                         OptionalJacobian<9, 9> H2,
                                         OptionalJacobian<9, 6> H3) const {
  // Predict state at time j
  Matrix9 D_predict_state_i;
  Matrix96 D_predict_bias_i;
  NavState predictedState_j = predict(
      state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0);

  // Calculate error
  Matrix9 D_error_state_j, D_error_predict;
  Vector9 error =
      state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
                               H1 || H3 ? &D_error_predict : 0);

  if (H1) *H1 << D_error_predict* D_predict_state_i;
  if (H2) *H2 << D_error_state_j;
  if (H3) *H3 << D_error_predict* D_predict_bias_i;

  return error;
}

//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
    const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
    OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
    OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {

  // Note that derivative of constructors below is not identity for velocity, but
  // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
  NavState state_i(pose_i, vel_i);
  NavState state_j(pose_j, vel_j);

  // Predict state at time j
  Matrix9 D_error_state_i, D_error_state_j;
  Vector9 error = computeError(state_i, state_j, bias_i,
                         H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5);

  // Separate out derivatives in terms of 5 arguments
  // Note that doing so requires special treatment of velocities, as when treated as
  // separate variables the retract applied will not be the semi-direct product in NavState
  // Instead, the velocities in nav are updated using a straight addition
  // This is difference is accounted for by the R().transpose calls below
  if (H1) *H1 << D_error_state_i.leftCols<6>();
  if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose();
  if (H3) *H3 << D_error_state_j.leftCols<6>();
  if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();

  return error;
}

//------------------------------------------------------------------------------

}  // namespace gtsam


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_EIGEN3_DIAGONALIZE_HPP
#define ROKKO_EIGEN3_DIAGONALIZE_HPP

#include <rokko/localized_matrix.hpp>
#include <rokko/localized_vector.hpp>
#include <rokko/parameters.hpp>
#include <Eigen/Dense>

namespace rokko {
namespace eigen3 {

char get_matrix_part(rokko::parameters const& params) {
  std::string matrix_part;
  if (params.defined("uplow"))
    matrix_part = params.get_string("uplow");
  if (params.defined("matrix_part"))
    matrix_part = params.get_string("matrix_part");
  if ((matrix_part[0] == 'u') || (matrix_part[0] == 'U'))
    return 'U';
  if ((matrix_part[0] == 'l') || (matrix_part[0] == 'L'))
    return 'L';
  else
    return '\0';
}

// only eigenvalues
template<typename T, typename MATRIX_MAJOR>
parameters diagonalize(localized_matrix<T, MATRIX_MAJOR>& mat, localized_vector<T>& eigvals,
		       rokko::parameters const& params) {
  parameters params_out;
  if (get_matrix_part(params) == 'U') {
    std::cerr << "Error: eigen3:qr (Eigen::SelfAdjointEigenSolver) does not support upper part of matrix." << std::endl;
    throw;
  }
  int info = 0;
  Eigen::SelfAdjointEigenSolver<typename localized_matrix<T, MATRIX_MAJOR>::super_type> ES(mat);
  eigvals = ES.eigenvalues();
  return params_out;
}

template<typename T, typename MATRIX_MAJOR>
parameters diagonalize(localized_matrix<T, MATRIX_MAJOR>& mat, std::vector<T>& eigvals_in,
		       rokko::parameters const& params) {
  parameters params_out;
  if (get_matrix_part(params) == 'U') {
    std::cerr << "Error: eigen3:qr (Eigen::SelfAdjointEigenSolver) does not support upper part of matrix." << std::endl;
    throw;
  }
  int info = 0;
  int dim = mat.rows();
  if (eigvals_in.size() < dim) eigvals_in.resize(dim);
  Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1> > eigvals(&eigvals_in[0], eigvals_in.size());
  Eigen::SelfAdjointEigenSolver<typename localized_matrix<T, MATRIX_MAJOR>::super_type> ES(mat);
  eigvals = ES.eigenvalues();
  return params_out;
}

// eigenvalues/eigenvectors
template<typename T, typename MATRIX_MAJOR>
parameters diagonalize(localized_matrix<T, MATRIX_MAJOR>& mat, localized_vector<T>& eigvals,
		       localized_matrix<T, MATRIX_MAJOR>& eigvecs, rokko::parameters const& params) {
  parameters params_out;
  if (get_matrix_part(params) == 'U') {
    std::cerr << "Error: eigen3:qr (Eigen::SelfAdjointEigenSolver) does not support upper part of matrix." << std::endl;
    throw;
  }
  int info = 0;
  Eigen::SelfAdjointEigenSolver<typename localized_matrix<T, MATRIX_MAJOR>::super_type> ES(mat);
  eigvals = ES.eigenvalues();
  eigvecs = ES.eigenvectors();
  return params_out;
}

template<typename T, typename MATRIX_MAJOR>
parameters diagonalize(localized_matrix<T, MATRIX_MAJOR>& mat, std::vector<T>& eigvals_in,
                localized_matrix<T, MATRIX_MAJOR>& eigvecs, rokko::parameters const& params) {
  parameters params_out;
  if (get_matrix_part(params) == 'U') {
    std::cerr << "Error: eigen3:qr (Eigen::SelfAdjointEigenSolver) does not support upper part of matrix." << std::endl;
    throw;
  }
  int info = 0;
  int dim = mat.rows();
  if (eigvals_in.size() < dim) eigvals_in.resize(dim);
  Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1> > eigvals(&eigvals_in[0], eigvals_in.size());
  Eigen::SelfAdjointEigenSolver<typename localized_matrix<T, MATRIX_MAJOR>::super_type> ES(mat);
  eigvals = ES.eigenvalues();
  eigvecs = ES.eigenvectors();
  return params_out;
}

} // namespace eigen3
} // namespace rokko

#endif // ROKKO_EIGEN3_DIAGONALIZE_HPP


#include <iostream>
#include <random>

#if !defined(EXRANDOM_HAVE_MPREAL)
#define EXRANDOM_HAVE_MPREAL 0
#endif
#if !defined(EXRANDOM_HAVE_BOOST_DEC)
#define EXRANDOM_HAVE_BOOST_DEC 0
#endif
#if !defined(EXRANDOM_HAVE_BOOST_GMP)
#define EXRANDOM_HAVE_BOOST_GMP 0
#endif
#if !defined(EXRANDOM_HAVE_BOOST_MPFR)
#define EXRANDOM_HAVE_BOOST_MPFR 0
#endif
#if !defined(EXRANDOM_HAVE_BOOST_FLOAT128)
#define EXRANDOM_HAVE_BOOST_FLOAT128 0
#endif

#if EXRANDOM_HAVE_MPREAL
#include <mpreal.h>
#endif
#if EXRANDOM_HAVE_BOOST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#if EXRANDOM_HAVE_BOOST_GMP
#include <boost/multiprecision/gmp.hpp>
#endif
#if EXRANDOM_HAVE_BOOST_DEC
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif
#if EXRANDOM_HAVE_BOOST_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

// N.B. This #include should come *after* the headers which define the
// floating point types.
#include <exrandom/unit_normal_distribution.hpp>

int main() {

#if EXRANDOM_HAVE_MPREAL || EXRANDOM_HAVE_BOOST_MPFR || \
  EXRANDOM_HAVE_BOOST_GMP || EXRANDOM_HAVE_BOOST_DEC || \
  EXRANDOM_HAVE_BOOST_FLOAT128
  unsigned s = std::random_device()(); // Set seed from random_device
  std::mt19937 g(s);                   // Initialize URNG
  std::cout << "Seed set to " << s << "\n\n";
#endif

#if EXRANDOM_HAVE_MPREAL
  {
    typedef mpfr::mpreal real;
    real::set_default_prec(240);
    exrandom::unit_normal_distribution<real> N;
    std::cout
      << "Sampling mpreal from the unit normal distribution:\n"
      << std::setprecision(mpfr::bits2digits(real::get_default_prec()));
    for (int i = 0; i < 20; ++i)
      std::cout << N(g) << "\n";
    std::cout << "\n";
  }
#else
  std::cerr << "This example requires mpreal.h\n\n";
#endif

#if EXRANDOM_HAVE_BOOST_MPFR
  {
    typedef boost::multiprecision::mpfr_float_50 real;
    exrandom::unit_normal_distribution<real> N;
    std::cout
      << "Sampling mpfr_float_50 from the unit normal distribution:\n"
      << std::setprecision(50);
    for (int i = 0; i < 20; ++i)
      std::cout << N(g) << "\n";
    std::cout << "\n";
  }
#else
  std::cerr << "This example requires boost/multiprecision/mpfr.hpp\n\n";
#endif

#if EXRANDOM_HAVE_BOOST_GMP
  {
    typedef boost::multiprecision::mpf_float_50 real;
    exrandom::unit_normal_distribution<real> N;
    std::cout
      << "Sampling mpf_float_50 from the unit normal distribution:\n"
      << std::setprecision(50);
    for (int i = 0; i < 20; ++i)
      std::cout << N(g) << "\n";
    std::cout << "\n";
  }
#else
  std::cerr << "This example requires boost/multiprecision/gmp.hpp\n\n";
#endif

#if EXRANDOM_HAVE_BOOST_DEC
  {
    typedef
      boost::multiprecision::number<boost::multiprecision::cpp_dec_float<20U>>
      real;
    exrandom::unit_normal_distribution<real> N;
    std::cout <<
      "Sampling number<cpp_dec_float<20U>> from the unit normal distribution:\n"
      << std::setprecision(20);
    for (int i = 0; i < 20; ++i)
      std::cout << N(g) << "\n";
    std::cout << "\n";
  }
#else
  std::cerr
    << "This example requires boost/multiprecision/cpp_dec_float.hpp\n\n";
#endif

#if EXRANDOM_HAVE_BOOST_FLOAT128
  {
    typedef boost::multiprecision::float128 real;
    exrandom::unit_normal_distribution<real> N;
    std::cout
      << "Sampling float128 exactly from the unit normal distribution:\n"
      << std::setprecision(33);
    for (int i = 0; i < 20; ++i)
      std::cout << N(g) << "\n";
    std::cout << "\n";
  }
#else
  std::cerr << "This example requires boost/multiprecision/float128.hpp\n\n";
#endif
}


/**
 * Created on Wed Jan 05 2022
 *
 * Shane Flandermeyer, shaneflandermeyer@gmail.com
 *
 * Perform 2D cell-averaging (CA) CFAR detection from the data given in the
 * matlab example at
 * https://www.mathworks.com/help/phased/ug/constant-false-alarm-rate-cfar-detection.html;jsessionid=a6023502856c0dcfb2d70ddac1a2
 */

// #include "cfar.h"
#include "cfar2d.h"
#include "matrix_utils.h"

#include <fstream>
#include <random>
#include <vector>
#include <Eigen/Dense>
#include <matplot/matplot.h>
#include <pulse_doppler.h>

using namespace plasma;
using namespace matplot;
using namespace Eigen;

std::vector<double> read_file(std::string filename) {
  // open the file:
  std::streampos fileSize;
  std::ifstream file(filename, std::ios::binary);

  // get its size:
  file.seekg(0, std::ios::end);
  fileSize = file.tellg();
  file.seekg(0, std::ios::beg);

  // read the data:
  std::vector<double> fileData(fileSize);
  file.read((char *)&fileData[0], fileSize);
  return fileData;
}

int main() {
  // TODO: Change this back to 2D
  CFARDetector2D cfar{2, 5, 1e-5};

  // Read range doppler map data from a file
  // TODO: Make a way to generate this internally
  std::vector<double> data = read_file("/home/shane/data.bin");
  ArrayXXd x = Map<ArrayXXd>(data.data(), 200, 1024);
  // DetectionReport det;
  auto start = std::chrono::high_resolution_clock::now();
  DetectionReport det = cfar.detect(x);
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration =
      std::chrono::duration_cast<std::chrono::milliseconds>(stop - start);
  std::cout << "CFAR took " << duration.count() << "  milliseconds"
            << std::endl;
  std::cout << "Number of Detections: " << det.num_detections << std::endl;
  // cfar.detect(x,6,852);

  // Figures
  figure();

  // Range doppler map
  x = 10 * log10(x);
  std::vector<std::vector<double>> rd_map(x.rows(),
                                          std::vector<double>(x.cols()));
  for (uint i = 0; i < x.rows(); i++) {
    for (uint j = 0; j < x.cols(); j++) {
      rd_map[i][j] = x.operator()(i, j);
    }
  }

  // Rows and columns of each CFAR detection
  std::vector<double> detection_row(det.indices.col(0).data(),
                                    det.indices.col(0).data() +
                                        det.indices.col(0).size());
  std::vector<double> detection_col(det.indices.col(1).data(),
                                    det.indices.col(1).data() +
                                        det.indices.col(1).size());

  imagesc(rd_map);
  hold(true);
  plot(detection_col, detection_row, "r+");
  hold(false);
  gca()->y_axis().reverse(false);
  show();
  return 0;
}


#pragma once
#include <unordered_map>
#include <string>
#include <memory>
#include <boost/thread/thread.hpp>
#include <stdint.h>
#include <atomic>
#include <chrono>

class Bucket {
private:
    std::atomic<double> _zero_time;
public:
    Bucket(double zero_time = 0.0) :
        _zero_time(zero_time) {};
    bool aquire(double now, double rate, double burst_size, double consume = 1.0) {
        auto zero_time_old = _zero_time.load();
        double zero_time_new;
        do {
            double tokens = std::min((now - zero_time_old) * rate, burst_size);
            if (tokens < consume) {
                return false;
            } else {
                tokens -= consume;
            }
            zero_time_new = now - tokens / rate;
        } while(!_zero_time.compare_exchange_weak(zero_time_old, zero_time_new));

        return true;
    }
};

class RateLimiter {
private:
    double _rate;
    double _burst_size;

    std::unordered_map<std::string, std::unique_ptr<Bucket> > _key_to_bucket;
    boost::shared_mutex _key_to_bucket_mu;

    static double now() {
        using dur = std::chrono::duration<double>;
        auto const now = std::chrono::steady_clock::now().time_since_epoch();
        return std::chrono::duration_cast<dur>(now).count();
    }
public:
    RateLimiter(double rate, double burst_size) :
        _rate(rate),
        _burst_size(burst_size) {};
    bool aquire(std::string &key) {
        boost::upgrade_lock<boost::shared_mutex> lock(_key_to_bucket_mu);

        auto bucket_it = _key_to_bucket.find(key);
        if (bucket_it == _key_to_bucket.end()) {
            boost::upgrade_to_unique_lock<boost::shared_mutex> unique_lock(lock);
            std::unique_ptr<Bucket> bucket(new Bucket());
            _key_to_bucket[key] = std::move(bucket);
            return true;
        } else {
            return bucket_it->second->aquire(now(), _rate, _burst_size);
        }
    };
};


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <random/random.hpp>
#include <random/alias.hpp>
#include <parallel/thread_pool.hpp>
#include <cmath>
#include <iostream>
#include <vector>

typedef double vertex_data_type;
typedef double edge_data_type;

template<typename NumType>
void uniform_speed(const size_t max_iter) {
  NumType sum(0);
  turi::timer ti;
  ti.start();
  for(size_t i = 0; i < max_iter; ++i) {
    sum += (NumType)(turi::random::uniform<NumType>(0, 10));
  }
  double slow_time = ti.current_time();
  ti.start();
  for(size_t i = 0; i < max_iter; ++i) {
    sum += (NumType)(turi::random::fast_uniform<NumType>(0, 10));
  }
  double fast_time = ti.current_time();
  std::cout << slow_time << ", " << fast_time << std::endl;
}

class thread_worker {
public:
  std::vector<int> values;
  void run() {
    namespace random = turi::random;
    for(size_t i = 0; i < values.size(); ++i) {
      values[i] = random::uniform<int>(0,3);
    }
  }
};

template<typename T>
std::ostream& operator<<(std::ostream& out, const std::vector<T>& values) {
  out << "{";
  for(size_t i = 0; i < values.size(); ++i) {
    out << values[i];
    if(i + 1 < values.size()) out << ", ";
  }
  return out << "}";
}

std::vector<int> operator+(const std::vector<int>& v1,
                           const std::vector<int>& v2) {
  assert(v1.size() == v2.size());
  std::vector<int> result(v1.size());
  for(size_t i = 0; i < result.size(); ++i) {
    result[i] = v1[i] + v2[i];
  }
  return result;
}

struct RandomTestSuite {
  size_t iterations;
  RandomTestSuite() : iterations(1E8) { }

  void test_nondet_generator() {
    turi::random::nondet_seed();
    turi::random::nondet_seed();
    turi::random::nondet_seed();
  }

  void test_random_number_generators() {
    std::cout << std::endl;
    std::cout << "beginning seed" << std::endl;
    namespace random = turi::random;
    turi::random::seed();
    turi::random::time_seed();
    turi::random::nondet_seed();
    turi::random::seed(12345);
    std::cout << "finished" << std::endl;

    const size_t num_iterations(20);
    std::vector<thread_worker> workers(10);
    for(size_t i = 0; i < workers.size(); ++i)
      workers[i].values.resize(num_iterations);
    turi::thread_group threads;
    for(size_t i = 0; i < workers.size(); ++i) {
      threads.launch(boost::bind(&thread_worker::run, &(workers[i])));
    }
    threads.join();
    for(size_t i = 0; i < workers.size(); ++i) {
      std::cout << workers[i].values << std::endl;
    }
    std::vector<int> sum(workers[0].values.size());
    for(size_t i = 0; i < workers.size(); ++i) {
      sum = sum + workers[i].values;
    }
    std::cout << "Result: " << sum << std::endl;
  }

  void shuffle() {
    namespace random = turi::random;
    random::nondet_seed();
    std::vector<int> numbers(100);
    for(size_t i = 0; i < numbers.size(); ++i) numbers[i] = (int)i + 1;
    for(size_t j = 0; j < 10; ++j) {
      // shuffle the numbers
      random::shuffle(numbers);
      std::cout << numbers << std::endl;
    }
  }

};

BOOST_FIXTURE_TEST_SUITE(_RandomTestSuite, RandomTestSuite)
  BOOST_AUTO_TEST_CASE(test_nondet_generator) {
    RandomTestSuite::test_nondet_generator();
  }
  BOOST_AUTO_TEST_CASE(test_random_number_generators) {
    RandomTestSuite::test_random_number_generators();
  }
BOOST_AUTO_TEST_SUITE_END()

  // void speed() {
  //   namespace random = turi::random;
  //   std::cout << "speed test run: " << std::endl;
  //   const size_t MAX_ITER(10000);
  //   std::cout << "size_t:   ";
  //   uniform_speed<size_t>(MAX_ITER);
  //   std::cout << "int:      ";
  //   uniform_speed<int>(MAX_ITER);
  //   std::cout << "uint32_t: ";
  //   uniform_speed<uint32_t>(MAX_ITER);
  //   std::cout << "uint16_t: ";
  //   uniform_speed<uint16_t>(MAX_ITER);
  //   std::cout << "char:     ";
  //   uniform_speed<char>(MAX_ITER);
  //   std::cout << "float:    ";
  //   uniform_speed<float>(MAX_ITER);
  //   std::cout << "double:   ";
  //   uniform_speed<double>(MAX_ITER);

  //   std::cout << "gaussian: ";
  //   double sum = 0;
  //   turi::timer time;
  //   time.start();
  //   for(size_t i = 0; i < MAX_ITER; ++i)
  //     sum += random::gaussian();
  //   std::cout << time.current_time() << std::endl;

  //   std::cout << "shuffle:  ";
  //   std::vector<int> numbers(6);
  //   for(size_t i = 0; i < numbers.size(); ++i) numbers[i] = (int)i + 1;
  //   time.start();
  //   for(size_t j = 0; j < MAX_ITER/numbers.size(); ++j) {
  //     // shuffle the numbers
  //     random::shuffle(numbers);
  //   }
  //   std::cout << time.current_time() << ", ";
  //   time.start();
  //   for(size_t j = 0; j < MAX_ITER/numbers.size(); ++j) {
  //     // shuffle the numbers
  //     std::random_shuffle(numbers.begin(), numbers.end());
  //   }
  //   std::cout << time.current_time() << std::endl;
  // }


#include <NTL/ZZ_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

ZZ_pEInfoT::ZZ_pEInfoT(const ZZ_pX& NewP)
{
   ref_count = 1;

   build(p, NewP);

   _card_init = 0;
   _card_base = ZZ_p::modulus();
   _card_exp = deg(NewP);
}

const ZZ& ZZ_pE::cardinality()
{
   if (!ZZ_pEInfo) Error("ZZ_pE::cardinality: undefined modulus");

   if (!ZZ_pEInfo->_card_init) {
      power(ZZ_pEInfo->_card, ZZ_pEInfo->_card_base, ZZ_pEInfo->_card_exp);
      ZZ_pEInfo->_card_init = 1;
   }

   return ZZ_pEInfo->_card;
}

ZZ_pEInfoT *ZZ_pEInfo = 0;

typedef ZZ_pEInfoT *ZZ_pEInfoPtr;

static
void CopyPointer(ZZ_pEInfoPtr& dst, ZZ_pEInfoPtr src)
{
   if (src == dst) return;

   if (dst) {
      dst->ref_count--;

      if (dst->ref_count < 0)
         Error("internal error: negative ZZ_pEContext ref_count");

      if (dst->ref_count == 0) delete dst;
   }

   if (src) {
      if (src->ref_count == NTL_MAX_LONG)
         Error("internal error: ZZ_pEContext ref_count overflow");

      src->ref_count++;
   }

   dst = src;
}

void ZZ_pE::init(const ZZ_pX& p)
{
   ZZ_pEContext c(p);
   c.restore();
}

ZZ_pEContext::ZZ_pEContext(const ZZ_pX& p)
{
   ptr = NTL_NEW_OP ZZ_pEInfoT(p);
}

ZZ_pEContext::ZZ_pEContext(const ZZ_pEContext& a)
{
   ptr = 0;
   CopyPointer(ptr, a.ptr);
}

ZZ_pEContext& ZZ_pEContext::operator=(const ZZ_pEContext& a)
{
   CopyPointer(ptr, a.ptr);
   return *this;
}

ZZ_pEContext::~ZZ_pEContext()
{
   CopyPointer(ptr, 0);
}

void ZZ_pEContext::save()
{
   CopyPointer(ptr, ZZ_pEInfo);
}

void ZZ_pEContext::restore() const
{
   CopyPointer(ZZ_pEInfo, ptr);
}

ZZ_pEBak::~ZZ_pEBak()
{
   if (MustRestore)
      CopyPointer(ZZ_pEInfo, ptr);

   CopyPointer(ptr, 0);
}

void ZZ_pEBak::save()
{
   MustRestore = 1;
   CopyPointer(ptr, ZZ_pEInfo);
}

void ZZ_pEBak::restore()
{
   MustRestore = 0;
   CopyPointer(ZZ_pEInfo, ptr);
}

const ZZ_pE& ZZ_pE::zero()
{
   static ZZ_pE z(ZZ_pE_NoAlloc);
   return z;
}

ZZ_pE::ZZ_pE()
{
   _ZZ_pE__rep.rep.SetMaxLength(ZZ_pE::degree());
}

istream& operator>>(istream& s, ZZ_pE& x)
{
   ZZ_pX y;

   s >> y;
   conv(x, y);

   return s;
}

void div(ZZ_pE& x, const ZZ_pE& a, const ZZ_pE& b)
{
   ZZ_pE t;

   inv(t, b);
   mul(x, a, t);
}

void div(ZZ_pE& x, const ZZ_pE& a, long b)
{
   NTL_ZZ_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(ZZ_pE& x, const ZZ_pE& a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(B);
   B = b;
   inv(B, B);
   mul(x, a, B);
}

void div(ZZ_pE& x, long a, const ZZ_pE& b)
{
   ZZ_pE t;
   inv(t, b);
   mul(x, a, t);
}

void div(ZZ_pE& x, const ZZ_p& a, const ZZ_pE& b)
{
   ZZ_pE t;
   inv(t, b);
   mul(x, a, t);
}

void inv(ZZ_pE& x, const ZZ_pE& a)
{
   InvMod(x._ZZ_pE__rep, a._ZZ_pE__rep, ZZ_pE::modulus());
}

NTL_END_IMPL


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [operators]
#include <boost/simd/predicates.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/if_else.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = {0.0f, 1.0f, -1.0f, 2.5f, -2.5f, bs::Nan<float>(), bs::Minf<float>(), bs::Inf<float>()};
  pack_ft q = bs::if_else(bs::is_odd(p), p, p*2.0f);

  std::cout << " p =  " << p << std::endl
            << " q =  " << q << std::endl
            <<  " -> (p == q) =  " << (p == q) << std::endl
            <<  " -> (p <= q) =  " << (p <= q) << std::endl
            <<  " -> (p >= q) =  " << (p >= q) << std::endl
            <<  " -> (p <  q) =  " << (p <  q) << std::endl
            <<  " -> (p >  q) =  " << (p >  q) << std::endl
            <<  " -> (p != q) =  " << (p != q) << std::endl
            <<  " -> !(p >= q) = " << !(p >= q)<< std::endl;
  return 0;
}
//! [operators]


/* boost random/exponential_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: exponential_distribution.hpp,v 1.12 2002/12/22 22:03:10 jmaurer Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {

// exponential distribution: p(x) = lambda * exp(-lambda * x)
template<class UniformRandomNumberGenerator, class RealType = double,
        class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class exponential_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit exponential_distribution(base_type& rng,
                                    result_type lambda = result_type(1))
    : _rng(rng), _lambda(lambda) { assert(lambda > result_type(0)); }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    return -result_type(1) / _lambda * log(result_type(1)-_rng());
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const exponential_distribution& x,
                         const exponential_distribution& y)
  { return x._lambda == y._lambda && x._rng == y._rng; }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const exponential_distribution& ed)
  {
    os << ed._lambda;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, exponential_distribution& ed)
  {
    is >> std::ws >> ed._lambda;
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const exponential_distribution& rhs) const
  { return _lambda == rhs._lambda && _rng == rhs._rng;  }
#endif
private:
  adaptor_type _rng;
  result_type _lambda;
};

} // namespace boost

#endif // BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP


/*
 * WavePI.cpp
 *
 *  Created on: 08.08.2017
 *      Author: thies
 */

#include <WavePI.h>
#include <base/ConstantMesh.h>
#include <base/Norm.h>
#include <base/Transformation.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/utilities.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/tria.h>
#include <inversion/InversionProgress.h>
#include <inversion/NonlinearLandweber.h>
#include <inversion/PostProcessor.h>
#include <inversion/REGINN.h>
#include <inversion/Regularization.h>
#include <measurements/ConvolutionMeasure.h>
#include <measurements/CubeBoundaryDistribution.h>
#include <measurements/DeltaMeasure.h>
#include <measurements/FieldMeasure.h>
#include <measurements/GridDistribution.h>
#include <measurements/MaskedFieldMeasure.h>
#include <measurements/SensorDistribution.h>
#include <measurements/SensorValues.h>
#include <norms/H1H1.h>
#include <norms/H1L2.h>
#include <norms/H2L2.h>
#include <norms/H2L2PlusL2H1.h>
#include <norms/L2Coefficients.h>
#include <norms/L2L2.h>
#include <norms/LPWrapper.h>
#include <problems/CProblem.h>
#include <problems/ConstantRhoProblem.h>
#include <problems/ConstantCProblem.h>
#include <problems/NuProblem.h>
#include <problems/QProblem.h>
#include <problems/RhoProblem.h>
#include <iostream>
#include <string>

namespace wavepi {

using namespace dealii;
using namespace wavepi::forward;
using namespace wavepi::base;
using namespace wavepi;
using namespace wavepi::inversion;
using namespace wavepi::measurements;
using namespace wavepi::problems;

template <int dim, typename Meas>
WavePI<dim, Meas>::WavePI(std::shared_ptr<SettingsManager> cfg) : cfg(cfg) {
  norm_vector       = std::make_shared<norms::L2Coefficients<dim>>();
  norm_l2l2         = std::make_shared<norms::L2L2<dim>>();
  norm_h1l2         = std::make_shared<norms::H1L2<dim>>(cfg->norm_h1l2_alpha);
  norm_h2l2         = std::make_shared<norms::H2L2<dim>>(cfg->norm_h2l2_alpha, cfg->norm_h2l2_beta);
  norm_h1h1         = std::make_shared<norms::H1H1<dim>>(cfg->norm_h1h1_alpha, cfg->norm_h1h1_gamma);
  norm_h2l2plusl2h1 = std::make_shared<norms::H2L2PlusL2H1<dim>>(
      cfg->norm_h2l2plusl2h1_alpha, cfg->norm_h2l2plusl2h1_beta, cfg->norm_h2l2plusl2h1_gamma);

  switch (cfg->norm_codomain) {
    case SettingsManager::NormType::vector:
      norm_codomain = norm_vector;
      break;
    case SettingsManager::NormType::l2l2:
      norm_codomain = norm_l2l2;
      break;
    case SettingsManager::NormType::h1l2:
      norm_codomain = norm_h1l2;
      break;
    case SettingsManager::NormType::h2l2:
      norm_codomain = norm_h2l2;
      break;
    case SettingsManager::NormType::h1h1:
      norm_codomain = norm_h1h1;
      break;
    case SettingsManager::NormType::h2l2plusl2h1:
      norm_codomain = norm_h2l2plusl2h1;
      break;
    default:
      AssertThrow(false, ExcMessage("unknown norm of codomain"));
  }

  switch (cfg->norm_domain) {
    case SettingsManager::NormType::vector:
      norm_domain = norm_vector;
      break;
    case SettingsManager::NormType::l2l2:
      norm_domain = norm_l2l2;
      break;
    case SettingsManager::NormType::h1l2:
      norm_domain = norm_h1l2;
      break;
    case SettingsManager::NormType::h2l2:
      norm_domain = norm_h2l2;
      break;
    case SettingsManager::NormType::h1h1:
      norm_domain = norm_h1h1;
      break;
    case SettingsManager::NormType::h2l2plusl2h1:
      norm_domain = norm_h2l2plusl2h1;
      break;
    default:
      AssertThrow(false, ExcMessage("WavePI:: unknown norm of codomain"))
  }

  if (cfg->norm_domain_enable_wrapping)
    norm_domain = std::make_shared<norms::LPWrapper<dim>>(norm_domain, cfg->norm_domain_p);

  initial_guess = std::make_shared<MacroFunctionParser<dim>>(cfg->expr_initial_guess, cfg->constants_for_exprs);

  param_rho = MacroFunctionParser<dim>::parse(cfg->expr_param_rho, cfg->constants_for_exprs, cfg->shape_scale);
  param_nu  = MacroFunctionParser<dim>::parse(cfg->expr_param_nu, cfg->constants_for_exprs, cfg->shape_scale);
  param_c   = MacroFunctionParser<dim>::parse(cfg->expr_param_c, cfg->constants_for_exprs, cfg->shape_scale);
  param_q   = MacroFunctionParser<dim>::parse(cfg->expr_param_q, cfg->constants_for_exprs, cfg->shape_scale);

  param_background = std::make_shared<MacroFunctionParser<dim>>(cfg->expr_param_background, cfg->constants_for_exprs);
}

template <int dim, typename Meas>
Point<dim> WavePI<dim, Meas>::make_point(double x, double y, double z) {
  switch (dim) {
    case 1:
      return Point<dim>(x);
    case 2:
      return Point<dim>(x, y);
    case 3:
      return Point<dim>(x, y, z);
  }
}

template <int dim, typename Meas>
std::shared_ptr<Tuple<DiscretizedFunction<dim>>> WavePI<dim, Meas>::interpolate_field(
    std::shared_ptr<SpaceTimeMesh<dim>> target_mesh, std::shared_ptr<Tuple<DiscretizedFunction<dim>>> data) const {
  auto data_new = std::make_shared<Tuple<DiscretizedFunction<dim>>>();

  for (size_t i = 0; i < data->size(); i++)
    data_new->push_back(DiscretizedFunction<dim>(target_mesh, (*data)[i], (*data)[i].get_norm()));

  return data_new;
}

#define get_measure_meas(DIM)                                                                                \
  template <>                                                                                \
  std::shared_ptr<Measure<DiscretizedFunction<DIM>, SensorValues<DIM>>> WavePI<DIM, SensorValues<DIM>>::get_measure( \
      size_t config_idx, std::shared_ptr<SpaceTimeMesh<DIM>> mesh,                                                   \
      std::shared_ptr<Norm<DiscretizedFunction<DIM>>> norm) {                                                        \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM);                                                        \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM_DATA);                                                   \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM_DATA_I + Utilities::int_to_string(config_idx, 1));       \
    std::shared_ptr<SensorDistribution<DIM>> sensor_distribution;                                                    \
    if (cfg->sensor_distributions[config_idx] == SettingsManager::SensorDistribution::grid) {                        \
      auto my_distribution = std::make_shared<GridDistribution<DIM>>();                                              \
      my_distribution->get_parameters(*cfg->prm);                                                                    \
      sensor_distribution = my_distribution;                                                                         \
    } else if (cfg->sensor_distributions[config_idx] == SettingsManager::SensorDistribution::cube_boundary) {        \
      auto my_distribution = std::make_shared<CubeBoundaryDistribution<DIM>>();                                      \
      my_distribution->get_parameters(*cfg->prm);                                                                    \
      sensor_distribution = my_distribution;                                                                         \
    } else                                                                                \
      AssertThrow(false, ExcInternalError());                                                                        \
    std::shared_ptr<Measure<Param, SensorValues<DIM>>> measure;                                                      \
    if (cfg->measures[config_idx] == SettingsManager::Measure::convolution) {                                        \
      auto my_measure = std::make_shared<ConvolutionMeasure<DIM>>(mesh, sensor_distribution, norm);                  \
      my_measure->get_parameters(*cfg->prm);                                                                         \
      measure = my_measure;                                                                                \
    } else if (cfg->measures[config_idx] == SettingsManager::Measure::delta) {                                       \
      measure = std::make_shared<DeltaMeasure<DIM>>(mesh, sensor_distribution, norm);                                \
    } else                                                                                \
      AssertThrow(false, ExcInternalError());                                                                        \
    cfg->prm->leave_subsection();                                                                                \
    cfg->prm->leave_subsection();                                                                                \
    cfg->prm->leave_subsection();                                                                                \
    return measure;                                                                                \
  }

get_measure_meas(1)
    //
    get_measure_meas(2)
    //
    get_measure_meas(3)
//

#define get_measure_cont(D)                                                                                \
  template <>                                                                                \
  std::shared_ptr<Measure<DiscretizedFunction<D>, DiscretizedFunction<D>>>                                              \
  WavePI<D, DiscretizedFunction<D>>::get_measure(size_t config_idx, std::shared_ptr<SpaceTimeMesh<D>> mesh,             \
                                                 std::shared_ptr<Norm<DiscretizedFunction<D>>> norm) {                  \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM);                                                           \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM_DATA);                                                      \
    cfg->prm->enter_subsection(SettingsManager::KEY_PROBLEM_DATA_I + Utilities::int_to_string(config_idx, 1));          \
    std::shared_ptr<Measure<Param, DiscretizedFunction<D>>> measure;                                                    \
    if (cfg->measures[config_idx] == SettingsManager::Measure::field)                                                   \
      measure = std::make_shared<FieldMeasure<D>>(mesh, norm);                                                          \
    else if (cfg->measures[config_idx] == SettingsManager::Measure::masked_field) {                                     \
      auto mask      = std::make_shared<MacroFunctionParser<D>>(cfg->expr_masks[config_idx], cfg->constants_for_exprs); \
      auto mask_disc = std::make_shared<DiscretizedFunction<D>>(mesh, *mask, norm);                                     \
      measure        = std::make_shared<MaskedFieldMeasure<D>>(mesh, norm, mask_disc);                                  \
    } else                                                                                \
      AssertThrow(false, ExcInternalError());                                                                           \
    cfg->prm->leave_subsection();                                                                                \
    cfg->prm->leave_subsection();                                                                                \
    cfg->prm->leave_subsection();                                                                                \
    return measure;                                                                                \
  }

        get_measure_cont(1)
    //
    get_measure_cont(2)
    //
    get_measure_cont(3)
//

#define interpolate_data_cont(D)                                                                            \
  template <>                                                                                \
  void WavePI<D, DiscretizedFunction<D>>::interpolate_data(std::shared_ptr<SpaceTimeMesh<D>> target_mesh) { \
    this->data = interpolate_field(target_mesh, data);                                                      \
  }

        interpolate_data_cont(1)
    //
    interpolate_data_cont(2)
    //
    interpolate_data_cont(3)
//

// do nothing, just keep the sensor values
#define interpolate_data_meas(D)                                                                  \
  template <>                                                                                \
  void WavePI<D, SensorValues<D>>::interpolate_data(std::shared_ptr<SpaceTimeMesh<D>> target_mesh \
                                                    __attribute((unused))) {}

        interpolate_data_meas(1)
    //
    interpolate_data_meas(2)
    //
    interpolate_data_meas(3)
    //

    template <int dim, typename Meas>
    std::shared_ptr<SpaceTimeMesh<dim>> WavePI<dim, Meas>::initialize_mesh(size_t additional_refines,
                                                                           size_t additional_fe_degrees) const {
  LogStream::Prefix p("initialize_mesh");

  std::shared_ptr<Triangulation<dim>> triangulation = std::make_shared<Triangulation<dim>>();

  if (cfg->shape == SettingsManager::MeshShape::hyper_cube)
    GridGenerator::hyper_cube(*triangulation, cfg->shape_options["left"], cfg->shape_options["right"]);
  else if (cfg->shape == SettingsManager::MeshShape::hyper_l)
    GridGenerator::hyper_L(*triangulation, cfg->shape_options["left"], cfg->shape_options["right"]);
  else if (cfg->shape == SettingsManager::MeshShape::hyper_ball) {
    Point<dim> center =
        make_point(cfg->shape_options["center_x"], cfg->shape_options["center_y"], cfg->shape_options["center_z"]);

    GridGenerator::hyper_ball(*triangulation, center, cfg->shape_options["radius"]);
  } else if (cfg->shape == SettingsManager::MeshShape::cheese) {
    std::vector<unsigned int> holes({1, 1});

    if (dim == 3) holes.push_back(1);

    GridGenerator::cheese(*triangulation, holes);
    dealii::GridTools::scale(cfg->shape_options["scale"], *triangulation);
  } else
    AssertThrow(false, ExcInternalError());

  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(cfg->initial_refines + additional_refines);

  std::vector<double> times = cfg->times;

  for (size_t i = 0; i < additional_refines; i++) {
    std::vector<double> new_times;
    new_times.reserve(2 * times.size());

    for (size_t j = 0; j < times.size(); j++) {
      new_times.push_back(times[j]);
      if (j + 1 < times.size()) new_times.push_back(0.5 * (times[j] + times[j + 1]));
    }

    times = new_times;
  }

  std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(
      times, FE_Q<dim>(cfg->fe_degree + additional_fe_degrees), QGauss<dim>(cfg->quad_order), triangulation);

  // auto a_mesh = std::make_shared<AdaptiveMesh<dim>>(cfg->times, FE_Q<dim>(cfg->fe_degree),
  //      QGauss<dim>(cfg->quad_order), triangulation);
  //
  // mesh = a_mesh;

  //// TEST: flag some cells for refinement, and refine them in some step
  //{
  //   LogStream::Prefix pd("TEST");
  //   for (auto cell : triangulation->active_cell_iterators())
  //      if (cell->center()[1] > 0)
  //         cell->set_refine_flag();
  //
  //   std::vector<bool> ref;
  //   std::vector<bool> coa;
  //
  //   triangulation->save_refine_flags(ref);
  //   triangulation->save_coarsen_flags(coa);
  //
  //   std::vector<Patch> patches = a_mesh->get_forward_patches();
  //   patches[cfg->initial_time_steps / 2].emplace_back(ref, coa);
  //
  //   a_mesh->set_forward_patches(patches);
  //
  //   mesh->get_dof_handler(0);
  //}

  deallog << "Number of active cells: " << triangulation->n_active_cells() << std::endl;
  deallog << "Number of degrees of freedom in the first spatial mesh: " << mesh->get_dof_handler(0)->n_dofs()
          << std::endl;
  deallog << "cell diameters: minimal = " << dealii::GridTools::minimal_cell_diameter(*triangulation);
  deallog << ", maximal = " << dealii::GridTools::maximal_cell_diameter(*triangulation) << std::endl;
  deallog << "dt: " << cfg->dt / (1 << additional_refines) << std::endl;

  double mem = 0;
  for (size_t i = 0; i < mesh->length(); i++)
    mem += 8 * mesh->n_dofs(i);

  // (in case of !store_derivative, twice this amount otherwise)
  deallog << "expected size of a DiscretizedFunction<" << dim << ">: " << mem / (1024 * 1024) << " MiB" << std::endl;

  auto pattern = mesh->get_sparsity_pattern(0);
  deallog << "Sparsity pattern nnz=" << pattern->n_nonzero_elements() << "="
          << pattern->n_nonzero_elements() / (double)mesh->get_dof_handler(0)->n_dofs()
          << "*n_dofs, bandwidth=" << pattern->bandwidth() << ", max entries per row=" << pattern->max_entries_per_row()
          << std::endl;

  // TODO: also output sparsity pattern

  return mesh;
}

template <int dim, typename Meas>
void WavePI<dim, Meas>::initialize_problem() {
  LogStream::Prefix p("initialize_problem");

  // initialize measurement configurations
  measures.clear();
  pulses.clear();

  for (auto config_idx : cfg->configs)
    measures.push_back(get_measure(config_idx, mesh, norm_codomain));

  for (auto expr : cfg->exprs_rhs)
    pulses.push_back(std::make_shared<MacroFunctionParser<dim>>(expr, cfg->constants_for_exprs));

  AssertThrow(pulses.size() == measures.size(), ExcInternalError());

  wave_eq = std::make_shared<WaveEquation<dim>>(mesh);

  // (the exact parameter is inserted later again as a discretized variant)
  wave_eq->set_param_rho(param_rho, cfg->rho_dynamic);
  wave_eq->set_param_c(param_c);
  wave_eq->set_param_q(param_q);
  wave_eq->set_param_nu(param_nu);
  wave_eq->get_parameters(*cfg->prm);

  if (cfg->transform == SettingsManager::TransformType::identity)
    transform = std::make_shared<IdentityTransform<dim>>();
  else if (cfg->transform == SettingsManager::TransformType::log) {
    auto tmp = std::make_shared<LogTransform<dim>>();
    tmp->get_parameters(*cfg->prm);
    transform = tmp;
  } else if (cfg->transform == SettingsManager::TransformType::artanh) {
    auto tmp = std::make_shared<ArtanhTransform<dim>>();
    tmp->get_parameters(*cfg->prm);
    transform = tmp;
  } else
    AssertThrow(false, ExcInternalError());

  deallog << "discretizing background parameter" << std::endl;

  // WORKAROUND: force initialization of muparser object
  // (discretizing directly breaks stuff [segfault, bad allocs], maybe due to threading.)
  param_background->evaluate(Point<dim>::unit_vector(1), 0.0);
  auto param_background_discretized = std::make_shared<Param>(mesh, *param_background, norm_domain);

  deallog << "constructing problem" << std::endl;

  switch (cfg->problem_type) {
    case SettingsManager::ProblemType::q:
      /* Reconstruct q */
      param_exact = wave_eq->get_param_q();
      problem =
          std::make_shared<QProblem<dim, Meas>>(*wave_eq, pulses, measures, transform, param_background_discretized);
      break;
    case SettingsManager::ProblemType::c:
      /* Reconstruct c */
      param_exact = wave_eq->get_param_c();
      problem =
          std::make_shared<CProblem<dim, Meas>>(*wave_eq, pulses, measures, transform, param_background_discretized);
      break;
    case SettingsManager::ProblemType::nu:
      /* Reconstruct nu */
      param_exact = wave_eq->get_param_nu();
      problem =
          std::make_shared<NuProblem<dim, Meas>>(*wave_eq, pulses, measures, transform, param_background_discretized);
      break;
    case SettingsManager::ProblemType::rho:
      /* Reconstruct rho */
      param_exact = wave_eq->get_param_rho();
      problem =
          std::make_shared<RhoProblem<dim, Meas>>(*wave_eq, pulses, measures, transform, param_background_discretized);
      break;
    case SettingsManager::ProblemType::rho_constant:
      /* Reconstruct rho */
      param_exact = wave_eq->get_param_rho();
      problem     = std::make_shared<ConstantRhoProblem<dim, Meas>>(*wave_eq, pulses, measures, transform,
                                                                param_background_discretized);
      break;
    case SettingsManager::ProblemType::c_constant:
      /* Reconstruct rho */
      param_exact = wave_eq->get_param_c();
      problem     = std::make_shared<ConstantCProblem<dim, Meas>>(*wave_eq, pulses, measures, transform,
                                                                param_background_discretized);
      break;
    default:
      AssertThrow(false, ExcInternalError())
  }

  // transform param_exact
  param_exact_untransformed = param_exact;
  param_exact               = transform->transform(param_exact);

  problem->set_norm_domain(norm_domain);
  problem->set_norm_codomain(norm_codomain);
}

template <int dim, typename Meas>
void WavePI<dim, Meas>::synthesize_data() {
  LogStream::Prefix p("generate_data");
  LogStream::Prefix pp("run");  // make logs of forward operator appear in the right level

  deallog << "Discretizing exact parameter" << std::endl;
  param_exact_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, *param_exact);
  param_exact_disc->set_norm(norm_domain);

  deallog << "Computing exact data" << std::endl;
  Tuple<Meas> data_exact = problem->forward(*param_exact_disc);

  double data_exact_norm = data_exact.norm();

  // in itself not wrong, but makes relative errors and noise levels meaningless.
  AssertThrow(data_exact_norm > 0, ExcMessage("Exact Data is zero"));

  data = std::make_shared<Tuple<Meas>>(data_exact);
  data->add(1.0, Tuple<Meas>::noise(data_exact, cfg->epsilon * data_exact_norm));

#ifdef WAVEPI_MPI
  if (Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) > 1) {
    size_t mpi_rank = Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);

    if (mpi_rank == 0)
      deallog << "Distributing noisy data to other nodes" << std::endl;
    else
      deallog << "Rank " << mpi_rank << " waiting for data from root node" << std::endl;

    for (size_t i = 0; i < data->size(); i++)
      (*data)[i].mpi_bcast(0);

    deallog << "Distribution of data complete." << std::endl;
  }
#endif
}

template <int dim, typename Meas>
void WavePI<dim, Meas>::log_error(DiscretizedFunction<dim>& reconstruction,
                                  std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm,
                                  DiscretizedFunction<dim>& exact) {
  reconstruction.set_norm(norm);

  double norm_exact = 0.0;
  double err        = reconstruction.absolute_error(exact, &norm_exact);

  if (norm_exact > 1e-16)
    deallog << "Relative " << norm->name() << " error of the reconstruction: " << err / norm_exact << std::endl;
  else
    deallog << "Absolute " << norm->name() << " error of the reconstruction: " << err << std::endl;

  reconstruction.set_norm(norm_domain);
}

template <int dim, typename Meas>
void WavePI<dim, Meas>::log_error_initial(DiscretizedFunction<dim>& reconstruction_minus_initial,
                                          std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm,
                                          DiscretizedFunction<dim>& exact_minus_initial) {
  reconstruction_minus_initial.set_norm(norm);

  double norm_exact = 0.0;
  double err        = reconstruction_minus_initial.absolute_error(exact_minus_initial, &norm_exact);

  if (norm_exact > 1e-16)
    deallog << "Relative " << norm->name() << " error of (reconstruction - initial guess): " << err / norm_exact
            << std::endl;
  else
    deallog << "Absolute " << norm->name() << " error of (reconstruction - initial guess): " << err << std::endl;

  reconstruction_minus_initial.set_norm(norm_domain);
}

template <int dim, typename Meas>
void WavePI<dim, Meas>::run() {
  Timer timer_data, timer_mesh_problem;

  if (cfg->synthesis_additional_refines > 0 || cfg->synthesis_additional_fe_degrees > 0) {
    timer_data.start();
    deallog << "Generating mesh for data synthesis" << std::endl;
    this->mesh = initialize_mesh(cfg->synthesis_additional_refines, cfg->synthesis_additional_fe_degrees);

    deallog << "Initializing problem for data synthesis" << std::endl;
    initialize_problem();

    deallog << "Generating synthetic data on fine mesh" << std::endl;
    synthesize_data();
    timer_data.stop();

    timer_mesh_problem.start();
    deallog << "Generating mesh" << std::endl;
    auto inversion_mesh = initialize_mesh();
    timer_mesh_problem.stop();

    timer_data.start();
    deallog << "Interpolating data from fine mesh" << std::endl;
    interpolate_data(inversion_mesh);
    timer_data.stop();

    timer_mesh_problem.start();
    deallog << "Initializing problem" << std::endl;
    this->mesh = inversion_mesh;
    initialize_problem();
    timer_mesh_problem.stop();
  } else {
    timer_mesh_problem.start();
    this->mesh = initialize_mesh();

    deallog << "Initializing problem" << std::endl;
    initialize_problem();
    timer_mesh_problem.stop();

    deallog << "Generating synthetic data (inverse crime)" << std::endl;
    timer_data.start();
    synthesize_data();
    timer_data.stop();
  }

  // we do not want the data synthesis to be a part of the stats
  problem->reset_statistics();

  deallog << "wall time for data synthesis        : " << Util::format_duration(timer_data.wall_time()) << std::endl;
  deallog << "wall time for mesh and problem init : " << Util::format_duration(timer_mesh_problem.wall_time())
          << std::endl;

  Timer timer_inversion;
  timer_inversion.start();

  std::shared_ptr<Regularization<Param, Tuple<Meas>, Exact>> regularization;

  deallog.push("Initial Guess");
  auto initial_guess_transformed = transform->transform(initial_guess);
  auto initial_guess_discretized = std::make_shared<Param>(mesh, *initial_guess_transformed);

  // make sure that the initial guess has the right norm
  initial_guess_discretized->set_norm(norm_domain);

  deallog.pop();

  cfg->prm->enter_subsection(SettingsManager::KEY_INVERSION);
  switch (cfg->method) {
    case SettingsManager::NonlinearMethod::reginn:
      regularization =
          std::make_shared<REGINN<Param, Tuple<Meas>, Exact>>(problem, initial_guess_discretized, *cfg->prm);
      break;
    case SettingsManager::NonlinearMethod::nonlinear_landweber:
      regularization = std::make_shared<NonlinearLandweber<Param, Tuple<Meas>, Exact>>(
          problem, initial_guess_discretized, *cfg->prm);
      break;
    default:
      AssertThrow(false, ExcInternalError());
  }
  cfg->prm->leave_subsection();

#ifdef WAVEPI_MPI
  size_t mpi_rank = Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
#else
  size_t mpi_rank = 0;
#endif

  if (mpi_rank == 0) {
    regularization->add_listener(std::make_shared<OutputProgressListener<dim, Tuple<Meas>>>(*cfg->prm, transform));
    regularization->add_listener(std::make_shared<StatOutputProgressListener<Param, Tuple<Meas>, Exact>>(*cfg->prm));
  }

  regularization->add_listener(std::make_shared<GenericInversionProgressListener<Param, Tuple<Meas>, Exact>>("i"));
  regularization->add_listener(std::make_shared<CtrlCProgressListener<Param, Tuple<Meas>, Exact>>());
  regularization->add_listener(std::make_shared<BoundCheckProgressListener<dim, Tuple<Meas>, Exact>>(*cfg->prm));
  regularization->add_listener(std::make_shared<WatchdogProgressListener<Param, Tuple<Meas>, Exact>>(*cfg->prm));

  regularization->add_post_processor(std::make_shared<BoundEnforcingPostProcessor<dim>>(*cfg->prm));

  cfg->log_parameters();

#ifdef WAVEPI_MPI
  if (Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) > pulses.size())
    deallog << "WARNING: More MPI processes than subproblems" << std::endl;
  else if (pulses.size() % Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) != 0)
    deallog << "WARNING: n_mpi_processes % pulses.size() != 0" << std::endl;
#endif

  // do the inversion
  auto reconstruction = regularization->invert(*data, cfg->tau * cfg->epsilon * data->norm(), param_exact_disc);

  // transform back and output errors in the untransformed setting
  reconstruction = transform->transform_inverse(reconstruction);

  Param param_exact_untrans_disc(mesh, *param_exact_untransformed);
  param_exact_untrans_disc.set_norm(reconstruction.get_norm());

  log_error(reconstruction, norm_domain, param_exact_untrans_disc);

  deallog << "reconstruction error with respect to other norms: " << std::endl;

  log_error(reconstruction, norm_vector, param_exact_untrans_disc);
  log_error(reconstruction, norm_l2l2, param_exact_untrans_disc);
  log_error(reconstruction, norm_h1l2, param_exact_untrans_disc);
  log_error(reconstruction, norm_h1h1, param_exact_untrans_disc);
  log_error(reconstruction, norm_h2l2, param_exact_untrans_disc);

  // same for reconstruction - initial guess
  Param initial_guess_untrans_disc(mesh, *initial_guess);
  initial_guess_untrans_disc.set_norm(reconstruction.get_norm());

  reconstruction -= initial_guess_untrans_disc;
  param_exact_untrans_disc -= initial_guess_untrans_disc;

  log_error_initial(reconstruction, norm_domain, param_exact_untrans_disc);
  log_error_initial(reconstruction, norm_vector, param_exact_untrans_disc);
  log_error_initial(reconstruction, norm_l2l2, param_exact_untrans_disc);
  log_error_initial(reconstruction, norm_h1l2, param_exact_untrans_disc);
  log_error_initial(reconstruction, norm_h1h1, param_exact_untrans_disc);
  log_error_initial(reconstruction, norm_h2l2, param_exact_untrans_disc);

  timer_inversion.stop();

  if (problem->get_statistics()) {
    auto stats = problem->get_statistics();
    deallog << "Statistics: " << std::endl;

    deallog << "forward         : " << stats->calls_forward << " calls, avg "
            << Util::format_duration(stats->time_forward / stats->calls_forward) << " per call, " << std::fixed
            << std::setprecision(2) << (stats->time_forward / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;

    deallog << " lin forward     : " << stats->calls_linearization_forward << " calls, avg "
            << Util::format_duration(stats->time_linearization_forward / stats->calls_linearization_forward)
            << " per call, " << std::fixed << std::setprecision(2)
            << (stats->time_linearization_forward / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;

    deallog << " lin adjoint     : " << stats->calls_linearization_adjoint << " calls, avg "
            << Util::format_duration(stats->time_linearization_adjoint / stats->calls_linearization_adjoint)
            << " per call, " << std::fixed << std::setprecision(2)
            << (stats->time_linearization_adjoint / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;

    deallog << " measure forward : " << stats->calls_measure_forward << " calls, avg "
            << Util::format_duration(stats->time_measure_forward / stats->calls_measure_forward) << " per call, "
            << std::fixed << std::setprecision(2) << (stats->time_measure_forward / timer_inversion.wall_time() * 100)
            << "% of total time" << std::endl;

    deallog << " measure adjoint : " << stats->calls_measure_adjoint << " calls, avg "
            << Util::format_duration(stats->time_measure_adjoint / stats->calls_measure_adjoint) << " per call, "
            << std::fixed << std::setprecision(2) << (stats->time_measure_adjoint / timer_inversion.wall_time() * 100)
            << "% of total time" << std::endl;

    deallog << " duality         : " << stats->calls_duality << " calls, avg "
            << Util::format_duration(stats->time_duality / stats->calls_duality) << " per call, " << std::fixed
            << std::setprecision(2) << (stats->time_duality / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;

    deallog << " IO              : " << Util::format_duration(stats->time_io) << " ≈ " << std::fixed
            << std::setprecision(2) << (stats->time_io / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;

    deallog << " post-processing : " << Util::format_duration(stats->time_postprocessing) << " ≈ " << std::fixed
            << std::setprecision(2) << (stats->time_postprocessing / timer_inversion.wall_time() * 100)
            << "% of total time" << std::endl;

#ifdef WAVEPI_MPI
    if (Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) > 1) {
      deallog << " Additional time needed for MPI communication:" << std::endl;
      deallog << "  forward         : average "
              << Util::format_duration(stats->time_forward_communication / stats->calls_forward) << " per call, "
              << std::fixed << std::setprecision(2)
              << (stats->time_forward_communication / timer_inversion.wall_time() * 100) << "% of total time"
              << std::endl;
      deallog << "  lin forward     : avg "
              << Util::format_duration(stats->time_linearization_forward_communication /
                                       stats->calls_linearization_forward)
              << " per call, " << std::fixed << std::setprecision(2)
              << (stats->time_linearization_forward_communication / timer_inversion.wall_time() * 100)
              << "% of total time" << std::endl;
      deallog << "  lin adjoint     : avg "
              << Util::format_duration(stats->time_linearization_adjoint_communication /
                                       stats->calls_linearization_adjoint)
              << " per call, " << std::fixed << std::setprecision(2)
              << (stats->time_linearization_adjoint_communication / timer_inversion.wall_time() * 100)
              << "% of total time" << std::endl;
    }
#endif

    double unaccounted_time = timer_inversion.wall_time();
    unaccounted_time -= stats->time_duality;
    unaccounted_time -= stats->time_forward;
    unaccounted_time -= stats->time_linearization_forward;
    unaccounted_time -= stats->time_linearization_adjoint;
    unaccounted_time -= stats->time_measure_forward;
    unaccounted_time -= stats->time_measure_adjoint;
    unaccounted_time -= stats->time_io;
    unaccounted_time -= stats->time_postprocessing;

#ifdef WAVEPI_MPI
    if (Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) > 1) {
      // (might not be zero if timers have never been started)

      unaccounted_time -= stats->time_forward_communication;
      unaccounted_time -= stats->time_linearization_forward_communication;
      unaccounted_time -= stats->time_linearization_adjoint_communication;
    }
#endif

    // most likely spent doing vector operations, norm evalutations, etc. for the inversion methods
    // and some of it for setup (interpolation, mesh generation, ...)
    deallog << "time not accounted for: " << Util::format_duration(unaccounted_time) << " ≈ " << std::fixed
            << std::setprecision(2) << (unaccounted_time / timer_inversion.wall_time() * 100) << "% of total time"
            << std::endl;
    deallog << "total wall time for the inversion : " << Util::format_duration(timer_inversion.wall_time())
            << std::endl;
    deallog << "total cpu time for the inversion : " << Util::format_duration(timer_inversion.cpu_time()) << std::endl;
  }
}

#ifdef WAVEPI_1D
template class WavePI<1, DiscretizedFunction<1>>;
template class WavePI<1, SensorValues<1>>;
#endif

#ifdef WAVEPI_2D
template class WavePI<2, DiscretizedFunction<2>>;
template class WavePI<2, SensorValues<2>>;
#endif

#ifdef WAVEPI_3D
template class WavePI<3, DiscretizedFunction<3>>;
template class WavePI<3, SensorValues<3>>;
#endif

}  // namespace wavepi
/* namespace wavepi */


/*    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.
 *
 *    References
 *      Easy calculation. Newton's Law of Gravity Tutorial,
 *          http://easycalculation.com/physics/classical-physics/learn-newtons-law.php, last
 *          accessed: 12th February, 2012.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

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

#include <Eigen/Core>

#include "Tudat/SimulationSetup/tudatSimulationHeader.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_gravitational_torque )

//! Test to check degree two gravitational torque
BOOST_AUTO_TEST_CASE( testDegreeTwoGravitationalTorque )
{
    using namespace tudat::simulation_setup;
    using namespace tudat::basic_astrodynamics;
    using namespace tudat::gravitation;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation time settings.
    const double simulationStartEpoch = 0.0;
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Define body settings for simulation.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Moon" );

    //! Test torque for various inertia tensors
    for( unsigned int testCase = 0; testCase < 7; testCase++ )
    {

        // Create body objects.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( bodiesToCreate );

        // Define degree two coefficients
        Eigen::Matrix3d cosineCoefficients = Eigen::Matrix3d::Zero( );
        Eigen::Matrix3d sineCoefficients = Eigen::Matrix3d::Zero( );
        Eigen::Matrix3d inertiaTensorDeviation = Eigen::Matrix3d::Zero( );
        if( testCase > 0 )
        {
            cosineCoefficients( 0, 0 ) = 1.0;

            if( testCase == 2 )
            {
                cosineCoefficients( 2, 0 ) = 0.01;
            }
            else if( testCase == 3 )
            {
                cosineCoefficients( 2, 2 ) = 0.01;
            }
            else if( testCase == 4 )
            {
                sineCoefficients( 2, 2 ) = 0.01;
            }
            else if( testCase == 5 )
            {
                cosineCoefficients( 2, 1 ) = 0.01;
            }
            else if( testCase == 6 )
            {
                sineCoefficients( 2, 1 ) = 0.01;
            }

            bodySettings[ "Moon" ]->gravityFieldSettings = std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                        spice_interface::getBodyGravitationalParameter( "Moon" ), spice_interface::getAverageRadius( "Moon" ),
                        cosineCoefficients, sineCoefficients, "IAU_Moon" );
        }

        if( testCase > 0 )
        {
            double c20InertiaContribution  = cosineCoefficients( 2, 0 ) *
                    tudat::basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 0 ) / 3.0;
            inertiaTensorDeviation( 0, 0 ) += c20InertiaContribution;
            inertiaTensorDeviation( 1, 1 ) += c20InertiaContribution;
            inertiaTensorDeviation( 2, 2 ) -= 2.0 * c20InertiaContribution;

            double c21InertiaContribution  = cosineCoefficients( 2, 1 ) *
                    tudat::basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 1 );
            inertiaTensorDeviation( 0, 2 ) -= c21InertiaContribution;
            inertiaTensorDeviation( 2, 0 ) -= c21InertiaContribution;

            double c22InertiaContribution  = 2.0 * cosineCoefficients( 2, 2 ) *
                    tudat::basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 2 );
            inertiaTensorDeviation( 0, 0 ) -= c22InertiaContribution;
            inertiaTensorDeviation( 1, 1 ) += c22InertiaContribution;

            double s21InertiaContribution  = sineCoefficients( 2, 1 ) *
                    tudat::basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 1 );
            inertiaTensorDeviation( 1, 2 ) -= s21InertiaContribution;
            inertiaTensorDeviation( 2, 1 ) -= s21InertiaContribution;

            double s22InertiaContribution  = 2.0 * sineCoefficients( 2, 2 ) *
                    tudat::basic_mathematics::calculateLegendreGeodesyNormalizationFactor( 2, 2 );
            inertiaTensorDeviation( 0, 1 ) -= s22InertiaContribution;
            inertiaTensorDeviation( 1, 0 ) -= s22InertiaContribution;

            inertiaTensorDeviation *= spice_interface::getAverageRadius( "Moon" ) * spice_interface::getAverageRadius( "Moon" ) *
                    spice_interface::getBodyGravitationalParameter( "Moon" ) / physical_constants::GRAVITATIONAL_CONSTANT;
        }

        // Create bodies
        NamedBodyMap bodyMap = createBodies( bodySettings );
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Crate torque model
        SelectedTorqueMap selectedTorqueModelMap;
        selectedTorqueModelMap[ "Moon" ][ "Earth" ].push_back(
                    std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );
        basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                    bodyMap, selectedTorqueModelMap, { "Moon" } );
        std::shared_ptr< TorqueModel > secondDegreeGravitationalTorque =
                torqueModelMap.at( "Moon" ).at( "Earth" ).at( 0 );

        // Update environment to current time
        double evaluationTime = tudat::physical_constants::JULIAN_DAY / 2.0;

        bodyMap.at( "Moon" )->setStateFromEphemeris( evaluationTime );
        bodyMap.at( "Moon" )->setCurrentRotationalStateToLocalFrameFromEphemeris( evaluationTime );
        bodyMap.at( "Moon" )->setBodyInertiaTensorFromGravityField( 0.4 );

        bodyMap.at( "Earth" )->setStateFromEphemeris( evaluationTime );
        bodyMap.at( "Earth" )->setCurrentRotationalStateToLocalFrameFromEphemeris( evaluationTime );
        bodyMap.at( "Earth" )->setBodyInertiaTensorFromGravityField( 0.4 );

        if( testCase == 0 )
        {
            // Test id gravity field coefficients are correctly reconstructed
            inertiaTensorDeviation = bodyMap.at( "Moon" )->getBodyInertiaTensor( );
            std::shared_ptr< SphericalHarmonicsGravityField > moonGravityField =
                    std::dynamic_pointer_cast< SphericalHarmonicsGravityField >(
                        bodyMap.at( "Moon" )->getGravityFieldModel( ) );

            double recomputedC20 = std::sqrt( 0.2 ) /
                    ( moonGravityField->getGravitationalParameter( ) * moonGravityField->getReferenceRadius( ) *
                      moonGravityField->getReferenceRadius( ) / tudat::physical_constants::GRAVITATIONAL_CONSTANT ) * (
                        0.5 * ( inertiaTensorDeviation( 0, 0 ) + inertiaTensorDeviation( 1, 1 ) ) -
                        inertiaTensorDeviation( 2, 2 ) );
            BOOST_CHECK_CLOSE_FRACTION( recomputedC20, moonGravityField->getCosineCoefficients( )( 2, 0 ), 1.0E-12 );

            double recomputedC21 = -std::sqrt( 3.0 / 5.0 ) /
                    ( moonGravityField->getGravitationalParameter( ) * moonGravityField->getReferenceRadius( ) *
                      moonGravityField->getReferenceRadius( ) / tudat::physical_constants::GRAVITATIONAL_CONSTANT ) * (
                        inertiaTensorDeviation( 0, 2 ) );
            BOOST_CHECK_CLOSE_FRACTION( recomputedC21, moonGravityField->getCosineCoefficients( )( 2, 1 ), 1.0E-12 );

            double recomputedC22 = std::sqrt( 3.0 / 20.0 ) /
                    ( moonGravityField->getGravitationalParameter( ) * moonGravityField->getReferenceRadius( ) *
                      moonGravityField->getReferenceRadius( ) / tudat::physical_constants::GRAVITATIONAL_CONSTANT ) * (
                        inertiaTensorDeviation( 1, 1 ) - inertiaTensorDeviation( 0, 0 ) );
            BOOST_CHECK_CLOSE_FRACTION( recomputedC22, moonGravityField->getCosineCoefficients( )( 2, 2 ), 1.0E-12 );

            double recomputedS21 = -std::sqrt( 3.0 / 5.0 ) /
                    ( moonGravityField->getGravitationalParameter( ) * moonGravityField->getReferenceRadius( ) *
                      moonGravityField->getReferenceRadius( ) / tudat::physical_constants::GRAVITATIONAL_CONSTANT ) * (
                        inertiaTensorDeviation( 1, 2 ) );
            BOOST_CHECK_CLOSE_FRACTION( recomputedS21, moonGravityField->getSineCoefficients( )( 2, 1 ), 1.0E-12 );

            double recomputedS22 = -std::sqrt( 12.0 / 5.0 ) /
                    ( 2.0 * moonGravityField->getGravitationalParameter( ) * moonGravityField->getReferenceRadius( ) *
                      moonGravityField->getReferenceRadius( ) / tudat::physical_constants::GRAVITATIONAL_CONSTANT ) * (
                        inertiaTensorDeviation( 0, 1 ) );
            BOOST_CHECK_CLOSE_FRACTION( recomputedS22, moonGravityField->getSineCoefficients( )( 2, 2 ), 1.0E-12 );
        }

        // Update torque model to current time.
        secondDegreeGravitationalTorque->updateMembers( evaluationTime );

        // Compute torque manually, and test against computed result
        Eigen::Vector3d earthRelativePosition =
                bodyMap.at( "Moon" )->getCurrentRotationToLocalFrame( ) *
                ( bodyMap.at( "Earth" )->getPosition( ) - bodyMap.at( "Moon" )->getPosition( ) );
        Eigen::Vector3d manualTorque = 3.0 * earthRelativePosition.cross( inertiaTensorDeviation * earthRelativePosition ) *
                bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) /
                std::pow( earthRelativePosition.norm( ), 5.0 );

        Eigen::Vector3d currentTorque = secondDegreeGravitationalTorque->getTorque( );
        Eigen::Vector3d torqueError = ( currentTorque - manualTorque );

        std::cout<<"Current torques "<<currentTorque.transpose( )<<std::endl<<
                   torqueError<<std::endl;

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( torqueError( i ) ), 1.0E-14 * currentTorque.norm( ) );
        }
    }
}

//! Test to check spherical harmonic torque.
BOOST_AUTO_TEST_CASE( testSphericalGravitationalTorque )
{
    using namespace tudat::simulation_setup;
    using namespace tudat::basic_astrodynamics;
    using namespace tudat::gravitation;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation time settings.
    const double simulationStartEpoch = 0.0;
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    //! Test for zero and non-zero spherical harmonic coefficients
    for( unsigned int testCase = 0; testCase < 2; testCase++ )
    {
        // Define body settings for simulation.
        std::vector< std::string > bodiesToCreate;
        bodiesToCreate.push_back( "Earth" );
        bodiesToCreate.push_back( "Moon" );

        // Create body objects.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( bodiesToCreate );

        // Set effective point-mass gravity
        if( testCase == 0 )
        {
            Eigen::Matrix3d cosineCoefficients = Eigen::Matrix3d::Zero( );
            Eigen::Matrix3d sineCoefficients = Eigen::Matrix3d::Zero( );
            cosineCoefficients( 0, 0 ) = 1.0;

            bodySettings[ "Moon" ]->gravityFieldSettings = std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                        spice_interface::getBodyGravitationalParameter( "Moon" ), spice_interface::getAverageRadius( "Moon" ),
                        cosineCoefficients, sineCoefficients, "IAU_Moon" );
        }

        NamedBodyMap bodyMap = createBodies( bodySettings );
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Create two torque models
        SelectedTorqueMap selectedTorqueModelMap;
        selectedTorqueModelMap[ "Moon" ][ "Earth" ].push_back(
                    std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );
        selectedTorqueModelMap[ "Moon" ][ "Earth" ].push_back(
                    std::make_shared< SphericalHarmonicTorqueSettings >( 2, 2 ) );
        basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                    bodyMap, selectedTorqueModelMap, { "Moon" } );
        std::shared_ptr< TorqueModel > secondDegreeGravitationalTorque =
                torqueModelMap.at( "Moon" ).at( "Earth" ).at( 0 );
        std::shared_ptr< TorqueModel > sphercialHarmonicGravitationalTorque =
                torqueModelMap.at( "Moon" ).at( "Earth" ).at( 1 );

        // Update Moon to current time.
        double evaluationTime = tudat::physical_constants::JULIAN_DAY / 2.0;
        bodyMap.at( "Moon" )->setStateFromEphemeris( evaluationTime );
        bodyMap.at( "Moon" )->setCurrentRotationalStateToLocalFrameFromEphemeris( evaluationTime );
        bodyMap.at( "Moon" )->setBodyInertiaTensorFromGravityField( 0.0 );

        {
            // Test reconstructed spherical harmonic coefficients
            Eigen::Matrix3d moonCosineCoefficients =
                    std::dynamic_pointer_cast< tudat::gravitation::SphericalHarmonicsGravityField >(
                        bodyMap.at( "Moon" )->getGravityFieldModel( ) )->getCosineCoefficients( ).block( 0, 0, 3, 3 );
            Eigen::Matrix3d moonSineCoefficients =
                    std::dynamic_pointer_cast< tudat::gravitation::SphericalHarmonicsGravityField >(
                        bodyMap.at( "Moon" )->getGravityFieldModel( ) )->getSineCoefficients( ).block( 0, 0, 3, 3 );
            Eigen::MatrixXd moonReconstructedCosineCoefficients = Eigen::Matrix3d::Zero( ),
                    moonReconstructedSineCoefficients = Eigen::Matrix3d::Zero( );

            double reconstructedScaledMeanMomentOfInertia;
            gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                        bodyMap.at( "Moon" )->getBodyInertiaTensor( ),
                        bodyMap.at( "Moon" )->getGravityFieldModel( )->getGravitationalParameter( ),
                        std::dynamic_pointer_cast< SphericalHarmonicsGravityField >(
                            bodyMap.at( "Moon" )->getGravityFieldModel( ) )->getReferenceRadius( ), true,
                        moonReconstructedCosineCoefficients, moonReconstructedSineCoefficients,
                        reconstructedScaledMeanMomentOfInertia );

            for( unsigned int j = 0; j < 3; j++ )
            {
                for( unsigned int k = 0; k <= j; k++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( moonReconstructedCosineCoefficients( j, k ) -
                                                  moonCosineCoefficients( j, k ) ), 1.0E-20 );

                    BOOST_CHECK_SMALL( std::fabs( moonReconstructedSineCoefficients( j, k ) -
                                                  moonSineCoefficients( j, k ) ), 1.0E-20 );
                }
            }
        }

        // Update Earth to current time.
        bodyMap.at( "Earth" )->setStateFromEphemeris( evaluationTime );
        bodyMap.at( "Earth" )->setCurrentRotationalStateToLocalFrameFromEphemeris( evaluationTime );
        bodyMap.at( "Earth" )->setBodyInertiaTensorFromGravityField( 0.0 );

        // Update and compute torque values
        secondDegreeGravitationalTorque->updateMembers( evaluationTime );
        Eigen::Vector3d currentExplicitTorque = secondDegreeGravitationalTorque->getTorque( );
        sphercialHarmonicGravitationalTorque->updateMembers( evaluationTime );
        Eigen::Vector3d currentSphericalHarmonicTorque = sphercialHarmonicGravitationalTorque->getTorque( );

        // Test difference between models (should be equivalent)
        Eigen::Vector3d torqueError = currentExplicitTorque - currentSphericalHarmonicTorque;
        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( torqueError( i ) ), 1.0E-14 * currentExplicitTorque.norm( ) );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#pragma once
#include <Eigen/Geometry>
#include "../../vector_cast.hh"
#include "../DerivedPtrHolder.hh"

namespace kt84 {

template <class TMeshBase, class TMesh>
struct BoundingBox : public DerivedPtrHolder<TMesh, BoundingBox<TMeshBase, TMesh>> {
    static const int N = TMeshBase::Point::size_;
    Eigen::AlignedBox<double, N> boundingBox;

    void boundingBox_compute() {
        TMesh* mesh = DerivedPtrHolder<TMesh, BoundingBox<TMeshBase, TMesh>>::derived_ptr;
        boundingBox.setEmpty();
        for (auto v : mesh->vertices())
            boundingBox.extend(vector_cast<N, Eigen::Matrix<double, N, 1>>(mesh->point(v)));
    }
    double boundingBox_diagonal_norm() const {
        return boundingBox.diagonal().norm();
    }
};

}


#ifndef BITREP_MPI
# error "This program requires the flag BITREP_MPI"
#endif

#include "mpi.h"
#include "reduce.h"
#include "convreduce.h"
#include <cstdio>
#include <vector>
#include <boost/random.hpp>

using namespace std;

vector<double> input;

template<int k>
inline double doReduce(int n, int N, const double* v, MPI_Comm comm, double& tComp, double& tComm)
{
    if (k > 0)
        return bitrep::singlesweep_timing<k>(n, N, input.data(), comm, tComp, tComm);
    else if (k < 0)
        return bitrep::doublesweep_timing<-k>(n, N, input.data(), comm, tComp, tComm);
    else
        return bitrep::convreduce_timing(n, N, input.data(), comm, tComp, tComm);
}

void generateVector(int n)
{
    boost::random::mt19937 rng;
    boost::random::uniform_real_distribution<double> distr(1., 2.);

    input.resize(n);

    for (int i = 0; i < n; ++i)
        input[i] = distr(rng);
}

template<int k>
void test(int n, int N, MPI_Comm comm, bool verbose, int repeat)
{
    double tComp, tComm, TComp = 0., TComm = 0.;
    double sComp = 0., sComm = 0.;
    doReduce<k>(n, N, input.data(), comm, tComp, tComm);

    for (int r = 0; r < repeat; ++r) {
        doReduce<k>(n, N, input.data(), comm, tComp, tComm);
        TComp += tComp;
        TComm += tComm;
        sComp += tComp*tComp;
        sComm += tComm*tComm;
    }

    TComp /= repeat;
    TComm /= repeat;
    sComp /= repeat;
    sComm /= repeat;

    sComp = sqrt(sComp - TComp*TComp);
    sComm = sqrt(sComm - TComm*TComm);

    if (verbose)
        printf("%8d | %9.5f (%09.5f) | %9.5f (%09.5f)\n", n, TComp*1000., sComp*1000., TComm*1000., sComm*1000.);
}

int main(int argc, char **argv)
{
    MPI_Init(&argc, &argv);

    int nproc, pid;
    MPI_Comm_size(MPI_COMM_WORLD, &nproc);
    MPI_Comm_rank(MPI_COMM_WORLD, &pid);
    const bool root = pid == 0;

    int nproc2 = 1;
    for(int p = 1; 2*p <= nproc; p *= 2)
        nproc2 *= 2;

    const int nmax = 1 << 25;
    generateVector(nmax);

    // Choose test
    int testfuncid = 1;
    if (argc > 1)
        testfuncid = atoi(argv[1]);

    typedef void (*testfunc_t)(int, int, MPI_Comm, bool, int);
    testfunc_t testfuncs[] = {
        test<-4>,
        test<-3>,
        test<-2>,
        test<-1>,
        test< 0>,
        test< 1>,
        test< 2>,
        test< 3>,
        test< 4>
    };
    testfunc_t testfunc = testfuncs[testfuncid+4];
    const int k = testfuncid;
    if (root) {
        cout << nproc << " participating processes\n";

        if (k > 0)
            cout << "Testing single-sweep with " << k << " levels\n" << endl;
        else if (k == 0)
            cout << "Testing conventional sum\n" << endl;
        else if (k < 0)
            cout << "Testing double-sweep with " << -k << " levels\n" << endl;

        cout << "Local n  |    Computation time   |   Communication time\n";
        cout << "---------+-----------------------+----------------------\n";
    }

    for (int n = 1; n <= nmax; n *= 2) {
        int N = n * nproc;
        testfunc(n, N, MPI_COMM_WORLD, root, 10);
    }

    if (root)
    {
        cout << "--------------------------------------------------------\n\n\n";
    }

    MPI_Finalize();
}