#pragma once

#include <Geometry/LineSegment.hpp>

#include <opencv2/core/core.hpp>
#include <Eigen/Dense>

#include <numeric>
#include <vector>

namespace pcv
{

/**
 * Utility function to help determine "leftness" of a point compared to the line
 * segment drawn between the first two points. Leftness is indicated by a
 * negative return value, which is why the tempalate parameter must be signed.
 * Idea from Computational Geometry in C by Joseph O'Rourke.
 */
template<class SignedNumericType>
SignedNumericType getTwiceSignedArea2D(
    const Eigen::Matrix<SignedNumericType, 2, 1> &endpoint0,
    const Eigen::Matrix<SignedNumericType, 2, 1> &endpoint1,
    const Eigen::Matrix<SignedNumericType, 2, 1> &testpoint)
{
    static_assert(std::is_signed<SignedNumericType>::value);

    // Aliases for more readable arithmetic.
    const auto &a = endpoint0;
    const auto &b = endpoint1;
    const auto &c = testpoint;

    return ((b.x() - a.x()) * (c.y() - a.y()))
         - ((c.x() - a.x()) * (b.y() - a.y()));
}

/**
 * \class ConvexPolygon
 * Create a polygon by specifying its vertices in a clockwise direction. Points
 * are 2D Eigen::Vectors.
 */
template<class NumericType>
class ConvexPolygon
{
public:
    ConvexPolygon() = default;

    ConvexPolygon(const ConvexPolygon &polygon) = default;

    /**
     * Move construction leverages std::vector's move constructor.
     */
    ConvexPolygon(ConvexPolygon &&polygon) = default;

    ~ConvexPolygon() = default;

    ConvexPolygon(const std::vector<Eigen::Matrix<NumericType, 2, 1>> &vertices) : vertices(vertices)
    {}

    ConvexPolygon(std::vector<Eigen::Matrix<NumericType, 2, 1>> &&vertices) : vertices(vertices)
    {}

    void addVertex(Eigen::Matrix<NumericType, 2, 1> &&vertex)
    {
        vertices.emplace_back(vertex);
    }

    void addVertex(const Eigen::Matrix<NumericType, 2, 1> &vertex)
    {
        vertices.push_back(vertex);
    }

    std::vector<Eigen::Matrix<NumericType, 2, 1>> getVertices() const
    {
        return vertices;
    }

    /**
     * Determine if a given point is inside or outside the polygon in O(n) where
     * n is the number of vertices of the polygon. This function requires a
     * point to be represented as two doubles, even if the points can be
     * represented with integers or the polygon has a non-floating point
     * templated NumericType. The current implementation does not account for
     * floating-point error.
     */
    bool isPointContained(Eigen::Vector2d testPoint) const;

    /**
     * Constructs a ConvexPolygon representing the minimum size rectangle with
     * sides perpendicular and parallel to the x-axis that completely encompasses
     * all points in the polygon.
     */
    ConvexPolygon<NumericType> getBoundingBox() const;

    /**
     * Transforms the polygon in-place with a 3x3 homographic transformation
     * matrix. Can only transform floating-point vertex definitions of polygons.
     * This method is not thread-safe.
     */
    void transform(const Eigen::Matrix3d &transform);

    /**
     * Get the ConvexPolygon that bounds the intersection of the polygons. The
     * public interface makes no runtime performance guarantee!
     */
    ConvexPolygon<double> getIntersectionWith(
            const ConvexPolygon<NumericType> &rhsPoly) const;

private:
    std::vector<Eigen::Matrix<NumericType, 2, 1>> vertices;
};

/**
 * Need to use another template parameter for rhsPoly's numeric type.
 */
template<class NumericType>
ConvexPolygon<double> ConvexPolygon<NumericType>::getIntersectionWith(
        const ConvexPolygon<NumericType> &rhsPoly) const
{
    const auto rhsVertices = rhsPoly.getVertices();
    auto rhsPrevVertex = rhsVertices.back();
    auto lhsPrevVertex = vertices.back();

    ConvexPolygon<double> intersection(
        std::accumulate(
            std::cbegin(vertices),
            std::cend(vertices),
            std::vector<Eigen::Vector2d>{},
            [&lhsPrevVertex, &rhsPrevVertex, &rhsVertices](
                std::vector<Eigen::Vector2d> accumulator,
                decltype(lhsPrevVertex) lhsVertexIter)
            {
                for (const auto &rhsVertexIter : rhsVertices)
                {
                    auto intersection = getSegmentIntersectionPoint2(
                        lhsPrevVertex, lhsVertexIter, rhsPrevVertex, rhsVertexIter);
                    if (intersection)
                    {
                        accumulator.emplace_back(*intersection);
                    }
                    rhsPrevVertex = rhsVertexIter;
                }

                lhsPrevVertex = lhsVertexIter;
                return accumulator;
            }));

    return intersection;
}

template<class NumericType>
bool ConvexPolygon<NumericType>::isPointContained(
    Eigen::Vector2d testPoint) const
{
    auto cachedVertex = vertices.back();

    return std::accumulate(
        std::cbegin(vertices),
        std::cend(vertices),
        true,
        [&cachedVertex, testPoint](bool isContained, decltype(cachedVertex) vertexIter)
        {
            isContained &=
                getTwiceSignedArea2D(
                    static_cast<Eigen::Vector2d>(cachedVertex.template cast<double>()),
                    static_cast<Eigen::Vector2d>(vertexIter.template cast<double>()),
                    testPoint) <= 0;

            cachedVertex = vertexIter;
            return isContained;
        });
}

template<class NumericType>
ConvexPolygon<NumericType> ConvexPolygon<NumericType>::getBoundingBox() const
{
    NumericType minX = vertices[0].x();
    NumericType minY = vertices[0].y();
    NumericType maxX = vertices[0].x();
    NumericType maxY = vertices[0].y();

    // Start iterator one after the beginning.
    const auto vertex = vertices.cbegin();
    std::advance(vertices, 1);
    for(; vertex != std::cend(vertex); std::next(vertex))
    {
        minX = std::min(vertex.x(), minX);
        minY = std::min(vertex.y(), minY);
        maxX = std::max(vertex.x(), maxX);
        maxY = std::max(vertex.y(), maxY);
    }

    // This is weird, the size is not dynamic, it's 2 because the points are 2D.
    ConvexPolygon<NumericType> output;
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        minX, minY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        minX, maxY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        maxX, maxY});
    output.addVertex(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>{
        maxX, minY});

    return output;
}

/**
 * An O(n) operation where n is the max of the two polygon's number of vertices.
 */
template<class NumericType>
bool operator==(
    const ConvexPolygon<NumericType> &lhs,
    const ConvexPolygon<NumericType> &rhs)
{
    return lhs.getVertices() == rhs.getVertices();
}

template<class NumericType>
Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>
makePolygonIntersectionEigenGrid(
    const std::vector<ConvexPolygon<NumericType>> &polygons,
    std::size_t gridRows,
    std::size_t gridCols)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have numerical polygon type.");

    Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> mask =
        Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>::Zero(gridRows, gridCols);

    for (std::size_t r = 0; r < gridRows; ++r)
    {
        for (std::size_t c = 0; c < gridCols; ++c)
        {
            bool containment = true;
            for (const auto &polygon : polygons)
            {
                containment &= polygon.isPointContained(Eigen::Vector2d{c, gridRows-r-1});
            }
            mask(r, c) = containment;
        }
    }

    return mask;
}

template<class PolygonNumericType, class MatNumericType>
void makePolygonIntersectionOpencvGrid(
    const std::vector<ConvexPolygon<PolygonNumericType>> &polygons,
    std::size_t gridRows,
    std::size_t gridCols,
    cv::Mat_<MatNumericType> &output)
{
    cv::Mat_<MatNumericType> mask(gridRows, gridCols);

    for (std::size_t r = 0; r < gridRows; ++r)
    {
        for (std::size_t c = 0; c < gridCols; ++c)
        {
            bool containment = true;
            for (const auto &polygon : polygons)
            {
                containment &= polygon.isPointContained(Eigen::Vector2d{c, gridRows-r-1});
            }
            mask(r, c) = static_cast<MatNumericType>(containment);
        }
    }

    output = mask;
}

template<class NumericType>
inline void ConvexPolygon<NumericType>::transform(const Eigen::Matrix3d &transformMatrix)
{
    static_assert(std::is_floating_point<NumericType>::value,
                  "Can only perform in-place transformations with floating-point"
                  " polygons.");

    const auto &tform = transformMatrix;
    std::transform(
        std::begin(vertices), std::end(vertices), std::begin(vertices),
        [tform](decltype(vertices[0]) vertex)
        {
            Eigen::Matrix<NumericType, 3, 1> homogenousVertex;
            homogenousVertex << vertex[0], vertex[1], 1;

            Eigen::Matrix<NumericType, 3, 1> homogenousNewVertex =
                tform * homogenousVertex;

            Eigen::Matrix<NumericType, 2, 1> newVertex;
            newVertex << homogenousNewVertex[0], homogenousNewVertex[1];

            // Bring back to pixel coordinates.
            newVertex /= homogenousNewVertex[2];
            return newVertex;
        });
}

} // end namespace pcv


// 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__INTERNAL__C1_HPP_
#define SMOOTH__INTERNAL__C1_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief C(1) Lie group of rotation and scaling
 *
 * Elements are on the form \f$ k R \f$ where \f$ k > 0 \f$ is a scalar and \f$ R \in SO(2) \f$.
 *
 * Memory layout
 * -------------
 * Group:    a b
 * Tangent:  s Ωz
 *
 * Lie group Matrix form
 * ---------------------
 * [ b -a ]
 * [ a  b ]
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ s -Ωz ]
 * [ Ωz  s ]
 *
 * Constraints
 * -----------
 * Group:   a * a - b * b > 0
 * Tangent: -pi < Ωz <= pi
 */
template<typename _Scalar>
class C1Impl
{
public:
  using Scalar = _Scalar;

  static constexpr Eigen::Index RepSize = 2;
  static constexpr Eigen::Index Dim     = 2;
  static constexpr Eigen::Index Dof     = 2;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out << Scalar(0), Scalar(1); }

  static void setRandom(GRefOut g_out)
  {
    using std::sin, std::cos;

    const Scalar u = Eigen::internal::template random_impl<Scalar>::run(0, 2 * M_PI);
    const Scalar t = Eigen::internal::template random_impl<Scalar>::run(0.01, 100);
    g_out << t * sin(u), t * cos(u);
  }

  static void matrix(GRefIn g_in, MRefOut m_out) { m_out << g_in(1), -g_in(0), g_in(0), g_in(1); }

  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out)
  {
    g_out << g_in1[0] * g_in2[1] + g_in1[1] * g_in2[0], g_in1[1] * g_in2[1] - g_in1[0] * g_in2[0];
  }

  static void inverse(GRefIn g_in, GRefOut g_out)
  {
    const Scalar t = g_in[0] * g_in[0] + g_in[1] * g_in[1];
    g_out << -g_in[0] / t, g_in[1] / t;
  }

  static void log(GRefIn g_in, TRefOut a_out)
  {
    using std::atan2, std::sqrt, std::log;

    const Scalar t = sqrt(g_in[0] * g_in[0] + g_in[1] * g_in[1]);
    a_out << log(t), atan2(g_in[0], g_in[1]);
  }

  static void Ad(GRefIn, TMapRefOut A_out) { A_out.setIdentity(); }

  static void exp(TRefIn a_in, GRefOut g_out)
  {
    using std::cos, std::exp, std::sin;

    const Scalar t = exp(a_in.x());
    g_out << t * sin(a_in.y()), t * cos(a_in.y());
  }

  static void hat(TRefIn a_in, MRefOut A_out) { A_out << a_in(0), -a_in(1), a_in(1), a_in(0); }

  static void vee(MRefIn A_in, TRefOut a_out)
  {
    a_out << (A_in(0, 0) + A_in(1, 1)) / Scalar(2), (A_in(1, 0) - A_in(0, 1)) / Scalar(2);
  }

  static void ad(TRefIn, TMapRefOut A_out) { A_out.setZero(); }

  static void dr_exp(TRefIn, TMapRefOut A_out) { A_out.setIdentity(); }

  static void dr_expinv(TRefIn, TMapRefOut A_out) { A_out.setIdentity(); }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__C1_HPP_


//
// Created by matt on 21/01/2021.
//

#include "simple_kf.h"

#include <iostream>

#include <Eigen/Core>
#include <Eigen/LU>

namespace kf0 {

KalmanFilter::KalmanFilter() {
  // State, position (x, y) and velocity (x, y)
  x_.setZero();
  // Initialise covariance with high values
  P_.setZero();
  P_.diagonal() = Eigen::Vector4d::Constant(100.0);
  // Process model
  // v' = v
  // p' = p + v*dt
  F_.setIdentity();
  F_(0, 2) = dt; // px' = px + vx*dt
  F_(1, 3) = dt; // py' = py + vy*dt
  // Process noise
  Q_ = Eigen::Matrix<double, 4, 4>::Identity() * 2.0; // TODO placeholder
  // Measurement model
  // Measure position
  H_.setZero();
  H_(0, 0) = 1.0;
  H_(1, 1) = 1.0;
  // Measurement noise
  R_.setIdentity(); // TODO placeholder

  //    std::cout << "F: " << '\n';
  //    std::cout << F_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << '\n';
  //    std::cout << "Q: " << '\n';
  //    std::cout << Q_ << '\n';
  //    std::cout << "H: " << '\n';
  //    std::cout << H_ << '\n';
  //    std::cout << "R: " << '\n';
  //    std::cout << R_ << "\n\n";
}

Eigen::Matrix<double, 4, 1> KalmanFilter::Predict() {
  x_ = F_ * x_;
  P_ = F_ * P_ * F_.transpose() + Q_;

  //    std::cout << "Predict\n";
  //    std::cout << "F: " << '\n';
  //    std::cout << F_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << "\n\n";
  return x_;
}

Eigen::Matrix<double, 4, 1>
KalmanFilter::Update(const Eigen::Matrix<double, 2, 1> &z) {
  z_hat_ = H_ * x_;
  y_ = z - z_hat_;
  S_ = H_ * P_ * H_.transpose() + R_;
  K_ = P_ * H_.transpose() * S_.inverse();
  x_ = x_ + K_ * y_;
  P_ = (Eigen::Matrix4d::Identity() - K_ * H_) * P_;

  //    std::cout << "Update\n";
  //    std::cout << "z: " << '\n';
  //    std::cout << z.transpose() << '\n';
  //    std::cout << "z_hat: " << '\n';
  //    std::cout << z_hat_.transpose() << '\n';
  //    std::cout << "y: " << '\n';
  //    std::cout << y_.transpose() << '\n';
  //    std::cout << "S: " << '\n';
  //    std::cout << S_ << '\n';
  //    std::cout << "K: " << '\n';
  //    std::cout << K_ << '\n';
  //    std::cout << "x: " << '\n';
  //    std::cout << x_.transpose() << '\n';
  //    std::cout << "P: " << '\n';
  //    std::cout << P_ << "\n\n";

  return x_;
}

} // namespace kf0


// 1-d PIC code to solve plasma two-stream instability problem.

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <blitz/array.h>
#include <fftw.h>

using namespace blitz;

void Output (char* fn1, char* fn2, double t,
	     Array<double,1> r, Array<double,1> v);
void Density (Array<double,1> r, Array<double,1>& n);
void Electric (Array<double,1> phi, Array<double,1>& E);
void Poisson1D (Array<double,1>& u, Array<double,1> v, double kappa);
void rk4_fixed (double& x, Array<double,1>& y,
              void (*rhs_eval)(double, Array<double,1>, Array<double,1>&),
              double h);
void rhs_eval (double t, Array<double,1> y, Array<double,1>& dydt);
void Load (Array<double,1> r, Array<double,1> v, Array<double,1>& y);
void UnLoad (Array<double,1> y, Array<double,1>& r, Array<double,1>& v);
double distribution (double vb);

double L; int N, J;

int main()
{
  // Parameters
  L;            // Domain of solution 0 <= x <= L (in Debye lengths)
  N;            // Number of electrons
  J;            // Number of grid points
  double vb;    // Beam velocity
  double dt;    // Time-step (in inverse plasma frequencies)
  double tmax;  // Simulation run from t = 0. to t = tmax

  // Get parameters
  printf ("Please input N:  "); scanf ("%d", &N);
  printf ("Please input vb:  "); scanf ("%lf", &vb);
  printf ("Please input L:  "); scanf ("%lf", &L);
  printf ("Please input J:  "); scanf ("%d", &J);
  printf ("Please input dt:  "); scanf ("%lf", &dt);
  printf ("Please input tmax:  "); scanf ("%lf", &tmax);
  int skip = int (tmax / dt) / 10;
  if ((N < 1) || (J < 2) || (L <= 0.) || (vb <= 0.)
      || (dt <= 0.) || (tmax <= 0.) || (skip < 1))
    {
      printf ("Error - invalid input parameters\n");
      exit (1);
    }

  // Set names of output files
  char* phase[11]; char* data[11];
  phase[0] = "phase0.out";phase[1] = "phase1.out";phase[2] = "phase2.out";
  phase[3] = "phase3.out";phase[4] = "phase4.out";phase[5] = "phase5.out";
  phase[6] = "phase6.out";phase[7] = "phase7.out";phase[8] = "phase8.out";
  phase[9] = "phase9.out";phase[10] = "phase10.out";data[0] = "data0.out";
  data[1] = "data1.out"; data[2] = "data2.out"; data[3] = "data3.out";
  data[4] = "data4.out"; data[5] = "data5.out"; data[6] = "data6.out";
  data[7] = "data7.out"; data[8] = "data8.out"; data[9] = "data9.out";
  data[10] = "data10.out";

  // Initialize solution
  double t = 0.;
  int seed = time (NULL); srand (seed);
  Array<double,1> r(N), v(N);
  for (int i = 0; i < N; i++)
    {
      r(i) = L * double (rand ()) / double (RAND_MAX);
      v(i) = distribution (vb);
    }
  Output (phase[0], data[0], t, r, v);

  // Evolve solution
  Array<double,1> y(2*N);
  Load (r, v, y);
  for (int k = 1; k <= 10; k++)
    {
      for (int kk = 0; kk < skip; kk++)
        {
           // Take time-step
           rk4_fixed (t, y, rhs_eval, dt);

           // Make sure all coordinates in range 0 to L.
           for (int i = 0; i < N; i++)
             {
               if (y(i) < 0.) y(i) += L;
               if (y(i) > L) y(i) -= L;
             }

           printf ("t = %11.4e\n", t);
        }
      printf ("Plot %3d\n", k);

      // Output data
      UnLoad (y, r, v);
      Output(phase[k], data[k], t, r, v);
    }

  return 0;
}
The following routine outputs the simulation data to various data-files.

// Write data to output files

void Output (char* fn1, char* fn2, double t,
	     Array<double,1> r, Array<double,1> v)
{
  // Write phase-space data
  FILE* file = fopen (fn1, "w");
  for (int i = 0; i < N; i++)
    fprintf (file, "%e %e\n", r(i), v(i));
  fclose (file);

  // Write electric field data
  Array<double,1> ne(J), n(J), phi(J), E(J);
  Density (r, ne);
  for (int j = 0; j < J; j++)
    n(j) = double (J) * ne(j) / double (N) - 1.;
  double kappa = 2. * M_PI / L;
  Poisson1D (phi, n, kappa);
  Electric (phi, E);

  file = fopen (fn2, "w");
  for (int j = 0; j < J; j++)
    {
      double x = double (j) * L / double (J);
      fprintf (file, "%e %e %e %e\n", x, ne(j), n(j), E(j));
    }
  double x = L;
  fprintf (file, "%e %e %e %e\n", x, ne(0), n(0), E(0));
  fclose (file);
}
The following routine returns a random velocity distributed on a double Maxwellian distribution function corresponding to two counter-streaming beams. The algorithm used to achieve this is called the rejection method, and will be discussed later in this course.

// Function to distribute electron velocities randomly so as
// to generate two counter propagating warm beams of thermal
// velocities unity and mean velocities +/- vb.
// Uses rejection method.

double distribution (double vb)
{
  // Initialize random number generator
  static int flag = 0;
  if (flag == 0)
    {
      int seed = time (NULL);
      srand (seed);
      flag = 1;
    }

  // Generate random v value
  double fmax = 0.5 * (1. + exp (-2. * vb * vb));
  double vmin = - 5. * vb;
  double vmax = + 5. * vb;
  double v = vmin + (vmax - vmin) * double (rand ()) / double (RAND_MAX);

  // Accept/reject value
  double f = 0.5 * (exp (-(v - vb) * (v - vb) / 2.) +
		    exp (-(v + vb) * (v + vb) / 2.));
  double x = fmax * double (rand ()) / double (RAND_MAX);
  if (x > f) return distribution (vb);
  else return v;
}
The routine below evaluates the electron number density on an evenly spaced mesh given the instantaneous electron coordinates.

// Evaluates electron number density n(0:J-1) from
// array r(0:N-1) of electron coordinates.

void Density (Array<double,1> r, Array<double,1>& n)
{
  // Initialize
  double dx = L / double (J);
  n = 0.;

  // Evaluate number density.
  for (int i = 0; i < N; i++)
    {
      int j = int (r(i) / dx);
      double y = r(i) / dx - double (j);
      n(j) += (1. - y) / dx;
      if (j+1 == J) n(0) += y / dx;
      else n(j+1) += y / dx;
    }
}
The following functions are wrapper routines for using the fftw library with periodic functions.

// Functions to calculate Fourier transforms of real data
// using fftw Fast-Fourier-Transform routine.
// Input/ouput arrays are assumed to be of extent J.

// Calculates Fourier transform of array f in arrays Fr and Fi
void fft_forward (Array<double,1>f, Array<double,1>&Fr,
      Array<double,1>& Fi)
{
  fftw_complex ff[J], FF[J];

  // Load data
  for (int j = 0; j < J; j++)
    {
      c_re (ff[j]) = f(j); c_im (ff[j]) = 0.;
    }

  // Call fftw routine
  fftw_plan p = fftw_create_plan (J, FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_one (p, ff, FF);
  fftw_destroy_plan (p);

  // Unload data
  for (int j = 0; j < J; j++)
    {
      Fr(j) = c_re (FF[j]); Fi(j) = c_im (FF[j]);
    }

  // Normalize data
  Fr /= double (J);
  Fi /= double (J);
}

// Calculates inverse Fourier transform of arrays Fr and Fi in array f
void fft_backward (Array<double,1> Fr, Array<double,1> Fi,
      Array<double,1>& f)
{
  fftw_complex ff[J], FF[J];

  // Load data
  for (int j = 0; j < J; j++)
    {
      c_re (FF[j]) = Fr(j); c_im (FF[j]) = Fi(j);
    }

  // Call fftw routine
  fftw_plan p = fftw_create_plan (J, FFTW_BACKWARD, FFTW_ESTIMATE);
  fftw_one (p, FF, ff);
  fftw_destroy_plan (p);

  // Unload data
  for (int j = 0; j < J; j++)
      f(j) = c_re (ff[j]);
}
The following routine solves Poisson's equation in 1-D to find the instantaneous electric potential on a uniform grid.

// Solves 1-d Poisson equation:
//    d^u / dx^2 = v   for  0 <= x <= L
// Periodic boundary conditions:
//    u(x + L) = u(x),  v(x + L) = v(x)
// Arrays u and v assumed to be of length J.
// Now, jth grid point corresponds to
//    x_j = j dx  for j = 0,J-1
// where dx = L / J.
// Also,
//    kappa = 2 pi / L

void Poisson1D (Array<double,1>& u, Array<double,1> v, double kappa)
{
  // Declare local arrays.
  Array<double,1> Vr(J), Vi(J), Ur(J), Ui(J);

  // Fourier transform source term
  fft_forward (v, Vr, Vi);

  // Calculate Fourier transform of u
  Ur(0) = Ui(0) = 0.;
  for (int j = 1; j <= J/2; j++)
    {
      Ur(j) = - Vr(j) / double (j * j) / kappa / kappa;
      Ui(j) = - Vi(j) / double (j * j) / kappa / kappa;
    }
  for (int j = J/2; j < J; j++)
    {
      Ur(j) = Ur(J-j);
      Ui(j) = - Ui(J-j);
    }

  // Inverse Fourier transform to obtain u
  fft_backward (Ur, Ui, u);
}
The following function evaluates the electric field on a uniform grid from the electric potential.

// Calculate electric field from potential

void Electric (Array<double,1> phi, Array<double,1>& E)
{
  double dx = L / double (J);

  for (int j = 1; j < J-1; j++)
    E(j) = (phi(j-1) - phi(j+1)) / 2. / dx;
  E(0) = (phi(J-1) - phi(1)) / 2. / dx;
  E(J-1) = (phi(J-2) - phi(0)) / 2. / dx;
}
The following routine is the right-hand side routine for the electron equations of motion. Is is designed to be used with the fixed-step RK4 solver described earlier in this course.

// Electron equations of motion:
//    y(0:N-1)  = r_i
//    y(N:2N-1) = dr_i/dt

void rhs_eval (double t, Array<double,1> y, Array<double,1>& dydt)
{
  // Declare local arrays
  Array<double,1> r(N), v(N), rdot(N), vdot(N), r0(N);
  Array<double,1> ne(J), rho(J), phi(J), E(J);

  // Unload data from y
  UnLoad (y, r, v);

  // Make sure all coordinates in range 0 to L
  r0 = r;
  for (int i = 0; i < N; i++)
    {
      if (r0(i) < 0.) r0(i) += L;
      if (r0(i) > L) r0(i) -= L;
    }

  // Calculate electron number density
  Density (r0, ne);

  // Solve Poisson's equation
  double n0 = double (N) / L;
  for (int j = 0; j < J; j++)
    rho(j) = ne(j) / n0 - 1.;
  double kappa = 2. * M_PI / L;
  Poisson1D (phi, rho, kappa);

  // Calculate electric field
  Electric (phi, E);

  // Equations of motion
  for (int i = 0; i < N; i++)
    {
      double dx = L / double (J);
      int j = int (r0(i) / dx);
      double y = r0(i) / dx - double (j);

      double Efield;
      if (j+1 == J)
         Efield = E(j) * (1. - y) + E(0) * y;
      else
         Efield = E(j) * (1. - y) + E(j+1) * y;

      rdot(i) = v(i);
      vdot(i) = - Efield;
    }

  // Load data into dydt
  Load (rdot, vdot, dydt);
}
The following functions load and unload the electron phase-space coordinates into the solution vector y used by the RK4 routine.

// Load particle coordinates into solution vector

void Load (Array<double,1> r, Array<double,1> v, Array<double,1>& y)
{
  for (int i = 0; i < N; i++)
    {
      y(i) = r(i);
      y(N+i) = v(i);
    }
}

// Unload particle coordinates from solution vector

void UnLoad (Array<double,1> y, Array<double,1>& r, Array<double,1>& v)
{
  for (int i = 0; i < N; i++)
    {
      r(i) = y(i);
      v(i) = y(N+i);
    }
}


#include <iostream>
#include <iomanip>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/AngleAxis.h>

using namespace std;
using namespace Eigen;

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta);
bool isRotationMatirx(Eigen::Matrix3d R);
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R);

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;

int main1(){
    double roll_deg = 0.5;      // 绕X轴
    double pitch_deg = 0.8;     // 绕Y轴
    double yaw_deg = 108.5;     // 绕Z轴

    // 转化为弧度
    double roll_arc = roll_deg * DEG_TO_ARC;    // 绕X轴
    double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴
    double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴

    roll_arc =  1.12854674230422;    // 绕X轴
    pitch_arc = 1.08062392168743;  // 绕Y轴
    yaw_arc =   1.10953306595605;      // 绕Z轴

}

int main()
{
//    真值获取
    double lc_r = 0.015;
    double lc_p = 0.032;
    double lc_y = 0.061;

    double t_lc_x = 0.04;
    double t_lc_y = 0.08;
    double t_lc_z = -0.061;

    Eigen::AngleAxisd roll(AngleAxisd(lc_r, Vector3d::UnitZ()));
    Eigen::AngleAxisd pitch(AngleAxisd(-lc_p, Vector3d::UnitX()));
    Eigen::AngleAxisd yaw(AngleAxisd(-lc_y, Vector3d::UnitY()));
    Eigen::Matrix3d R_base;
    R_base<<
           0, -1, 0,
        0, 0, -1,
        1, 0,  0;
    Matrix3d gt_R_lc;
    gt_R_lc = roll * yaw * pitch;

//0.0150279
//-0.0609931
//-0.0329161

    Vector3d gt_t_lc = R_base * Eigen::Vector3d(t_lc_x, t_lc_y, t_lc_z);
    Matrix3d gt_R_cl = gt_R_lc.transpose();
    Vector3d gt_t_cl = -gt_R_cl * gt_t_lc;
    cout <<setprecision(15)<<"gt_rpy_cl:\n"<<gt_R_cl.eulerAngles(2,1,0)<<endl;
    cout <<"gt_R_cl:\n"<<gt_R_cl<<endl;
    cout <<"gt_t_cl:\n"<<gt_t_cl<<endl;

    // 转化为弧度
    double roll_arc = gt_R_cl.eulerAngles(2,1,0).z();    // 绕X轴
    double pitch_arc = gt_R_cl.eulerAngles(2,1,0).y();  // 绕Y轴
    double yaw_arc = gt_R_cl.eulerAngles(2,1,0).x();      // 绕Z轴

    cout << endl;
    cout << "roll_arc = " << roll_arc << endl;
    cout << "pitch_arc = " << pitch_arc << endl;
    cout << "yaw_arc = " << yaw_arc << endl;

    // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);

    // 使用Eigen库将欧拉角转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
    rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
    cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;

    // 使用自定义函数将欧拉角转换为旋转矩阵
    rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);
    cout << "rotation matrix2 =\n" << rotation_matrix2 << endl << endl;

    // 使用Eigen将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序，yaw,pitch,roll
    cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1]
         << " " << eulerAngle1[0] << endl << endl;

    // 使用自定义函数将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
    cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
         << " " << eulerAngle2[2] << endl << endl;

    return 0;
}

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
    Eigen::Matrix3d R_x;    // 计算旋转矩阵的X分量
    R_x <<
        1,              0,               0,
        0,  cos(theta[0]),  -sin(theta[0]),
        0,  sin(theta[0]),   cos(theta[0]);

    Eigen::Matrix3d R_y;    // 计算旋转矩阵的Y分量
    R_y <<
        cos(theta[1]),   0, sin(theta[1]),
        0,   1,             0,
        -sin(theta[1]),  0, cos(theta[1]);

    Eigen::Matrix3d R_z;    // 计算旋转矩阵的Z分量
    R_z <<
        cos(theta[2]), -sin(theta[2]), 0,
        sin(theta[2]),  cos(theta[2]), 0,
        0,              0,             1;
    Eigen::Matrix3d R = R_z * R_y * R_x;
    return R;
}

bool isRotationMatirx(Eigen::Matrix3d R)
{
    double err=1e-6;
    Eigen::Matrix3d shouldIdenity;
    shouldIdenity=R*R.transpose();
    Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
    return (shouldIdenity - I).norm() < err;
}

Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(R));
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}


/**
 * Initializing the location of data points in the low-D space
 */
#include <iostream>
#include <armadillo>
#include <chrono>
#include <omp.h>
#include <Eigen/Sparse>

#define SCALE 0.0001

using namespace std;
using namespace arma;
using namespace Eigen;

void Initialization (bool randominitializing, double** embedding, ofstream& logFile, int N, SparseMatrix<float> & graphSM, float MaxWeight, int DimLowSpace, int n_epochs){

	srand(17);
	/**
	 *  zero approximation in float precision
	 */
	float epsilon=1e-6;
	/**
	 * By deafult, low-D space dimensions are between -10 and 10
	 */
	int minDimLowDSpace=-10;
	int maxDimLowDSpace=10;

	if (!randominitializing){
		try{
			logFile<<" Spectral Initialization of Data in Lower Space"<<endl;
			cout<<" Spectral Initialization of Data in Lower Space"<<endl;

			/**
			 * graph is undirected weights (similarities) function for all the edges in the high-D space
			 */
			float** graph = new float*[N];
			for (int i = 0; i < N; ++i) { graph[i] = new float[N]; }

            #pragma omp parallel for
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					graph[i][j]=0;
				}
			}

			for (int k=0; k<graphSM.outerSize(); ++k){
				for (SparseMatrix<float>::InnerIterator it(graphSM,k); it; ++it) {
					graph[it.row()][it.col()]=it.value();
				}
			}

			/**
			 * Removing the small weights in accordance to https://github.com/lmcinnes/umap/blob/master/umap/umap_.py#L1032
			 */
            #pragma omp parallel for
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					if (graph[i][j] <  epsilon) continue;
					if (graph[i][j] <  MaxWeight/n_epochs) graph[i][j]=0;
				}
			}

			/**
			 * DegreeMatrix is a diagonal matrix contains information about the degree of each vertex
			 * sqrtDegreeMatrix transforms the diagonal values of DegreeMatrix by 1.0/sqrt()
			 */
			float** sqrtDegreeMatrix = new float*[N];
			for (int i = 0; i < N; ++i) { sqrtDegreeMatrix[i] = new float[N]; }

			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					sqrtDegreeMatrix[i][j]=0;
				}
			}

			for (int i = 0; i < N; ++i) {
				float sum=0;
				for (int j = 0; j < N; ++j) {
					sum+=graph[i][j];
				}
				sqrtDegreeMatrix[i][i]=1.0/sqrt(sum);
			}
			/**
			 * aux_mem is the column-wise transformation of sqrtDegreeMatrix as needed by armadillo function fmat
			 */
			float* aux_mem = new float[N*N];
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					aux_mem[j*N+i]=sqrtDegreeMatrix[i][j];
				}
			}
			delete [] sqrtDegreeMatrix;
			/**
			 * Making an armadillo sparse matrix spmatDegreeMatrix from sqrtDegreeMatrix
			 */
			fmat matDegreeMatrix(aux_mem,N,N,false,true);
			sp_fmat spmatDegreeMatrix(matDegreeMatrix);
			/**
			 * aux_mem2 is the column-wise transformation of adjacencyMatrix as needed by armadillo function fmat
			 */
			float* aux_mem2 = new float[N*N];
			for (int i = 0; i < N; ++i){
				for (int j = 0; j < N; ++j){
					aux_mem2[j*N+i]=graph[i][j];   //column-wise
				}
			}

			delete[] graph;

			/**
			 * Making an armadillo sparse matrix spmatadjacencyMatrix from adjacencyMatrix
			 */
			fmat matadjacencyMatrix(aux_mem2,N,N,false,true);
			sp_fmat spmatadjacencyMatrix(matadjacencyMatrix);
			/**
			 * Making an armadillo sparse matrix of identity
			 */
			sp_fmat Unity = speye<sp_fmat>(N,N);
			/**
			 * Making an armadillo sparse matrix of Laplacian
			 */
			sp_fmat laplacianMatrix;
			laplacianMatrix= Unity-spmatDegreeMatrix*spmatadjacencyMatrix*spmatDegreeMatrix;
			/**
			 * Solving eigenvalue and eigenvector for Laplacian matrix
			 */
			fvec eigval;
			fmat eigvec;
			eigs_sym(eigval, eigvec, laplacianMatrix, DimLowSpace+1 , "sm");
			/**
			 * Converting eigenvectors to tmpvector
			 * will throw "error: Mat::col(): index out of bounds" if no eigvec was available
			 */
			typedef std::vector<float> stdvec;
			std::vector< std::vector<float> > tmpvector;

			for (int i = 1; i < DimLowSpace+1; ++i) {
				stdvec vectest = arma::conv_to< stdvec >::from(eigvec.col(i));
				tmpvector.push_back(vectest);
			}
			/**
			 * using tmpvector to intialize the locations of the points in low-D space
			 * embedding should not be outside the chosen dimensions for low-D space
			 */
			double maxembedding=0;
			for (int j = 0; j < DimLowSpace; ++j) {
				for (int i = 0; i < N; ++i) {
					double tmp=tmpvector[j][i];
					embedding[i][j]= tmp;

					if (abs(tmp) > maxembedding) maxembedding=tmp;
				}
			}

			double expansion=double(maxDimLowDSpace)/maxembedding;

			// Also adding a noise as prescribed in https://github.com/lmcinnes/umap/blob/master/umap/umap_.py#L1040
			unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
			std::default_random_engine generator (seed);
			std::normal_distribution<double> distribution (0.0,1.0);

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < DimLowSpace; ++j) {
					embedding[i][j] =embedding[i][j]* expansion+ SCALE*distribution(generator);
				}
			}

		} catch(std::exception& e){
			logFile<<" Spectral Initialization Failed. Will proceed with random initialization."<<endl;
			cout<<" Spectral Initialization Failed. Will proceed with random initialization."<<endl;
			randominitializing=true ; }
	}
	/**
	 * If the above procedure fails or randominitializing=1 as an input argument, the
	 * location of the points are determined randomly
	 */
	if (randominitializing){
		logFile<<" Random Initialization of Data in low-D Space"<<endl;
		cout<<" Random Initialization of Data in low-D Space"<<endl;
		for (int i = 0; i < N; ++i) {
			for (int j = 0; j < DimLowSpace; ++j) {
				double tmp=(double)rand()/RAND_MAX;
				embedding[i][j]=minDimLowDSpace+(maxDimLowDSpace-minDimLowDSpace)*tmp;
			}
		}
	}

	return;
}


// 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;
    }
  }
}


/*
 *
 * 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)); }


#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;
}