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

#ifndef SMOOTH__SO2_HPP_
#define SMOOTH__SO2_HPP_

#include <Eigen/Core>

#include <complex>

#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "internal/so2.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SO3;
// \endcond

/**
 * @brief Base class for SO2 Lie group types.
 *
 * Internally represented as \f$\mathbb{U}(1)\f$ (complex numbers).
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [q_z, q_w] \f$
 * - Tangent:  \f$ \mathbf{a} = [\omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$q_z^2 + q_w^2 = 1 \f$
 * - Tangent: \f$ -\pi < \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * \f[
 * \mathbf{X} =
 * \begin{bmatrix}
 *  q_w & -q_z \\
 *  q_z &  q_w
 * \end{bmatrix} \in \mathbb{R}^{2 \times 2}
 * \f]
 *
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   0 & -\omega_z \\
 *  \omega_z &   0 \\
 * \end{bmatrix} \in \mathbb{R}^{2 \times 2}
 * \f]
 */
template<typename _Derived>
class SO2Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  SO2Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Angle represetation.
   */
  Scalar angle() const { return Base::log().x(); }

  /**
   * @brief Complex number (U(1)) representation.
   */
  std::complex<Scalar> u1() const
  {
    return std::complex<Scalar>(static_cast<const _Derived &>(*this).coeffs().y(),
      static_cast<const _Derived &>(*this).coeffs().x());
  }

  /**
   * @brief Rotation action on 2D vector.
   */
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return Base::matrix() * v;
  }

  /**
   * @brief Lift to SO3.
   *
   * Rotation of SO2 is embedded in SO3 as a rotation around the z axis.
   *
   * @note SO3 header must be included.
   */
  SO3<Scalar> lift_so3() const
  {
    using std::cos, std::sin;

    const Scalar yaw = Base::log().x();
    return SO3<Scalar>(Eigen::Quaternion<Scalar>(cos(yaw / 2), 0, 0, sin(yaw / 2)));
  }
};

// \cond
template<typename _Scalar>
class SO2;
// \endcond

// \cond
template<typename _Scalar>
struct lie_traits<SO2<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = SO2Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = SO2<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of SO2 Lie group.
 *
 * @see SO2Base for memory layout.
 */
template<typename _Scalar>
class SO2 : public SO2Base<SO2<_Scalar>>
{
  using Base = SO2Base<SO2<_Scalar>>;
  SMOOTH_GROUP_API(SO2);

public:
  /**
   * @brief Construct from coefficients.
   *
   * @param qz sine of rotation angle
   * @param qw cosine of rotation angle
   *
   * @note Inputs are are normalized to ensure group constraint.
   */
  SO2(const Scalar & qz, const Scalar & qw)
  {
    using std::sqrt;

    const Scalar n = sqrt(qw * qw + qz * qz);
    coeffs_.x()    = qz / n;
    coeffs_.y()    = qw / n;
  }

  /**
   * @brief Construct from angle.
   *
   * @param angle angle of rotation (radians).
   */
  explicit SO2(const Scalar & angle)
  {
    using std::cos, std::sin;

    coeffs_.x() = sin(angle);
    coeffs_.y() = cos(angle);
  }

  /**
   * @brief Construct from complex number.
   *
   * @param c complex number.
   *
   * @note Input is normalized to ensure group constraint.
   */
  SO2(const std::complex<Scalar> & c)
  {
    using std::sqrt;

    const Scalar n = sqrt(c.imag() * c.imag() + c.real() * c.real());
    coeffs_.x()    = c.imag() / n;
    coeffs_.y()    = c.real() / n;
  }
};

using SO2f = SO2<float>;   ///< SO2 with float scalar representation
using SO2d = SO2<double>;  ///< SO2 with double scalar representation

}  // namespace smooth

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<smooth::SO2<_Scalar>>>
    : public lie_traits<smooth::SO2<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of SO2 Lie group.
 *
 * @see SO2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<smooth::SO2<_Scalar>> : public smooth::SO2Base<Eigen::Map<smooth::SO2<_Scalar>>>
{
  using Base = smooth::SO2Base<Eigen::Map<smooth::SO2<_Scalar>>>;

  SMOOTH_MAP_API(Map);
};

// \cond
template<typename _Scalar>
struct smooth::lie_traits<Eigen::Map<const smooth::SO2<_Scalar>>>
    : public lie_traits<smooth::SO2<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of SO2 Lie group.
 *
 * @see SO2Base for memory layout.
 */
template<typename _Scalar>
class Eigen::Map<const smooth::SO2<_Scalar>>
    : public smooth::SO2Base<Eigen::Map<const smooth::SO2<_Scalar>>>
{
  using Base = smooth::SO2Base<Eigen::Map<const smooth::SO2<_Scalar>>>;

  SMOOTH_CONST_MAP_API(Map);
};

#endif  // SMOOTH__SO2_HPP_


// solving A * X = B
// in two steps -- factor (getrf()) and solve (getrs())

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace traits = boost::numeric::bindings::traits;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cin;
using std::cout;
using std::endl;

typedef std::complex<double> cmpx;
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx, ublas::column_major> cm_t;

int main (int argc, char **argv) {
  size_t n = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }

  cout << endl;

  if (n <= 0) {
  cout << "n -> ";
  cin >> n;
  }
  if (n < 5) {
    n = 5;
    cout << "min n = 5" << endl;
  }
  cout << endl;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  for (int i = 0; i < x.size1(); ++i) {
    x (i, 0) = 1.;
    x (i, 1) = 2.;
  }
  b = prod (a, x);
  m_t a2 (a);  // for part 2
  m_t b2 (b);

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  ublas::matrix_row<m_t> ar1 (a, 1), ar3 (a, 3);
  ublas::matrix_row<m_t> br1 (b, 1), br3 (b, 3);
  swap (ar1, ar3);   // swap rows to force pivoting
  swap (br1, br3);
  print_m (a, "A");  // print `new' system
  cout << endl;
  print_m (b, "B");
  cout << endl;

  std::vector<int> ipiv (n);  // pivot vector

  lapack::getrf (a, ipiv);      // factor a
  m_t ia (a);
  lapack::getrs (a, ipiv, b);   // solve from factorization
  print_m (b, "X");
  cout << endl;
  lapack::getri (ia, ipiv);     // invert a
  print_m (ia, "InvA");
  cout << endl;

  print_v (ipiv, "pivots");

  cout << endl;

  ublas::matrix_column<m_t> a2c1 (a2, 1), a2c4 (a2, 4);
  ublas::matrix_row<m_t> b2r1 (b2, 1), b2r4 (b2, 4);
  swap (a2c1, a2c4);   // swap columns
  swap (b2r1, b2r4);
  print_m (a2, "A");  // print `new' system
  cout << endl;
  print_m (b2, "B");
  cout << endl;

  lapack::getrf (a2, ipiv); // factor a
  m_t ia2 (a2);
  lapack::getrs ('T', a2, ipiv, b2); // solve
  print_m (b2, "X");
  cout << endl;
  lapack::getri (ia2, ipiv); // invert a2
  print_m (ia2, "InvA2");
  cout << endl;

  print_v (ipiv, "pivots");

  cout << endl;
}


// VMDモーションを平滑化する

#include <map>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <unsupported/Eigen/FFT>
#include "VMD.h"
#include "MMDFileIOUtil.h"
#include "interpolate.h"
#include "reducevmd.h"
#include "smoothvmd.h"

using namespace Eigen;
using namespace MMDFileIOUtil;
using namespace std;

// ローパスフィルタ。cutoff_freqより高い周波数成分を除去する
void lowpass_filter(vector<float>& v, float cutoff_freq)
{
  if (v.size() < 2) {
    return;
  }
  FFT<float> fft;
  vector<complex<float>> freqvec;
  fft.fwd(freqvec, v);

  // フィルタリング
  const float sampling_freq = 30.0; // サンプリング周波数[Hz]。MMDは30FPSなので。
  int data_size = v.size();
  int cutoff_idx;
  // 打ち切る位置(cutoff_idx)を求める： cutoff_idx / data_size = cutoff_freq / sampling_freq
  cutoff_idx = cutoff_freq * data_size / sampling_freq;

  vector<complex<float>> filtered(data_size);
  for (int i = 0; i < data_size; i++) {
    if (i < cutoff_idx) {
      filtered[i] = freqvec[i];
    } else {
      filtered[i] = 0.0;
    }
  }
  fft.inv(v, filtered);
}

// ボーンキーフレーム列fvの値を平滑化する
// 引数fvには同一ボーンのキーフレームがフレーム番号順に格納されているものとする
void smooth_bone_frame(vector<VMD_Frame>& fv, float cutoff_freq, bool bezier)
{
  sort(fv.begin(), fv.end());
  fv = fill_bone_frame(fv, bezier); // キーフレームの隙間をなくす
  if (cutoff_freq < 0) {
    return;
  }

  // ローパスフィルタにかける
  vector<float> x;
  for_each(fv.begin(), fv.end(), [&x](VMD_Frame f) { x.push_back(f.position.x()); });
  lowpass_filter(x, cutoff_freq);
  for (unsigned int i = 0; i < x.size(); i++) {
    fv[i].position.x() = x[i];
  }
  vector<float> y;
  for_each(fv.begin(), fv.end(), [&y](VMD_Frame f) { y.push_back(f.position.y()); });
  lowpass_filter(y, cutoff_freq);
  for (unsigned int i = 0; i < y.size(); i++) {
    fv[i].position.y() = y[i];
  }
  vector<float> z;
  for_each(fv.begin(), fv.end(), [&z](VMD_Frame f) { z.push_back(f.position.z()); });
  lowpass_filter(z, cutoff_freq);
  for (unsigned int i = 0; i < z.size(); i++) {
    fv[i].position.z() = z[i];
  }

  // 回転のローパスフィルタ
  // ※正しいやり方が分からないため、クォータニオンの各要素に対してローパスフィルタを掛けている。
  // TODO: クォータニオンのフーリエ変換
  // 同じ回転を表すクォータニオンが正負2通りあるので、wの符号が正のほうに統一する
  for (unsigned int i = 0; i < fv.size(); i++) {
    if (fv[i].rotation.w() < 0) {
      fv[i].rotation.w() *= -1;
      fv[i].rotation.x() *= -1;
      fv[i].rotation.y() *= -1;
      fv[i].rotation.z() *= -1;
    }
  }
  vector<float> rx;
  for_each(fv.begin(), fv.end(), [&rx](VMD_Frame f) { rx.push_back(f.rotation.x()); });
  lowpass_filter(rx, cutoff_freq);
  for (unsigned int i = 0; i < rx.size(); i++) {
    fv[i].rotation.x() = rx[i];
  }
  vector<float> ry;
  for_each(fv.begin(), fv.end(), [&ry](VMD_Frame f) { ry.push_back(f.rotation.y()); });
  lowpass_filter(ry, cutoff_freq);
  for (unsigned int i = 0; i < ry.size(); i++) {
    fv[i].rotation.y() = ry[i];
  }
  vector<float> rz;
  for_each(fv.begin(), fv.end(), [&rz](VMD_Frame f) { rz.push_back(f.rotation.z()); });
  lowpass_filter(rz, cutoff_freq);
  for (unsigned int i = 0; i < rz.size(); i++) {
    fv[i].rotation.z() = rz[i];
  }
  vector<float> rw;
  for_each(fv.begin(), fv.end(), [&rw](VMD_Frame f) { rw.push_back(f.rotation.w()); });
  lowpass_filter(rw, cutoff_freq);
  for (unsigned int i = 0; i < rw.size(); i++) {
    fv[i].rotation.w() = rw[i];
  }
  // 各要素(w, x, y, z)に対し独立に変換をかけているので、正規化しておく
  // （正規化しないと、回転した先の部分が歪む）
  for (unsigned int i = 0; i < fv.size(); i++) {
    fv[i].rotation.normalize();
  }
}

// 表情キーフレーム列mvの値を平滑化する
// 引数mvには同一モーフのキーフレームがフレーム番号順に格納されているものとする
void smooth_morph_frame(vector<VMD_Morph>& mv, float cutoff_freq)
{
  sort(mv.begin(), mv.end());
  mv = fill_morph_frame(mv); // キーフレームの隙間をなくす
  if (cutoff_freq < 0) {
    return;
  }

  // ローパスフィルタにかける
  vector<float> w;
  for_each(mv.begin(), mv.end(), [&w](VMD_Morph s) { w.push_back(s.weight); });
  lowpass_filter(w, cutoff_freq);
  for (unsigned int i = 0; i < w.size(); i++) {
    mv[i].weight = w[i];
    if (w[i] > 1.0) {
      mv[i].weight = 1.0;
    } else if (w[i] < 0.0) {
      mv[i].weight = 0.0;
    }
  }
}


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

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

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

#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/expm1.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/log.hpp>
#include <boost/simd/function/scalar/log1p.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( logspace_sub_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      A0 x =  a1-a0;
      if(is_eqz(x)) return Minf<A0>();
      A0 tmp = (x > -Log_2<A0>()) ? bs::log(-expm1(x)) : bs::log1p(-exp(x));
      return a0 + tmp;
    }
  };
} } }

#endif


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Author assumes no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS1_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS1_OVERLOADS_HPP

#include <complex>
#include <boost/numeric/bindings/atlas/cblas_inc.hpp>
#include <boost/numeric/bindings/traits/type.hpp>

namespace boost { namespace numeric { namespace bindings {

  namespace atlas { namespace detail {

    // dot <- x^T * y
    // .. real types:    calls cblas_xdot
    // .. complex types: calls cblas_xdotu
    inline
    float dot (int const N, float const* X, int const incX,
               float const* Y, int const incY) {
      return cblas_sdot (N, X, incX, Y, incY);
    }
    inline
    double dot (int const N, double const* X, int const incX,
                double const* Y, int const incY) {
      return cblas_ddot (N, X, incX, Y, incY);
    }
    inline
    traits::complex_f
    dot (int const N, traits::complex_f const* X, int const incX,
         traits::complex_f const* Y, int const incY) {
      traits::complex_f val;
      cblas_cdotu_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (&val));
      return val;
    }
    inline
    traits::complex_d
    dot (int const N, traits::complex_d const* X, int const incX,
         traits::complex_d const* Y, int const incY) {
      traits::complex_d val;
      cblas_zdotu_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (&val));
      return val;
    }

    // dotu <- x^T * y
    // .. complex types only
    inline
    void dotu (int const N, traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* val)
    {
      cblas_cdotu_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (val));
    }
    inline
    void dotu (int const N, traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* val)
    {
      cblas_zdotu_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (val));
    }

    // dotc <- x^H * y
    // .. complex types only
    inline
    void dotc (int const N, traits::complex_f const* X, int const incX,
               traits::complex_f const* Y, int const incY,
               traits::complex_f* val)
    {
      cblas_cdotc_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (val));
    }
    inline
    void dotc (int const N, traits::complex_d const* X, int const incX,
               traits::complex_d const* Y, int const incY,
               traits::complex_d* val)
    {
      cblas_zdotc_sub (N,
                       static_cast<void const*> (X), incX,
                       static_cast<void const*> (Y), incY,
                       static_cast<void*> (val));
    }

    // nrm2 <- ||x||_2
    inline
    float nrm2 (int const N, float const* X, int const incX) {
      return cblas_snrm2 (N, X, incX);
    }
    inline
    double nrm2 (int const N, double const* X, int const incX) {
      return cblas_dnrm2 (N, X, incX);
    }
    inline
    float nrm2 (int const N, traits::complex_f const* X, int const incX) {
      return cblas_scnrm2 (N, static_cast<void const*> (X), incX);
    }
    inline
    double nrm2 (int const N, traits::complex_d const* X, int const incX) {
      return cblas_dznrm2 (N, static_cast<void const*> (X), incX);
    }

    // asum <- ||re (x)|| + ||im (x)||
    inline
    float asum (int const N, float const* X, int const incX) {
      return cblas_sasum (N, X, incX);
    }
    inline
    double asum (int const N, double const* X, int const incX) {
      return cblas_dasum (N, X, incX);
    }
    inline
    float asum (int const N, traits::complex_f const* X, int const incX) {
      return cblas_scasum (N, static_cast<void const*> (X), incX);
    }
    inline
    double asum (int const N, traits::complex_d const* X, int const incX) {
      return cblas_dzasum (N, static_cast<void const*> (X), incX);
    }

    // iamax <- 1st i: max (|re (x_i)| + |im (x_i)|)
    inline
    CBLAS_INDEX iamax (int const N, float const* X, int const incX) {
      return cblas_isamax (N, X, incX);
    }
    inline
    CBLAS_INDEX iamax (int const N, double const* X, int const incX) {
      return cblas_idamax (N, X, incX);
    }
    inline
    CBLAS_INDEX
    iamax (int const N, traits::complex_f const* X, int const incX) {
      return cblas_icamax (N, static_cast<void const*> (X), incX);
    }
    inline
    CBLAS_INDEX
    iamax (int const N, traits::complex_d const* X, int const incX) {
      return cblas_izamax (N, static_cast<void const*> (X), incX);
    }

    // x <-> y
    inline
    void swap (int const N, float* X, int const incX,
               float* Y, int const incY) {
      cblas_sswap (N, X, incX, Y, incY);
    }
    inline
    void swap (int const N, double* X, int const incX,
               double* Y, int const incY) {
      cblas_dswap (N, X, incX, Y, incY);
    }
    inline
    void swap (int const N, traits::complex_f* X, int const incX,
               traits::complex_f* Y, int const incY) {
      cblas_cswap (N,
                   static_cast<void*> (X), incX,
                   static_cast<void*> (Y), incY);
    }
    inline
    void swap (int const N, traits::complex_d* X, int const incX,
               traits::complex_d* Y, int const incY) {
      cblas_zswap (N,
                   static_cast<void*> (X), incX,
                   static_cast<void*> (Y), incY);
    }

    // y <- x
    inline
    void copy (int const N, float const* X, int const incX,
               float* Y, int const incY) {
      cblas_scopy (N, X, incX, Y, incY);
    }
    inline
    void copy (int const N, double const* X, int const incX,
               double* Y, int const incY) {
      cblas_dcopy (N, X, incX, Y, incY);
    }
    inline
    void copy (int const N, traits::complex_f const* X, int const incX,
               traits::complex_f* Y, int const incY) {
      cblas_ccopy (N,
                   static_cast<void const*> (X), incX,
                   static_cast<void*> (Y), incY);
    }
    inline
    void copy (int const N, traits::complex_d const* X, int const incX,
               traits::complex_d* Y, int const incY) {
      cblas_zcopy (N,
                   static_cast<void const*> (X), incX,
                   static_cast<void*> (Y), incY);
    }

    // y <- alpha * x + y
    inline
    void axpy (int const N,
               float const alpha, float const* X, int const incX,
               float* Y, int const incY)
    {
      cblas_saxpy (N, alpha, X, incX, Y, incY);
    }
    inline
    void axpy (int const N,
               double const alpha, double const* X, int const incX,
               double* Y, int const incY)
    {
      cblas_daxpy (N, alpha, X, incX, Y, incY);
    }
    inline
    void axpy (int const N,
               traits::complex_f const& alpha,
               traits::complex_f const* X, int const incX,
               traits::complex_f* Y, int const incY)
    {
      cblas_caxpy (N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void*> (Y), incY);
    }
    inline
    void axpy (int const N,
               traits::complex_d const& alpha,
               traits::complex_d const* X, int const incX,
               traits::complex_d* Y, int const incY)
    {
      cblas_zaxpy (N,
                   static_cast<void const*> (&alpha),
                   static_cast<void const*> (X), incX,
                   static_cast<void*> (Y), incY);
    }

    // y <- alpha * x + beta * y
    inline
    void axpby (int const N,
                float const alpha, float const* X, int const incX,
                float const beta, float* Y, int const incY)
    {
      catlas_saxpby (N, alpha, X, incX, beta, Y, incY);
    }
    inline
    void axpby (int const N,
                double const alpha, double const* X, int const incX,
                double const beta, double* Y, int const incY)
    {
      catlas_daxpby (N, alpha, X, incX, beta, Y, incY);
    }
    inline
    void axpby (int const N,
                traits::complex_f const& alpha,
                traits::complex_f const* X, int const incX,
                traits::complex_f const& beta,
                traits::complex_f* Y, int const incY)
    {
      catlas_caxpby (N,
                     static_cast<void const*> (&alpha),
                     static_cast<void const*> (X), incX,
                     static_cast<void const*> (&beta),
                     static_cast<void*> (Y), incY);
    }
    inline
    void axpby (int const N,
                traits::complex_d const& alpha,
                traits::complex_d const* X, int const incX,
                traits::complex_d const& beta,
                traits::complex_d* Y, int const incY)
    {
      catlas_zaxpby (N,
                     static_cast<void const*> (&alpha),
                     static_cast<void const*> (X), incX,
                     static_cast<void const*> (&beta),
                     static_cast<void*> (Y), incY);
    }

    // x_i <- alpha for all i
    inline
    void set (int const N, float const alpha, float* X, int const incX) {
      catlas_sset (N, alpha, X, incX);
    }
    inline
    void set (int const N, double const alpha, double* X, int const incX) {
      catlas_dset (N, alpha, X, incX);
    }
    inline
    void set (int const N, traits::complex_f const& alpha,
              traits::complex_f* X, int const incX) {
      catlas_cset (N, static_cast<void const*> (&alpha),
                   static_cast<void*> (X), incX);
    }
    inline
    void set (int const N, traits::complex_d const& alpha,
              traits::complex_d* X, int const incX) {
      catlas_zset (N, static_cast<void const*> (&alpha),
                   static_cast<void*> (X), incX);
    }

    // x <- alpha * x
    inline
    void scal (int const N, float const alpha, float* X, int const incX) {
      cblas_sscal (N, alpha, X, incX);
    }
    inline
    void scal (int const N, double const alpha, double* X, int const incX) {
      cblas_dscal (N, alpha, X, incX);
    }
    inline
    void scal (int const N, traits::complex_f const& alpha,
               traits::complex_f* X, int const incX) {
      cblas_cscal (N, static_cast<void const*> (&alpha),
                   static_cast<void*> (X), incX);
    }
    inline
    void scal (int const N, float const alpha,
               traits::complex_f* X, int const incX) {
      cblas_csscal (N, alpha, static_cast<void*> (X), incX);
    }
    inline
    void scal (int const N, traits::complex_d const& alpha,
               traits::complex_d* X, int const incX) {
      cblas_zscal (N, static_cast<void const*> (&alpha),
                   static_cast<void*> (X), incX);
    }
    inline
    void scal (int const N, double const alpha,
               traits::complex_d* X, int const incX) {
      cblas_zdscal (N, alpha, static_cast<void*> (X), incX);
    }

  }} // namepaces detail & atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CBLAS1_OVERLOADS_HPP


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file standard_gaussian.hpp
 * \date May 2014
 * \author Jan Issac (jan.issac@gmail.com)
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <random>
#include <type_traits>

#include <fl/util/math.hpp>
#include <fl/util/random.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/types.hpp>
#include <fl/distribution/interface/sampling.hpp>
#include <fl/distribution/interface/moments.hpp>
#include <fl/exception/exception.hpp>

namespace fl
{

/**
 * \ingroup distributions
 */
template <typename StandardVariate>
class StandardGaussian
    : public Sampling<StandardVariate>,
      public Moments<
                StandardVariate,
                typename DiagonalSecondMomentOf<StandardVariate>::Type>
{
public:
    typedef StandardVariate Variate;

    typedef Moments<
                StandardVariate,
                typename DiagonalSecondMomentOf<StandardVariate>::Type
            > MomentsBase;

    typedef typename MomentsBase::SecondMoment SecondMoment;
    typedef typename MomentsBase::SecondMoment DiagonalSecondMoment;

public:
    explicit
    StandardGaussian(int dim = DimensionOf<StandardVariate>())
        : dimension_ (dim),
          mu_(Variate::Zero(dim, 1)),
          cov_(DiagonalSecondMoment(dim)),
          generator_(fl::seed()),
          gaussian_distribution_(0.0, 1.0)
    {
        cov_.setIdentity();
    }

    virtual ~StandardGaussian() noexcept { }

    virtual StandardVariate sample() const
    {
        StandardVariate gaussian_sample(dimension(), 1);

        for (int i = 0; i < dimension_; i++)
        {
            gaussian_sample(i, 0) = gaussian_distribution_(generator_);
        }

        return gaussian_sample;
    }

    virtual int dimension() const
    {
        return dimension_;
    }

    virtual void dimension(int new_dimension)
    {
        if (dimension_ == new_dimension) return;

        if (fl::IsFixed<StandardVariate::SizeAtCompileTime>())
        {
            fl_throw(
                fl::ResizingFixedSizeEntityException(dimension_,
                                                     new_dimension,
                                                     "Gaussian"));
        }

        dimension_ = new_dimension;
    }

    virtual const Variate& mean() const
    {
        return mu_;
    }

    virtual const DiagonalSecondMoment& covariance() const
    {
        return cov_;
    }

protected:
    /** \cond internal */
    int dimension_;
    Variate mu_;
    DiagonalSecondMoment cov_;
    mutable fl::mt11213b generator_;
    mutable std::normal_distribution<Real> gaussian_distribution_;
    /** \endcond */
};

/**
 * Floating point implementation for Scalar types float, double and long double
 */
template <>
class StandardGaussian<Real>
    : public Sampling<Real>,
      public Moments<Real, Real>
{
public:
    StandardGaussian()
        : mu_(0.),
          var_(1.),
          generator_(fl::seed()),
          gaussian_distribution_(mu_, var_)
    { }

    Real sample() const
    {
        return gaussian_distribution_(generator_);
    }

    virtual const Real& mean() const
    {
        return mu_;
    }

    virtual const Real& covariance() const
    {
        return var_;
    }

protected:
    /** \cond internal */
    Real mu_;
    Real var_;
    mutable fl::mt11213b generator_;
    mutable std::normal_distribution<Real> gaussian_distribution_;
    /** \endcond */
};

}


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

 * 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 GncPoseAveragingExample.cpp
 * @brief example of GNC estimating a single pose from pose priors possibly corrupted with outliers
 * You can run this example using: ./GncPoseAveragingExample nrInliers nrOutliers
 * e.g.,: ./GncPoseAveragingExample 10 5  (if the numbers are not specified, default
 * values nrInliers = 10 and nrOutliers = 10 are used)
 * @date May 8, 2021
 * @author Luca Carlone
 */

#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GncOptimizer.h>

#include <string>
#include <fstream>
#include <iostream>
#include <random>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace gtsam;

int main(int argc, char** argv){
  cout << "== Robust Pose Averaging Example === " << endl;

  // default number of inliers and outliers
  size_t nrInliers = 10;
  size_t nrOutliers = 10;

  // User can pass arbitrary number of inliers and outliers for testing
  if (argc > 1)
    nrInliers = atoi(argv[1]);
  if (argc > 2)
    nrOutliers = atoi(argv[2]);
  cout << "nrInliers " << nrInliers << " nrOutliers "<< nrOutliers << endl;

  // Seed random number generator
  random_device rd;
  mt19937 rng(rd());
  uniform_real_distribution<double> uniform(-10, 10);
  normal_distribution<double> normalInliers(0.0, 0.05);

  Values initial;
  initial.insert(0, Pose3::identity()); // identity pose as initialization

  // create ground truth pose
  Vector6 poseGtVector;
  for(size_t i = 0; i < 6; ++i){
    poseGtVector(i) = uniform(rng);
  }
  Pose3 gtPose = Pose3::Expmap(poseGtVector); // Pose3( Rot3::Ypr(3.0, 1.5, 0.8), Point3(4,1,3) );

  NonlinearFactorGraph graph;
  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(6,0.05);
  // create inliers
  for(size_t i=0; i<nrInliers; i++){
    Vector6 poseNoise;
    for(size_t i = 0; i < 6; ++i){
      poseNoise(i) = normalInliers(rng);
    }
    Pose3 poseMeasurement = gtPose.retract(poseNoise);
    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
  }

  // create outliers
  for(size_t i=0; i<nrOutliers; i++){
    Vector6 poseNoise;
    for(size_t i = 0; i < 6; ++i){
      poseNoise(i) = uniform(rng);
    }
    Pose3 poseMeasurement = gtPose.retract(poseNoise);
    graph.add(gtsam::PriorFactor<gtsam::Pose3>(0,poseMeasurement,model));
  }

  GncParams<LevenbergMarquardtParams> gncParams;
  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(graph,
      initial,
      gncParams);

  Values estimate = gnc.optimize();
  Pose3 poseError = gtPose.between( estimate.at<Pose3>(0) );
  cout << "norm of translation error: " << poseError.translation().norm() <<
      " norm of rotation error: " << poseError.rotation().rpy().norm() << endl;
  // poseError.print("pose error: \n ");
  return 0;
}


#ifndef __KALMANFILTER_MAIN_CPP__
#define __KALMANFILTER_MAIN_CPP__

#include <cstring>
#include <string>
#include <cstdio>
#include <vector>

#include <cmath>

#include <iostream>
#include <fstream>
#include <iomanip>

#include <Eigen/Dense>

#include "KalmanFilter.hpp"

#define b printf("Line: %d\n", __LINE__);

const int STATE_DIMENSIONS = 5;
const int MEASUREMENT_DIMENSIONS = 5;
const int NUM_MEASUREMENTS = 6;

const double DELTA_T = 0.001;
const double LINEAR_VELOCITY = 0.14;
const double ANGULAR_VELOCITY = LINEAR_VELOCITY * tan(0);
const double ARBITRARY_VARIANCE = 0.1;

/* Given data, resulting matrix arrangement:
        Measurements:           Covariances:
    0    odometer.x              Something arbitrary
    1    GPS.x                   GPS.x covariance
    2    odometer.y              Something arbitrary
    3    GPS.y                   GPS.y covariance
    4    odometer.\theta         Something arbitrary
    5    IMU.\theta              IMU.\theta covariance
 */
const int ODOM_X = 0;
const int ODOM_Y = 2;
const int ODOM_T = 4;
const int GPS_X = 1;
const int GPS_Y = 3;
const int IMU_T = 5;
const int X_ESTIMATE = 0;
const int Y_ESTIMATE = 1;
const int V_ESTIMATE = 2;
const int T_ESTIMATE = 3;
const int W_ESTIMATE = 4;

typedef struct {
    std::string dataFilename;
    std::string xyFilename;
    std::string tFilename;
    std::string vFilename;
    std::string wFilename;
    double stateCovarianceModifier;
} Configurations;

typedef struct {
    Eigen::VectorXd measurements;
    Eigen::VectorXd variances;
} DataItem;

Configurations processCmdLineArgs(int pArgc, char** pArgs);

std::vector<DataItem> readDataFile(const std::string& dataFileName);

DataItem readDataLine(std::ifstream& fin);

std::vector<KalmanFilter::KalmanState> applyKalmanFilterToData(
    Configurations& pConfigurations,
    std::vector<DataItem>& pData);

Eigen::MatrixXd computeUpdatedNaturalModelMatrix(
    int pStateDimensionality,
    double pTimeStepMagnitude,
    Eigen::VectorXd& pPreviousState);

Eigen::MatrixXd computeUpdatedStateNoiseCovariance(
    int pStateDimensionality,
    double pXPosCovariance,
    double pYPosCovariance);

Eigen::MatrixXd computeUpdatedObservationNoiseCovariance(
    int pObservationDimensionality,
    double pXPosCovariance,
    double pYPosCovariance,
    double pTPosCovariance);

Eigen::VectorXd reduceDataToInputMeasurements(
    Eigen::VectorXd& pMeasurementData,
    int pNewVectorSize,
    const std::vector<std::pair<int, int>>& pDataIndices);

void writeDataToFile(
    std::string pFileName,
    std::vector<DataItem>& pInputData,
    const std::vector<std::string>& pDataColumnLabels,
    const std::vector<int>& pDataIndices,
    std::vector<KalmanFilter::KalmanState>& pKalmanOutputData,
    const std::vector<std::string>& pOutputColumnLabels,
    const std::vector<int>& pOutputIndices);

void testSimpleKalmanFilter();

int main(int argc, char** argv) {
    Configurations configurations;

    try {
        puts("Initializing program...");
        configurations = processCmdLineArgs(argc, argv);

        puts("Reading data...");
        std::vector<DataItem> data = readDataFile(configurations.dataFilename);

        puts("Applying KalmanFilter...");
        std::vector<KalmanFilter::KalmanState> results =
            applyKalmanFilterToData(configurations, data);

        puts("Writing results to file...");
        writeDataToFile(
            configurations.xyFilename,
            data,
            {"Odom_X", "Odom_Y", "GPS_X", "GPS_Y"},
            {ODOM_X, ODOM_Y, GPS_X, GPS_Y},
            results,
            {"X-estimate", "Y-estimate"},
            {X_ESTIMATE, Y_ESTIMATE}
        );
        writeDataToFile(
            configurations.tFilename,
            data,
            {"Odom_T", "IMU_T"},
            {ODOM_T, IMU_T},
            results,
            {"T-estimate",},
            {T_ESTIMATE}
        );
        writeDataToFile(
            configurations.vFilename,
            data,
            {},
            {},
            results,
            {"v-estimate"},
            {V_ESTIMATE}
        );
        writeDataToFile(
            configurations.wFilename,
            data,
            {},
            {},
            results,
            {"w-estimate",},
            {W_ESTIMATE}
        );

    } catch (std::exception& e) {
        puts("An error occured - program failure!");
        puts(e.what());
    }

    return 0;
}

Configurations processCmdLineArgs(int pArgc, char** pArgs) {
    Configurations configurations;

    if (pArgc < (3 + 1)) {
        throw std::exception();
    }

// TODO: use strtok for cleaner arg parsing

    for (int i = 1; i < pArgc; i += 2) {
        if (strcmp(pArgs[i], "-data") == 0) {
            configurations.dataFilename = pArgs[i + 1];
        } else if (strcmp(pArgs[i], "-xy") == 0) {
            configurations.xyFilename = pArgs[i + 1];
        } else if (strcmp(pArgs[i], "-theta") == 0) {
            configurations.tFilename = pArgs[i + 1];
        } else if (strcmp(pArgs[i], "-v") == 0) {
            configurations.vFilename = pArgs[i + 1];
        } else if (strcmp(pArgs[i], "-w") == 0) {
            configurations.wFilename = pArgs[i + 1];
        } else if (strcmp(pArgs[i], "-cov") == 0) {
            configurations.stateCovarianceModifier = atof(pArgs[i + 1]);
        }
    }

    return configurations;
}

std::vector<DataItem> readDataFile(const std::string& dataFileName) {
    std::vector<DataItem> data;

    try {
        std::ifstream fin;
        fin.clear();
        fin.open(dataFileName.c_str());

        std::string dummyString;
        int dummy;
        char delimeter;
        DataItem temp;

        fin >> dummyString;

        temp = readDataLine(fin);

        while (fin.good()) {
            data.push_back(temp);

            temp = readDataLine(fin);
        }

        fin.close();
    } catch (std::exception& e) {
        throw e;
    }

    return data;
}

DataItem readDataLine(std::ifstream& fin) {
    DataItem dataItem;
    dataItem.measurements.resize(NUM_MEASUREMENTS);
    dataItem.variances.resize(NUM_MEASUREMENTS);

    try {
        int dummy;
        char delimeter;

        dataItem.measurements.setZero();
        dataItem.variances.setConstant(NUM_MEASUREMENTS, 1, ARBITRARY_VARIANCE);

        fin >>
            dummy >> delimeter >>
            dataItem.measurements(ODOM_X) >> delimeter >> // odometer.x
            dataItem.measurements(ODOM_Y) >> delimeter >> // odometer.y
            dataItem.measurements(ODOM_T) >> delimeter >> // odometer.\theta
            dataItem.measurements(IMU_T) >> delimeter >>  // IMU.\theta
            dataItem.variances(IMU_T) >> delimeter >>     // IMU.\theta covariance
            dataItem.measurements(GPS_X) >> delimeter >>  // GPS.x
            dataItem.measurements(GPS_Y) >> delimeter >>  // GPS.y
            dataItem.variances(GPS_X) >> delimeter >>     // GPS.x covariance
            dataItem.variances(GPS_Y);                    // GPS.y covariance

            dataItem.measurements(IMU_T) += 0.172; // calibration - just matches things to start
    } catch (std::exception& e) {
        throw e;
    }

    return dataItem;
}

std::vector<KalmanFilter::KalmanState> applyKalmanFilterToData(
    Configurations& pConfigurations,
    std::vector<DataItem>& pData) {

    std::vector<KalmanFilter::KalmanState> results;

    try {
        KalmanFilter kalmanFilter(STATE_DIMENSIONS, MEASUREMENT_DIMENSIONS);

        Eigen::VectorXd tempMeasurements(NUM_MEASUREMENTS);
        tempMeasurements = pData[0].measurements;

        Eigen::VectorXd Z(MEASUREMENT_DIMENSIONS);
        Z = reduceDataToInputMeasurements(
            pData[0].measurements,
            MEASUREMENT_DIMENSIONS,
            {
                {GPS_X, X_ESTIMATE},
                {GPS_Y, Y_ESTIMATE},
                {IMU_T, T_ESTIMATE}
            }
        );

        Eigen::MatrixXd A(STATE_DIMENSIONS, STATE_DIMENSIONS);
        A = computeUpdatedNaturalModelMatrix(
                STATE_DIMENSIONS,
                DELTA_T,
                tempMeasurements
        );

        Eigen::MatrixXd B(STATE_DIMENSIONS, STATE_DIMENSIONS);
        B.setIdentity(STATE_DIMENSIONS, STATE_DIMENSIONS);

        Eigen::VectorXd u(STATE_DIMENSIONS);
        u.setZero();

        Eigen::MatrixXd H(MEASUREMENT_DIMENSIONS, STATE_DIMENSIONS);
        H.setIdentity(MEASUREMENT_DIMENSIONS, STATE_DIMENSIONS);

        Eigen::MatrixXd R(MEASUREMENT_DIMENSIONS, MEASUREMENT_DIMENSIONS);
        R = computeUpdatedObservationNoiseCovariance(MEASUREMENT_DIMENSIONS, 0.1, 0.1, 0.01);

        Eigen::MatrixXd Q(STATE_DIMENSIONS, STATE_DIMENSIONS);
        Q = computeUpdatedStateNoiseCovariance(STATE_DIMENSIONS, 0.00001, 0.00001);
        Q *= pConfigurations.stateCovarianceModifier;

        KalmanFilter::KalmanState previousState;
        previousState.state.resize(STATE_DIMENSIONS, 1);
        previousState.state.setZero();
        previousState.errorCovariance.resize(STATE_DIMENSIONS, STATE_DIMENSIONS);
        previousState.errorCovariance.setIdentity();
        previousState.errorCovariance *= 0.01;

        kalmanFilter.setNaturalModel(A);
        kalmanFilter.setControlModel(B);
        kalmanFilter.setTransitionModel(H);
        kalmanFilter.setStateNoiseCovariance(Q);
        kalmanFilter.setMeasurementNoiseCovariance(R);

        for (int i = 0; i < pData.size(); i++) {

            A = computeUpdatedNaturalModelMatrix(
                    STATE_DIMENSIONS,
                    DELTA_T,
                    previousState.state
            );
            R = computeUpdatedObservationNoiseCovariance(
                    MEASUREMENT_DIMENSIONS,
                    pData[i].variances(GPS_X),
                    pData[i].variances(GPS_Y),
                    pData[i].variances(IMU_T)
            );
            Z = reduceDataToInputMeasurements(
                pData[i].measurements,
                MEASUREMENT_DIMENSIONS,
                {
                    {GPS_X, X_ESTIMATE},
                    {GPS_Y, Y_ESTIMATE},
                    {IMU_T, T_ESTIMATE}
                }
            );

            kalmanFilter.setNaturalModel(A);
            kalmanFilter.setMeasurementNoiseCovariance(R);
            previousState = kalmanFilter.KalmanFilterIteration(previousState, Z, u);

            results.push_back(previousState);
        }

    } catch (std::exception& e) {
        throw e;
    }

    return results;
}

Eigen::MatrixXd computeUpdatedNaturalModelMatrix(
    int pStateDimensionality,
    double pTimeStepMagnitude,
    Eigen::VectorXd& pPreviousState) {

    double previousAngle = pPreviousState(T_ESTIMATE);
    double previousAngularVelocity = pPreviousState(W_ESTIMATE);

    Eigen::MatrixXd A(pStateDimensionality, pStateDimensionality);
    A << 1, 0, pTimeStepMagnitude * cos(previousAngle), 0,                  0,
         0, 1, pTimeStepMagnitude * sin(previousAngle), 0,                  0,
         0, 0,                                       1, 0,                  0,
         0, 0,                                       0, 1, pTimeStepMagnitude,
         0, 0,                                       0, 0,                  1;

    return A;
}

Eigen::MatrixXd computeUpdatedStateNoiseCovariance(
    int pStateDimensionality,
    double pXPosCovariance,
    double pYPosCovariance) {

    Eigen::MatrixXd Q(pStateDimensionality, pStateDimensionality);
    Q << pXPosCovariance,               0,     0,     0,     0,
                       0, pYPosCovariance,     0,     0,     0,
                       0,               0, 0.001,     0,     0,
                       0,               0,     0, 0.001,     0,
                       0,               0,     0,     0, 0.001;
    return Q;
}

Eigen::MatrixXd computeUpdatedObservationNoiseCovariance(
    int pObservationDimensionality,
    double pXPosCovariance,
    double pYPosCovariance,
    double pTPosCovariance) {

    Eigen::MatrixXd R(pObservationDimensionality, pObservationDimensionality);
    R << pXPosCovariance,               0,    0,               0,    0,
                       0, pYPosCovariance,    0,               0,    0,
                       0,               0, 0.01,               0,    0,
                       0,               0,    0, pTPosCovariance,    0,
                       0,               0,    0,               0, 0.01;

    return R;
}

Eigen::VectorXd reduceDataToInputMeasurements(
    Eigen::VectorXd& pMeasurementData,
    int pNewVectorSize,
    const std::vector<std::pair<int, int>>& pDataIndices) {

    Eigen::VectorXd newMeasurementVector(pNewVectorSize);
    newMeasurementVector.setZero();

    for (std::pair<int, int> indices : pDataIndices) {
        newMeasurementVector(indices.second) = pMeasurementData(indices.first);
    }

    return newMeasurementVector;
}

void writeDataToFile(
    std::string pFileName,
    std::vector<DataItem>& pInputData,
    const std::vector<std::string>& pDataColumnLabels,
    const std::vector<int>& pDataIndices,
    std::vector<KalmanFilter::KalmanState>& pKalmanOutputData,
    const std::vector<std::string>& pOutputColumnLabels,
    const std::vector<int>& pOutputIndices) {

    try {
        std::ofstream fout;

        fout.clear();
        fout.open(pFileName + ".txt");

        std::string fileComment = "# ";
        fileComment += pFileName;
        fileComment += "data";
        fout << fileComment << std::endl;

        fileComment = "# time\t";
        for (std::string label : pDataColumnLabels) {
            fileComment += label;
            fileComment += "\t";
        }
        for (std::string label : pOutputColumnLabels) {
            fileComment += label;
            fileComment += "\t";
        }
        fout << fileComment << std::endl;

        for (int i = 0; i < pKalmanOutputData.size(); i++) {
            fout << std::setw(5);
            fout << i << '\t';

            for (int index : pDataIndices) {
                fout << std::setw(14);
                fout << pInputData[i].measurements(index) << "\t";
            }
            for (int index : pOutputIndices) {
                fout << std::setw(14);
                fout << pKalmanOutputData[i].state(index) << "\t";
            }

            fout << std::endl;
        }

        fout.close();

    } catch (std::exception& e) {
        throw e;
    }

}

void testSimpleKalmanFilter() {
        Eigen::MatrixXd A(1,1); A << 1;
        Eigen::MatrixXd B(1,1); B << 1;
        Eigen::MatrixXd H(1,1); H << 1;
        KalmanFilter::KalmanState state;
            state.state.resize(1);
            state.state << 0.0;
            state.errorCovariance.resize(1,1);
            state.errorCovariance << 1.0;
        Eigen::VectorXd Z(1); Z << 0.390;
        Eigen::VectorXd u(1); u << 0.0;
        Eigen::MatrixXd Q(1,1); Q << 0.0;
        Eigen::MatrixXd R(1,1); R << 0.1;

        KalmanFilter kalmanFilter(1, 1);
        kalmanFilter.setNaturalModel(A);
        kalmanFilter.setControlModel(B);
        kalmanFilter.setTransitionModel(H);
        kalmanFilter.setStateNoiseCovariance(Q);
        kalmanFilter.setMeasurementNoiseCovariance(R);

        Z << 0.390;
        state = kalmanFilter.KalmanFilterIteration(state, Z, u);
        std::cout << "state\n" << state.state << std::endl <<
                  "P\n" << state.errorCovariance << std::endl << std::endl;

                          Z << 0.5;
        state = kalmanFilter.KalmanFilterIteration(state, Z, u);
        std::cout << "state\n" << state.state << std::endl <<
                  "P\n" << state.errorCovariance << std::endl << std::endl;

                          Z << 0.480;
        state = kalmanFilter.KalmanFilterIteration(state, Z, u);
        std::cout << "state\n" << state.state << std::endl <<
                  "P\n" << state.errorCovariance << std::endl << std::endl;
}

#endif //__KALMANFILTER_MAIN_CPP__


// Copyright (c) 2018-2019, University of Bremen, M. Farzalipour Tabriz
// Copyrights licensed under the 2-Clause BSD License.
// See the accompanying LICENSE.txt file for terms.

#pragma once
#include "arma_io.hpp"
#include "general_io.hpp"
#include "slabcc_consts.hpp"
#include "spline.hpp"
#include <armadillo>
#include <fftw3.h>

// These functions extend the functionality of the included Armadillo library by
// providing:
// (arma,iostream) << and >> operators for reading from iostreams to armadillo
// types and vice versa fft/ifft functions for cube files: Wrappers for FFTW
// with MATLAB/Octave scaling convention in FFT functions the forward FFT scales
// by 1, and the reverse scales by 1/N. 3D ndgrid and 3D meshgrid 3D spline
// interpolation 3D shift: by number of the elements along one axis or with a
// relative 3D shift vector irowvec to matrix/cube size conversion scalar triple
// product

// a (very inefficient) 3D spline interpolation!
arma::cube interp3(const arma::rowvec &x, const arma::rowvec &y,
                   const arma::rowvec &z, const arma::cube &v,
                   const arma::rowvec &xi, const arma::rowvec &yi,
                   const arma::rowvec &zi);
arma::cube interp3(const arma::cube &v, const arma::rowvec &xi,
                   const arma::rowvec &yi, const arma::rowvec &zi);

// Rectangular grid in 3D space
std::tuple<arma::cube, arma::cube, arma::cube>
ndgrid(const arma::rowvec &v1, const arma::rowvec &v2, const arma::rowvec &v3);

// 3D meshgrid
std::tuple<arma::cube, arma::cube, arma::cube> meshgrid(const arma::rowvec &v1,
                                                        const arma::rowvec &v2,
                                                        const arma::rowvec &v3);

// shifts a cube by a relative 3D vector [0 1]
arma::cube shift(arma::cube cube_in, arma::rowvec3 shifts);

// Planar average of a cube in the defined direction
// direction: 0,1,2 > x,y,z
arma::vec planar_average(const arma::uword &direction,
                         const arma::cube &cube_in);

// 1D FFT of complex data.
// no normalization for forward FFT
arma::cx_vec fft(arma::cx_vec X);

// 1D FFT of real data.
// no normalization for forward FFT
arma::cx_vec fft(arma::vec X);

// 3D FFT of complex data.
// no normalization for forward FFT
arma::cx_cube fft(arma::cx_cube X);

// 3D FFT of real data.
// no normalization for forward FFT
arma::cx_cube fft(arma::cube X);

// 1D inverse FFT of complex data.
// normalized by N = X.n_elem
arma::cx_vec ifft(arma::cx_vec X);

// 3D inverse FFT of complex data.
// normalized by N = X.n_elem
arma::cx_cube ifft(arma::cx_cube X);

// returns a cube size object from the values inside a vector
arma::SizeCube as_size(const arma::urowvec3 &vec);

// returns a matrix size object from the values inside a vector
arma::SizeMat as_size(const arma::urowvec2 &vec);

// element-wise fmod
arma::mat fmod(arma::mat mat_in, const double &denom) noexcept;

// element-wise positive fmod
arma::mat fmod_p(arma::mat mat_in, const double &denom) noexcept;

// positive fmod
double fmod_p(double num, const double &denom) noexcept;

// just a simple square! May cause overflows!!
inline double square(const double &input) noexcept { return input * input; }

// Poisson solver in 3D with anisotropic dielectric profiles
// diel is the N*3 matrix of variations in dielectric tensor elements in
// direction normal to the surface
arma::cx_cube poisson_solver_3D(const arma::cx_cube &rho, arma::mat diel,
                                arma::rowvec3 lengths,
                                arma::uword normal_direction);

// generate a copy of the cube with the elements cyclic-shifted by N positions
// along: dim=0: each row dim=1: each column dim=2: each slice
template <typename T>
arma::Cube<T> shift(const arma::Cube<T> &A, const arma::sword &N,
                    const arma::uword &dim) {
  arma::Cube<T> Shifted_A(arma::size(A));
  const auto index_init = arma::regspace<arma::uvec>(0, A.n_elem - 1);
  arma::imat sub_shift =
      arma::conv_to<arma::imat>::from(arma::ind2sub(arma::size(A), index_init));
  const arma::uword size = arma::size(A)(dim);
  sub_shift.row(dim).for_each([&N, &size](arma::sword &i) noexcept {
    i += N;
    while (i < 0)
      i += size;
    i = i % size;
  });

  Shifted_A(arma::sub2ind(arma::size(A),
                          arma::conv_to<arma::umat>::from(sub_shift))) =
      A(index_init);

  return Shifted_A;
}

// Undo a fftshift
template <typename T> arma::Row<T> ifftshift(const arma::Row<T> &A) {
  return arma::shift(A, -1 * (A.n_elem / 2));
}

// Undo a fftshift
template <typename T> arma::Cube<T> ifftshift(arma::Cube<T> A) {
  for (arma::uword i = 0; i < 3; ++i) {
    A = arma::shift(A, -1 * (arma::size(A)(i) / 2), i);
  }

  return A;
}

// returns the size of a cube as a rowvec
template <typename T> arma::urowvec3 SizeVec(const arma::Cube<T> &c) {
  const arma::SizeCube size = arma::size(c);
  return arma::urowvec({size(0), size(1), size(2)});
}

// returns the size of a matrix as a rowvec
template <typename T> arma::urowvec2 SizeVec(const arma::Mat<T> &c) {
  const arma::SizeMat size = arma::size(c);
  return arma::urowvec({size(0), size(1)});
}

// sign of the val as -1/0/+1
template <typename T> int sgn(T val) { return (T(0) < val) - (val < T(0)); }


//  Copyright (c) 2015 Boost.Test team
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE tolerance_03
#include <boost/test/included/unit_test.hpp>
namespace utf = boost::unit_test;

double x = 10.000000;
double d =  0.000001;

BOOST_AUTO_TEST_CASE(passing, * utf::tolerance(0.0001))
{
  BOOST_TEST(x == x + d); // equal with tolerance
  BOOST_TEST(x >= x + d); // ==> greater-or-equal

  BOOST_TEST(d == .0);    // small with tolerance
}

BOOST_AUTO_TEST_CASE(failing, * utf::tolerance(0.0001))
{
  BOOST_TEST(x - d <  x); // less, but still too close
  BOOST_TEST(x - d != x); // unequal but too close

  BOOST_TEST(d > .0);     // positive, but too small
  BOOST_TEST(d < .0);     // not sufficiently negative
}
//]


#include <Eigen/Dense>
#include <Spectra/SymEigsSolver.h>
#include <fstream>
#include <ios>
#include <iostream>
#include <random>

#include "edlib/Hamiltonians/TITFIsing.hpp"
#include <edlib/edlib.hpp>

int main(int argc, char* argv[])
{
    constexpr int N = 16;

    using namespace edlib;
    using UINT = uint32_t;

    std::cout << "#N: " << N << std::endl;

    std::vector<double> hs;
    for(int i = 0; i <= 20; i++)
    {
        hs.emplace_back(i * 0.1);
    }
    for(auto h : hs)
    {
        Eigen::VectorXd ev[2];
        {
            Basis1DZ2<UINT> basis(N, 0, 1, false);
            TITFIsing<UINT> ham(basis, 1.0, h);
            const int dim = basis.getDim();

            NodeMV mv(dim, 0, dim, ham);

            Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
            eigs.init();
            eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
            if(eigs.info() != Spectra::SUCCESSFUL)
                return 1;
            ev[0] = eigs.eigenvalues();
        }
        {
            Basis1DZ2<UINT> basis(N, 0, -1, false);
            TITFIsing<UINT> ham(basis, 1.0, h);
            const int dim = basis.getDim();

            NodeMV mv(dim, 0, dim, ham);

            Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
            eigs.init();
            eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
            if(eigs.info() != Spectra::SUCCESSFUL)
                return 1;
            ev[1] = eigs.eigenvalues();
        }

        printf("%f\t%.10f\t%.10f\t%.10f\t%.10f\n", h, ev[0](0), ev[0](1), ev[1](0), ev[1](1));
    }

    return 0;
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/core/lightweight_test.hpp>
#include <boost/gil/image_processing/hough_parameter.hpp>

namespace gil = boost::gil;

void from_step_count_test()
{
    const double middle_point = 0.5;
    const std::size_t step_count = 5;
    const double neighborhood = 1.0;
    auto param =
        gil::hough_parameter<double>::from_step_count(middle_point, neighborhood, step_count);
    BOOST_TEST(param.start_point == middle_point - neighborhood);
    BOOST_TEST(param.step_count == step_count * 2 + 1);
    BOOST_TEST(param.step_size == neighborhood / step_count);

    bool middle_point_occured = false;
    for (std::size_t i = 0; i < param.step_count; ++i)
    {
        auto current = param.start_point + param.step_size * i;
        if (current == middle_point)
        {
            middle_point_occured = true;
            break;
        }
    }
    BOOST_TEST(middle_point_occured);
}

void from_step_size_test(const double middle_point, const double step_size,
                         const double neighborhood)
{
    const std::size_t expected_step_count =
        static_cast<std::size_t>(neighborhood / step_size) * 2 + 1;
    auto param =
        gil::hough_parameter<double>::from_step_size(middle_point, neighborhood, step_size);
    BOOST_TEST(param.start_point == middle_point - step_size * std::floor(expected_step_count / 2));
    BOOST_TEST(param.step_count == expected_step_count);
    BOOST_TEST(param.step_size == step_size);

    bool middle_point_occured = false;
    for (std::size_t i = 0; i < param.step_count; ++i)
    {
        auto current = param.start_point + param.step_size * i;
        if (current == middle_point)
        {
            middle_point_occured = true;
            break;
        }
    }
    BOOST_TEST(middle_point_occured);
}

void minimum_step_angle_test(const std::ptrdiff_t width, const std::ptrdiff_t height)
{
    const auto bigger_dim = width > height ? width : height;
    const double expected_angle = std::atan2(1.0, bigger_dim);
    BOOST_TEST(expected_angle == gil::minimum_angle_step({width, height}));
}

int main()
{
    from_step_count_test();
    // ideal case
    from_step_size_test(2.0, 0.25, 1.0);
    from_step_size_test(5.0, 2, 5.0);
    minimum_step_angle_test(1200, 800);
    minimum_step_angle_test(800, 1200);
    return boost::report_errors();
}


/*
 * Copyright 2009-2012 Karsten Ahnert
 * Copyright 2009-2012 Mario Mulansky
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <cmath>

#include <boost/array.hpp>

template< size_t N >
struct phase_lattice
{
    typedef double value_type;
    typedef boost::array< value_type , N > state_type;

    value_type m_epsilon;
    state_type m_omega;

    phase_lattice() : m_epsilon( 6.0/(N*N) ) // should be < 8/N^2 to see phase locking
    {
        for( size_t i=1 ; i<N-1 ; ++i )
            m_omega[i] = m_epsilon*(N-i);
    }

    void inline operator()( const state_type &x , state_type &dxdt , const double t ) const
    {
        double c = 0.0;

        for( size_t i=0 ; i<N-1 ; ++i )
        {
            dxdt[i] = m_omega[i] + c;
            c = ( x[i+1] - x[i] );
            dxdt[i] += c;
        }

        //dxdt[N-1] = m_omega[N-1] + sin( x[N-1] - x[N-2] );
    }

};


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 */

// @sect3{Include files}

// 像往常一样，前几个include文件已经知道了，所以我们将不再评论它们。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/tensor.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// 在这个例子中，我们需要矢量值的有限元。对这些的支持可以在下面的include文件中找到。

#include <deal.II/fe/fe_system.h>

// 我们将用常规的Q1元素组成矢量值的有限元素，这些元素可以在这里找到，像往常一样。

#include <deal.II/fe/fe_q.h>

// 这又是C++语言。

#include <fstream>
#include <iostream>

// 最后一步和以前的程序一样。特别是，就像在 step-7 中一样，我们把这个程序所特有的一切都打包到一个自己的命名空间中。

namespace Step8
{
  using namespace dealii;
// @sect3{The ElasticProblem class template}

// 主类除了名称外，与 step-6 的例子相比几乎没有变化。

// 唯一的变化是为 fe 变量使用了一个不同的类。我们现在使用的不是FE_Q这样具体的有限元类，而是一个更通用的类，FESystem。事实上，FESystem本身并不是一个真正的有限元，因为它没有实现自己的形状函数。相反，它是一个可以用来将其他几个元素堆叠在一起形成一个矢量值的有限元的类。在我们的例子中，我们将组成 FE_Q(1) 对象的矢量值元素，如下所示，在这个类的构造函数中。

  template <int dim>
  class ElasticProblem
  {
  public:
    ElasticProblem();
    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim> triangulation;
    DoFHandler<dim>    dof_handler;

    FESystem<dim> fe;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;
  };
// @sect3{Right hand side values}

// 在进入主类的实现之前，我们声明并定义描述右手边的函数。这一次，右手边是向量值，解决方案也是如此，所以我们将更详细地描述为此所需的变化。

// 为了防止出现返回向量没有被设置成正确大小的情况，我们对这种情况进行了测试，否则将在函数的开始部分抛出一个异常。请注意，强制输出参数已经具有正确的大小是deal.II中的一个惯例，并且几乎在所有地方都强制执行。原因是，否则我们将不得不在函数开始时检查，并可能改变输出向量的大小。这很昂贵，而且几乎总是不必要的（对函数的第一次调用会将向量设置为正确的大小，随后的调用只需要做多余的检查）。此外，如果我们不能依赖向量已经具有正确大小的假设，那么检查和可能调整向量大小的操作是不能被删除的；这与Assert调用是一个契约，如果程序在优化模式下编译，Assert调用将被完全删除。

// 同样，如果由于某种意外，有人试图在只有一个空间维度的情况下编译和运行程序（在这种情况下，弹性方程没有什么意义，因为它们还原为普通的拉普拉斯方程），我们在第二个断言中终止程序。然而，该程序在三维空间中也能正常工作。

  template <int dim>
  void right_hand_side(const std::vector<Point<dim>> &points,
                       std::vector<Tensor<1, dim>> &  values)
  {
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));
    Assert(dim >= 2, ExcNotImplemented());

// 该函数的其余部分实现了计算力值。我们将使用一个位于(0.5,0)和(-0.5,0)点周围的两个小圆圈（或球体，在3D中）的X方向的恒定（单位）力，以及位于原点周围的Y方向的力；在3D中，这些中心的Z分量也是零。

// 为此，让我们首先定义两个对象，表示这些区域的中心。请注意，在构建点对象时，所有的分量都被设置为零。

    Point<dim> point_1, point_2;
    point_1(0) = 0.5;
    point_2(0) = -0.5;

    for (unsigned int point_n = 0; point_n < points.size(); ++point_n)
      {

// 如果 points[point_n] 处于围绕这些点之一的半径为0.2的圆（球）中，那么将X方向的力设置为1，否则为0。

        if (((points[point_n] - point_1).norm_square() < 0.2 * 0.2) ||
            ((points[point_n] - point_2).norm_square() < 0.2 * 0.2))
          values[point_n][0] = 1.0;
        else
          values[point_n][0] = 0.0;

// 同样地，如果 points[point_n] 在原点附近，那么将y力设置为1，否则为0。

        if (points[point_n].norm_square() < 0.2 * 0.2)
          values[point_n][1] = 1.0;
        else
          values[point_n][1] = 0.0;
      }
  }

//  @sect3{The ElasticProblem class implementation}
// @sect4{ElasticProblem::ElasticProblem constructor}

// 下面是主类的构造函数。如前所述，我们想构造一个由多个标量有限元组成的矢量值有限元（即，我们想构造矢量值元素，使其每个矢量成分都由一个标量元素的形状函数组成）。当然，我们想堆叠在一起的标量有限元的数量等于解函数的分量数量，由于我们考虑每个空间方向上的位移，所以是 dim 。FESystem类可以处理这个问题：我们传递给它我们想组成系统的有限元，以及它的重复频率。

  template <int dim>
  ElasticProblem<dim>::ElasticProblem()
    : dof_handler(triangulation)
    , fe(FE_Q<dim>(1), dim)
  {}

// 事实上，FESystem类还有几个构造函数，可以进行更复杂的操作，而不仅仅是将几个相同类型的标量有限元堆叠在一起；我们将在后面的例子中了解这些可能性。

//  @sect4{ElasticProblem::setup_system}

// 设置方程组与 step-6 例子中使用的函数相同。DoFHandler类和这里使用的所有其他类都完全知道我们要使用的有限元是矢量值的，并且照顾到了有限元本身的矢量值。(事实上，它们不知道，但这不需要困扰你：因为它们只需要知道每个顶点、直线和单元有多少个自由度，它们不问它们代表什么，也就是说，考虑的有限元是矢量值的，还是例如在每个顶点上有几个自由度的标量Hermite元)。

  template <int dim>
  void ElasticProblem<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    VectorTools::interpolate_boundary_values(dof_handler,
                                             0,
                                             Functions::ZeroFunction<dim>(dim),
                                             constraints);
    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler,
                                    dsp,
                                    constraints,
                                    /*keep_constrained_dofs =  */ false);

    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);
  }
// @sect4{ElasticProblem::assemble_system}

// 这个程序中最大的变化是创建矩阵和右手边，因为它们是取决于问题的。我们将一步一步地完成这个过程  step-  ，因为它比以前的例子要复杂一些。

// 然而，这个函数的前几部分和以前一样：设置一个合适的正交公式，为我们使用的（矢量值）有限元以及正交对象初始化一个FEValues对象，并声明了一些辅助数组。此外，我们还声明了永远相同的两个缩写。  n_q_points  和  dofs_per_cell  。每个单元的自由度数量，我们现在显然是从组成的有限元中询问，而不是从底层的标量Q1元中询问。在这里，它是 dim 乘以Q1元素的每个单元的自由度数，尽管这不是我们需要关心的明确知识。

  template <int dim>
  void ElasticProblem<dim>::assemble_system()
  {
    QGauss<dim> quadrature_formula(fe.degree + 1);

    FEValues<dim> fe_values(fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();
    const unsigned int n_q_points    = quadrature_formula.size();

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

// 正如前面的例子所示，我们需要一个地方来存储单元格上所有正交点的系数值。在目前的情况下，我们有两个系数，lambda和mu。

    std::vector<double> lambda_values(n_q_points);
    std::vector<double> mu_values(n_q_points);

// 好吧，我们也可以省略上面的两个数组，因为我们将对lambda和mu使用常数系数，可以这样声明。它们都代表函数总是返回常量值1.0。尽管我们可以在矩阵的组合中省略各自的系数，但为了演示，我们在这里使用它们。

    Functions::ConstantFunction<dim> lambda(1.), mu(1.);

// 和上面的两个常量函数一样，我们将在每个单元格中只调用一次函数right_hand_side，以使事情更简单。

    std::vector<Tensor<1, dim>> rhs_values(n_q_points);

// 现在我们可以开始对所有单元格进行循环。

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0;
        cell_rhs    = 0;

        fe_values.reinit(cell);

// 接下来我们得到正交点的系数值。同样，对于右手边也是如此。

        lambda.value_list(fe_values.get_quadrature_points(), lambda_values);
        mu.value_list(fe_values.get_quadrature_points(), mu_values);
        right_hand_side(fe_values.get_quadrature_points(), rhs_values);

// 然后将局部刚度矩阵的条目和右手边的向量组合起来。这几乎是一对一地遵循本例介绍中描述的模式。 在位的几个评论之一是，我们可以计算数字  comp(i)  ，即使用下面的  fe.system_to_component_index(i).first  函数调用形状函数  i  的唯一非零向量成分的索引。

//（通过访问 system_to_component_index 函数返回值的 first 变量，你可能已经猜到其中还有更多的内容。事实上，该函数返回一个 std::pair@<unsigned int，无符号int @>, ，其中第一个元素是 comp(i) ，第二个元素是介绍中也指出的值 base(i) ，即这个形状函数在这个组件中所有非零的形状函数中的索引，即介绍中的字典 base(i) 。不过，这不是我们通常感兴趣的数字）。)

// 有了这些知识，我们就可以把局部矩阵的贡献集合起来。

        for (const unsigned int i : fe_values.dof_indices())
          {
            const unsigned int component_i =
              fe.system_to_component_index(i).first;

            for (const unsigned int j : fe_values.dof_indices())
              {
                const unsigned int component_j =
                  fe.system_to_component_index(j).first;

                for (const unsigned int q_point :
                     fe_values.quadrature_point_indices())
                  {
                    cell_matrix(i, j) +=

// 第一个项是  $\lambda \partial_i u_i, \partial_j v_j) + (\mu \partial_i u_j, \partial_j v_i)$  。注意， shape_grad(i,q_point) 返回正交点q_point处第i个形状函数的唯一非零分量的梯度。梯度的分量 comp(i) 是第i个形状函数的唯一非零矢量分量相对于comp(i)th坐标的导数，由附加的括号访问。

                      (                                                  //
                        (fe_values.shape_grad(i, q_point)[component_i] * //
                         fe_values.shape_grad(j, q_point)[component_j] * //
                         lambda_values[q_point])                         //
                        +                                                //
                        (fe_values.shape_grad(i, q_point)[component_j] * //
                         fe_values.shape_grad(j, q_point)[component_i] * //
                         mu_values[q_point])                             //
                        +                                                //

// 第二个项是  $(\mu \nabla u_i, \nabla v_j)$  。我们不需要访问梯度的具体分量，因为我们只需要计算两个梯度的标量乘积，这个问题由<tt>operator*</tt>的重载版本来负责，就像前面的例子一样。                            注意，通过使用<tt>?:</tt>操作符，我们只在<tt>component_i</tt>等于<tt>component_j</tt>时才这样做，否则会加上一个零（编译器会将其优化掉）。

                        ((component_i == component_j) ?        //
                           (fe_values.shape_grad(i, q_point) * //
                            fe_values.shape_grad(j, q_point) * //
                            mu_values[q_point]) :              //
                           0)                                  //
                        ) *                                    //
                      fe_values.JxW(q_point);                  //
                  }
              }
          }

// 组装右手边也和介绍中讨论的一样。

        for (const unsigned int i : fe_values.dof_indices())
          {
            const unsigned int component_i =
              fe.system_to_component_index(i).first;

            for (const unsigned int q_point :
                 fe_values.quadrature_point_indices())
              cell_rhs(i) += fe_values.shape_value(i, q_point) *
                             rhs_values[q_point][component_i] *
                             fe_values.JxW(q_point);
          }

// 从局部自由度到全局矩阵和右手向量的转移不取决于所考虑的方程，因此与之前所有的例子相同。

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(
          cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
      }
  }

//  @sect4{ElasticProblem::solve}

// 解算器并不关心方程组的来源，只要它保持正定和对称（这是使用CG解算器的要求），而这个方程组确实是这样。因此，我们不需要改变任何东西。

  template <int dim>
  void ElasticProblem<dim>::solve()
  {
    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> cg(solver_control);

    PreconditionSSOR<SparseMatrix<double>> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve(system_matrix, solution, system_rhs, preconditioner);

    constraints.distribute(solution);
  }
// @sect4{ElasticProblem::refine_grid}

// 对网格进行细化的函数与 step-6 的例子相同。正交公式再次适应了线性元素。请注意，误差估计器默认情况下是将从有限元解的所有分量中得到的估计值相加，也就是说，它使用所有方向的位移，权重相同。如果我们希望网格只适应x方向的位移，我们可以给函数传递一个额外的参数，告诉它这样做，而不考虑其他所有方向的位移作为误差指标。然而，对于目前的问题，似乎应该考虑所有的位移分量，而且权重相同。

  template <int dim>
  void ElasticProblem<dim>::refine_grid()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate(dof_handler,
                                       QGauss<dim - 1>(fe.degree + 1),
                                       {},
                                       solution,
                                       estimated_error_per_cell);

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.03);

    triangulation.execute_coarsening_and_refinement();
  }
// @sect4{ElasticProblem::output_results}

// 输出的情况与之前的例子中已经显示过的差不多了。唯一的区别是，求解函数是矢量值的。DataOut类会自动处理这个问题，但我们必须给求解向量的每个分量一个不同的名字。

// 为了做到这一点， DataOut::add_vector() 函数想要一个字符串的向量。由于分量的数量与我们工作的维数相同，我们使用下面的 switch 语句。

// 我们注意到，一些图形程序对变量名称中允许的字符有限制。因此，deal.II只支持所有程序都支持的这些字符的最小子集。基本上，这些字符是字母、数字、下划线和其他一些字符，但特别是没有空格和减号/横线。否则该库将抛出一个异常，至少在调试模式下是这样。

// 在列出了1d、2d和3d的情况后，如果我们遇到一个我们没有考虑到的情况，让程序死亡是一种很好的风格。请记住，如果第一个参数中的条件没有得到满足，Assert宏会产生一个异常。当然，条件 false 永远不可能被满足，所以只要程序运行到默认语句，就会中止。

  template <int dim>
  void ElasticProblem<dim>::output_results(const unsigned int cycle) const
  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);

    std::vector<std::string> solution_names;
    switch (dim)
      {
        case 1:
          solution_names.emplace_back("displacement");
          break;
        case 2:
          solution_names.emplace_back("x_displacement");
          solution_names.emplace_back("y_displacement");
          break;
        case 3:
          solution_names.emplace_back("x_displacement");
          solution_names.emplace_back("y_displacement");
          solution_names.emplace_back("z_displacement");
          break;
        default:
          Assert(false, ExcNotImplemented());
      }

// 在为解向量的不同组成部分设置了名称之后，我们可以将解向量添加到计划输出的数据向量列表中。请注意，下面的函数需要一个字符串向量作为第二个参数，而我们在以前所有例子中使用的函数在那里接受一个字符串。(事实上，我们之前使用的函数会将单个字符串转换成只有一个元素的向量，并将其转发给另一个函数)。

    data_out.add_data_vector(solution, solution_names);
    data_out.build_patches();

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtk");
    data_out.write_vtk(output);
  }

//  @sect4{ElasticProblem::run}

//  run 函数所做的事情与 step-6 中的相同，比如说。这一次，我们使用平方[-1,1]^d作为域，在开始第一次迭代之前，我们在全局上对其进行了四次细化。

// 细化的原因有点意外：我们使用QGauss正交公式，在每个方向上有两个点用于整合右手边；这意味着每个单元上有四个正交点（在二维）。如果我们只对初始网格进行一次全局细化，那么在域上每个方向上就只有四个正交点。然而，右侧函数被选择为相当局部的，在这种情况下，纯属偶然，恰好所有的正交点都位于右侧函数为零的点上（用数学术语来说，正交点恰好在右侧函数的support之外的点上）。这样一来，用正交计算的右手向量将只包含零（尽管如果我们完全用积分计算右手向量的话，它当然会是非零的），方程组的解就是零向量，也就是一个处处为零的有限元函数。从某种意义上说，我们不应该对这种情况的发生感到惊讶，因为我们选择了一个完全不适合手头问题的初始网格。

// 不幸的是，如果离散解是常数，那么KellyErrorEstimator类计算的误差指标对每个单元来说也是零，对 Triangulation::refine_and_coarsen_fixed_number() 的调用将不会标记任何单元进行细化（如果每个单元的指示误差是零，为什么要这样做？因此，下一次迭代中的网格也将只由四个单元组成，同样的问题再次发生。

// 结论是：虽然我们当然不会把初始网格选择得非常适合问题的精确解决，但我们至少必须选择它，使它有机会捕捉到解决方案的重要特征。在这种情况下，它需要能够看到右手边的情况。因此，我们进行了四次全局细化。(任何更大的全局细化步骤当然也可以。)

  template <int dim>
  void ElasticProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 8; ++cycle)
      {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, -1, 1);
            triangulation.refine_global(4);
          }
        else
          refine_grid();

        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;

        setup_system();

        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;

        assemble_system();
        solve();
        output_results(cycle);
      }
  }
} // namespace Step8
// @sect3{The main function}

// 在上面最后一行关闭了 Step8 命名空间后，下面是程序的主要功能，又和 step-6 中一模一样（当然，除了改变了类名）。

int main()
{
  try
    {
      Step8::ElasticProblem<2> elastic_problem_2d;
      elastic_problem_2d.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <vector>

#include <NTL/ZZ.h>

#include "../src/elements/element.hpp"
#include "../src/algorithms/utils.hpp"

/* Example: using the remainder tree algorithm to search for Wilson primes.
 * This was the first application for the remainder tree algorithm and also is
 * one of the simplest. Wilson's theorem says that for all primes p, the following is true:
 *(p-1)! = -1 (mod p). A Wilson prime satisfies the same identity modulo p^2.
 *
 * Let A_0 = A_1 = 1, and A_n = n-1. Also set m_n to be n if n is prime, and 1 otherwise.
 * Then, if A_0 * A_1 * ... A_p = -1 (mod m_n), then m_n is a Wilson prime.
 */

using std::vector;

/* The first decision to make is what kind of datatype to use. In this case it is clear that we only
 * need to use integers. However, for many applications such as those in polynomial rings or if the
 * recurrence relation of the dividends is of order higher than 1, other datatypes may be desired.
 * Most of the code was written with using NTL in mind. However, we wrap every value in the type Elt.
 * Elt is a templated class object which will forward constructors, operators, etc. to its underlying type.
 * To specialize it for a new type (for example, to optimize its .mulmod() method), create a new file in
 * src/elements and then include it in elt_custom.tpp
 * I recommend specialized the methods rather than the class itself. Take a look at elt_NTL.tpp for an example.
 */
using NTL::ZZ;

//The convention here is to generate from lower bound---inclusive to upper---exclusive
vector<Elt<ZZ>> gen_n(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; ++i) {
        if(i <= 1){
            output[i] = Elt<ZZ>(1);
        }
        else {
            output[i-lower] = Elt<ZZ> (i-1);
        }
    }
    return output;
}

vector<Elt<ZZ>> gen_second_prime_power(long lower, long upper) {
    vector<Elt<ZZ>> output(upper-lower);

    for(long i = lower; i < upper; i++){
        ZZ n(i);
        if(ProbPrime(n)) { //Technically a sieve is faster & more correct, but shouldn't make a big difference.
                            //This is just an example anyway.
            power(n, n, 2);
            output[i-lower] = Elt<ZZ>(n);
        }
        else{
            output[i-lower] = Elt<ZZ>(1);
        }
    }
    return output;
}

//TODO: explain how to modify calculate_factorial and compute V


/*
 *  testOrientedVoxels.cpp
 *  Trogdor6
 *
 *  Created by Paul Hansen on 6/29/10.
 *  Copyright 2010 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestVoxelizer

#include <boost/test/unit_test.hpp>

#include "TensorGridConstants.h"

#include <iomanip>

using namespace std;
using namespace RLE;
using namespace YeeUtilities;

BOOST_AUTO_TEST_CASE( EffectivePermittivity )
{
    double tolerance;
    if (sizeof(Precision::Float) == sizeof(float))
        tolerance = 1e-5;
    else
        tolerance = 1e-9;
    vector<Precision::RationalFunction> materials(2);
    materials[0] = 1.0;
    materials[1] = 2.0;

    vector<DynamicRLE3<double> > fillFactors(2);
    fillFactors[0].mark(0,0,0,0.5);
    fillFactors[1].mark(0,0,0,0.5);

    DynamicRLE3<Precision::RationalFunction> inversePermittivity;

    // Parameters just to fill space in the TensorGridConstants constructor
    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES, Rect3i(0,0,0,0,0,0));

    // Test normal-to-surface diagonal component:
    //  permittivity is harmonic mean
    //  permittivity is of the form 1/a0, so the numerator of 1/eps is a0,
    //   or 1/(harmonic mean).  The harmonic mean of 1 and 2 is 4/3, so a0 is
    //   0.75.
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,1.0);
        inversePermittivity = tgc.diagonalElement(materials, fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 0.75, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test parallel-to-surface diagonal component:
    //  permittivity is arithmetic mean
    //  1/a0 = 1.5, so a0 = 2/3
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,0.0);
        inversePermittivity = tgc.diagonalElement(materials, fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 2.0/3, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test off-diagonal component for diagonal orientation tensor:
    // should be zero.
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,0.0);
        inversePermittivity = tgc.offDiagonalElement(materials, fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 0.0, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test a nonzero off-diagonal of the tensor.
    {
        double harmonicMean = 4.0/3;
        double arithmeticMean = 1.5;
        double orientation_ij = 0.5;

        double epsInv = orientation_ij*(1.0/harmonicMean - 1.0/arithmeticMean);

        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0, orientation_ij);
        inversePermittivity = tgc.offDiagonalElement(materials,
            fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], epsInv, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }
}

BOOST_AUTO_TEST_CASE( BackgroundMaterial )
{
    double tolerance;
    if (sizeof(Precision::Float) == sizeof(float))
        tolerance = 1e-5;
    else
        tolerance = 1e-9;
    vector<Precision::RationalFunction> materials(1);
    materials[0] = 1.0;
//    materials[1] = 2.0;

    vector<DynamicRLE3<double> > fillFactors(1);
    fillFactors[0].mark(0,0,0,0.5);
//    fillFactors[1].mark(0,0,0,0.5);

    DynamicRLE3<Precision::RationalFunction> inversePermittivity;

    // Parameters just to fill space in the TensorGridConstants constructor
    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES,
        Rect3i(0,0,0,0,0,0),
        Precision::RationalFunction(2.0));

    // Test normal-to-surface diagonal component:
    //  permittivity is harmonic mean
    //  permittivity is of the form 1/a0, so the numerator of 1/eps is a0,
    //   or 1/(harmonic mean).  The harmonic mean of 1 and 2 is 4/3, so a0 is
    //   0.75.
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,1.0);
        inversePermittivity = tgc.diagonalElement(materials,
            fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 0.75, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test parallel-to-surface diagonal component:
    //  permittivity is arithmetic mean
    //  1/a0 = 1.5, so a0 = 2/3
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,0.0);
        inversePermittivity = tgc.diagonalElement(materials,
            fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 2.0/3, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test off-diagonal component for diagonal orientation tensor:
    // should be zero.
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,0.0);
        inversePermittivity = tgc.offDiagonalElement(materials, fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], 0.0, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }

    // Test a nonzero off-diagonal of the tensor.
    {
        double harmonicMean = 4.0/3;
        double arithmeticMean = 1.5;
        double orientation_ij = 0.5;

        double epsInv = orientation_ij*(1.0/harmonicMean - 1.0/arithmeticMean);

        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0, orientation_ij);
        inversePermittivity = tgc.offDiagonalElement(materials, fillFactors, orientation);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).numerator().order(), 0);
        BOOST_CHECK_EQUAL(inversePermittivity.at(0,0,0).denominator().order(), 0);

        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).numerator()[0], epsInv, tolerance);
        BOOST_CHECK_CLOSE(inversePermittivity.at(0,0,0).denominator()[0], 1.0, tolerance);
    }
}

BOOST_AUTO_TEST_CASE( Sensitivity_FillFactor_Analytical )
{
    double eps1 = 1.0, eps2 = 2.0;
    double fill1 = 0.5, fill2 = 1.0 - fill1;
    vector<Precision::RationalFunction> materials(2);
    materials[0] = eps1;
    materials[1] = eps2;

    vector<DynamicRLE3<double> > fillFactors(2);
    fillFactors[0].mark(0,0,0, fill1);
    fillFactors[1].mark(0,0,0, fill2);

    DynamicRLE3<Precision::RationalFunction> epsInv;
    DynamicRLE3<Precision::RationalFunction> DepsInv;

    vector<DynamicRLE3<double> > dFillFactors(2);
    dFillFactors[0].mark(0,0,0, 1.0);
    dFillFactors[1].mark(0,0,0, -1.0);
    DynamicRLE3<double> dOrientation; // all zeros!

    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES, Rect3i(0,0,0,0,0,0));

    // Test normal-to-surface diagonal component:
    //  permittivity is harmonic mean
    //  permittivity is of the form 1/a0, so the numerator of 1/eps is a0,
    //   or 1/(harmonic mean).  The harmonic mean of 1 and 2 is 4/3, so a0 is
    //   0.75.
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,1.0);
        epsInv = tgc.diagonalElement(materials,
            fillFactors, orientation);
        DepsInv = *tgc.diagonalSensitivity(materials,
            fillFactors, orientation, dFillFactors, dOrientation);
        double Da0 = 1/eps1 - 1/eps2; // derive it yerself!

        BOOST_CHECK_CLOSE(DepsInv.at(0,0,0).numerator()[0], Da0, 1); // to 1%
    }

    // Test parallel-to-surface diagonal component:
    //  permittivity is arithmetic mean
    //  1/a0 = 1.5, so a0 = 2/3
    {
        DynamicRLE3<double> orientation;
        orientation.mark(0,0,0,0.0);
        epsInv = tgc.diagonalElement(materials,
            fillFactors, orientation);
        DepsInv = *tgc.diagonalSensitivity(materials,
            fillFactors, orientation, dFillFactors, dOrientation);
        double meanEps = fill1*eps1 + fill2*eps2;
        double Da0 = (eps2-eps1)/meanEps/meanEps; // derive it yerself!

        BOOST_CHECK_CLOSE(DepsInv.at(0,0,0).numerator()[0], Da0, 1); // to 1%
    }
}

// Test sensitivity to variation of fill factors.
BOOST_AUTO_TEST_CASE( Sensitivity_FillFactor_FiniteDifference )
{
    vector<Precision::RationalFunction> materials(2);
    materials[0] = 1.0;
    materials[1] = Precision::RationalFunction(
        Precision::Polynomial(10.0, -20.0, 30.0),
        Precision::Polynomial(11.0, -22.0, 33.0) );

    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES, Rect3i(0,0,0,0,0,0));

//    typedef numeric_limits<Precision::Float> f;
    const double DELTA = 0.01;
    //const double DELTA = sqrt(f::epsilon());
    const double TOLERANCE = 10*DELTA;
//    cerr << "Using DELTA = " << DELTA << ", TOLERANCE = " << TOLERANCE << "\n";

    vector<DynamicRLE3<double> > fillFactors1(2), fillFactors2(2);
    fillFactors1[0].mark(0,0,0,0.5);
    fillFactors1[1].mark(0,0,0,0.5);
    fillFactors2[0] = fillFactors1[0] + DELTA;
    fillFactors2[1] = fillFactors1[1] - DELTA;
    vector<DynamicRLE3<double> > dFillFactors(2);
    dFillFactors[0] = (fillFactors2[0] - fillFactors1[0])/DELTA/2;
    dFillFactors[1] = (fillFactors2[1] - fillFactors1[1])/DELTA/2;

    DynamicRLE3<double> orientation;
    DynamicRLE3<double> dOrientation; // empty
    orientation.mark(0,0,0, 0.4); // some arbitrary value.

    DynamicRLE3<Precision::RationalFunction> epsInv1, epsInv2;
    DynamicRLE3<Precision::RationalFunction> epsInvSensitivity;

    epsInv1 = tgc.diagonalElement(materials, fillFactors1, orientation);
    epsInv2 = tgc.diagonalElement(materials, fillFactors2, orientation);
    epsInvSensitivity = *tgc.diagonalSensitivity(materials, fillFactors1, orientation, dFillFactors, dOrientation);

    TensorGridConstants::Differential diff(2*DELTA);

    if (0)
    {
        cerr << "eps1:\n" << epsInv1 << "\n";
        cerr << "eps2:\n" << epsInv2 << "\n";
        cerr << "eps sensitivity: " << epsInvSensitivity << "\n";
        cerr << "by differential:\n" << diff(epsInv2.at(0,0,0), epsInv1.at(0,0,0))
            << "\n";
    }

    double deltaNumer = epsInv2.at(0,0,0).numerator()[0] -
        epsInv1.at(0,0,0).numerator()[0];
    double deltaDenom = epsInv2.at(0,0,0).denominator()[0] -
        epsInv1.at(0,0,0).denominator()[0];
    double dNumer = epsInvSensitivity.at(0,0,0).numerator()[0];
    double dDenom = epsInvSensitivity.at(0,0,0).denominator()[0];

    BOOST_CHECK_SMALL(dDenom, TOLERANCE*100); // technically zero
    BOOST_CHECK_SMALL(deltaDenom, TOLERANCE*100); // technically zero
    BOOST_CHECK_CLOSE(dNumer, deltaNumer/DELTA/2, 1.0); // to within 1% is ok
}

BOOST_AUTO_TEST_CASE( Sensitivity_Orientation_FiniteDifference )
{
    const int WHATEVER = 0;
    DynamicRLE3<Precision::RationalFunction> WHO_CARES;
    TensorGridConstants tgc(WHATEVER, WHATEVER, WHO_CARES, Rect3i(0,0,0,0,0,0));

    vector<Precision::RationalFunction> materials(2);
    materials[0] = 1.0;
    materials[1] = 2.0;

    typedef numeric_limits<Precision::Float> f;
    const double DELTA = sqrt(f::epsilon());
    const double TOLERANCE = 10*DELTA;
    BOOST_TEST_MESSAGE("Using DELTA = " << DELTA << ", TOLERANCE = " << TOLERANCE);

    vector<DynamicRLE3<double> > fillFactors(2);
    vector<DynamicRLE3<double> > deltaFillFactors(2);
    fillFactors[0].mark(0,0,0,0.5);
    fillFactors[1].mark(0,0,0,0.5);

    DynamicRLE3<double> orientation1, orientation2;
    orientation1.mark(0,0,0, 0.4); // some arbitrary value.
    orientation2 = orientation1 + DELTA;
    DynamicRLE3<double> dOrientation = (orientation2 - orientation1)/DELTA;

    DynamicRLE3<Precision::RationalFunction> epsInv1, epsInv2;
    DynamicRLE3<Precision::RationalFunction> epsInvSensitivity;

    epsInv1 = tgc.diagonalElement(materials, fillFactors,
        orientation1);
    epsInv2 = tgc.diagonalElement(materials, fillFactors,
        orientation2);
    epsInvSensitivity = *tgc.diagonalSensitivity(materials,
        fillFactors, orientation1, deltaFillFactors, dOrientation);

//    cout << "eps1:\n" << epsInv1 << "\n";
//    cout << "eps2:\n" << epsInv2 << "\n";
//    cout << "eps sensitivity: " << epsInvSensitivity << "\n";
//    cout << "diff:\n" << epsInv2 - epsInv1 << "\n";

    double deltaNumer = epsInv2.at(0,0,0).numerator()[0] -
        epsInv1.at(0,0,0).numerator()[0];
    double deltaDenom = epsInv2.at(0,0,0).denominator()[0] -
        epsInv1.at(0,0,0).denominator()[0];
    double dNumer = epsInvSensitivity.at(0,0,0).numerator()[0];
    double dDenom = epsInvSensitivity.at(0,0,0).denominator()[0];

    BOOST_CHECK_SMALL(dDenom, TOLERANCE*100); // technically zero; tolerance %
    BOOST_CHECK_SMALL(deltaDenom, TOLERANCE*100); // technically zero
    BOOST_CHECK_CLOSE(dNumer, deltaNumer/DELTA, TOLERANCE*100); // within 0.01%
}


#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <aikido/common/PseudoInverse.hpp>

using namespace aikido::common;

TEST(PseudoInverse, Invertible)
{
  Eigen::Matrix2d mat;
  mat.setIdentity();
  Eigen::Matrix2d inverse = pseudoinverse(mat);

  EXPECT_TRUE((inverse * mat).isApprox(Eigen::Matrix2d::Identity()));
}

TEST(PseudoInverse, Vector)
{
  Eigen::Vector2d vec(1, 1);

  Eigen::MatrixXd inverse = pseudoinverse(vec);

  EXPECT_DOUBLE_EQ((inverse * vec)(0, 0), 1);
}

TEST(PseudoInverse, Matrix)
{
  Eigen::MatrixXd mat(Eigen::MatrixXd::Random(3, 4));
  Eigen::MatrixXd inverse = pseudoinverse(mat);

  EXPECT_TRUE((mat * inverse).isApprox(Eigen::Matrix3d::Identity()));
}