#include <Eigen/Eigenvalues>
#include <eigen3/unsupported/Eigen/MPRealSupport>
#include <mpfr.h>
#include <iostream>

typedef mpfr::mpreal realt;
typedef std::complex<realt> complext;
typedef realt __plant_typet;
#define CONTROL_TYPES_H_
#define interval(x) x
#include "benchmark.h"

using namespace Eigen;
using namespace std;
using namespace mpfr;

#define EXIT_INCREASE_K 2
#define EXIT_INCREASE_SAMPLE_RATE 3
#define PRECISION 256
#define NUM_PROG_ARGS 2 + NSTATES
#define K_SIZE_ARG_INDEX 1u
#define K_ARG_OFFSET 2u

typedef Matrix<realt, NSTATES, NSTATES> matrixt;

//const realt _controller_K[] = { 0.0234375,-0.1328125, 0.00390625 };
//const realt _controller_K[] = { 0.0234375,0.1328125, 0.00390625 };
realt _controller_K[NSTATES];

bool is_imaginary(const realt &imaginary_offset, const complext &complex) {
  const realt imag_value = std::imag(complex);
  cout << "imag_value: " << imag_value << endl;
  cout << "imaginary_offset: " << imaginary_offset << endl;
  return abs(imag_value) > imaginary_offset;
}

int main(const int argc, const char * const argv[]) {
  const realt two = "2.0";
  const realt two_pi = const_pi() * two;
  const realt imaginary_offset = pow(two, -PRECISION/4);
  if (argc != NUM_PROG_ARGS) return EXIT_FAILURE;
  mpreal::set_default_prec(PRECISION);
  const realt K_SIZE = argv[K_SIZE_ARG_INDEX];
  for (size_t i=0; i < NSTATES; ++i)
    _controller_K[i]=argv[i + K_ARG_OFFSET];

  Matrix<realt, NSTATES, NSTATES> A;
  for (size_t i = 0; i < NSTATES; ++i) {
    for (size_t j = 0; j < NSTATES; ++j) {
      A(i, j) = _controller_A[i][j];
    }
  }
  Matrix<realt, NSTATES, 1> B;
  for (size_t i = 0; i < NSTATES; ++i) {
    B(i) = _controller_B[i];
  }
  Matrix<realt, 1, NSTATES> K;
  for (size_t i = 0; i < NSTATES; ++i) {
    K[i] = _controller_K[i];
  }

  // Check K_SIZE
  matrixt result = A - B * K;
  EigenSolver<matrixt> eigenSpace(result);
  if (Success != eigenSpace.info())
    return EXIT_FAILURE;
  const EigenSolver<matrixt>::EigenvalueType eigenvalues = eigenSpace.eigenvalues();
  cout << "num_eigenvalues: " << eigenvalues.size() << endl;
  for (size_t i=0; i < eigenvalues.size(); ++i) {
    cout << "eigenvalue: " << eigenvalues[i] << endl;
    if (!is_imaginary(imaginary_offset, eigenvalues[i])) continue;
    const realt angle = std::arg(eigenvalues[i]);
    const realt expected = abs(two_pi / angle);
    cout << "expected_k_size=" << expected << endl;
    cout << "actual_k_size=" << K_SIZE << endl;
    if (expected > K_SIZE)
      return EXIT_INCREASE_K;
  }

  // Check sample rate
  /*Matrix<realt, Dynamic, Dynamic> vertices(Matrix<realt, Dynamic, Dynamic>::Ones(NSTATES, ::pow(2, NSTATES)));
  int step = 1;
  for (size_t row = 0; row < vertices.rows(); ++row) {
    for (size_t col = 1; col < vertices.cols(); ++col) {
      vertices.coeffRef(row, col) = vertices.coeffRef(row, col - 1);
      if (col % step == 0) vertices.coeffRef(row, col) = -vertices.coeffRef(row, col);
    }
    step <<= 1;
  }
  const Matrix<complext, NSTATES, NSTATES> eigenvectors(eigenSpace.eigenvectors());
  cout << "eigenvectors: " << endl << eigenvectors << endl;
  matrixt pseudo_eigenvectors(eigenvectors.real());
  for (size_t i=0; i < NSTATES - 1; ++i) {
    if (!is_imaginary(imaginary_offset, eigenvalues[i])) continue;
    pseudo_eigenvectors.col(i + 1) = eigenvectors.col(i).imag();
    ++i;
  }
  cout << "pseudo_eigenvectors: " << endl << pseudo_eigenvectors << endl;
  vertices *= pseudo_eigenvectors.transpose();*/

  return EXIT_SUCCESS;
}


#include <vector> /* representing state */
#include <algorithm> /* for numeric min/max */
#include <cmath> /* log, exp, fmod */

#include <boost/numeric/odeint.hpp>
#include <ios>
using namespace boost::numeric::odeint;

typedef double icing_float;

// [1] - Reference Paper: TBME-01160-2016
// [2] - Reference Paper: computer methods and programs in biomedicine 102 (2011) 192–205
template<typename U = icing_float>
class ChaseIcing {
    // Each enum maps to an index within the state vector for accessing that
    // function's value.
    enum Function { fn_G = 0
        , fn_Q
        , fn_I
        , fn_P1
        , fn_P2
        , fn_P
        , fn_u_en
    };
    using _state_type = std::vector<U>;

    /* a bunch of constants that should be in the namespace */
    const U n_I = 0.0075; // 1/min, Table II, [1]
    const U d1 = -std::log(0.5)/20.0; // 1/min, Table II, [1]
    const U d2 = -std::log(0.5)/100.0; // 1/min, Table II, [1]
    const U P_max = 6.11; // mmol/min, Table II, [1]

    auto P_min(const _state_type &x)
    {
        return std::min(d2 * x[fn_P2], P_max);
    }

    auto alpha_decay(const U variable, const U decay_parameter)
    {
        return variable / (1.0 + decay_parameter * variable);
    }

    auto Q_frac(const _state_type &x)
    {
        const U a_G = 1.0 / 65.0; // Table II, [1]
        const U Q = x[fn_Q];
        return alpha_decay(Q, a_G);
    }

    // PN(t) -> Parenteral nutrition input, eg IV dextrose
    auto _P(const _state_type &x)
    {
        const U _PN_ext = dextrose_rate; // TODO -> derive this over the network

        return P_min(x) + _PN_ext;
    }

    auto _u_en(const _state_type &x)
    {
        const U k1 = 14.9; // mU * l / mmol/min, Table II, [1]
        const U k2 = -49.9; // mU/min, Table II, [1]
        const U u_min = 16.7; // mU/min, Table II, [1]
        const U u_max = 266.7; // mU/min, Table II, [1]
        const U G = x[fn_G];
        return std::min(std::max(u_min, k1 * G + k2), u_max);
    }

    auto G_dot(const _state_type &x)
    {
        const U p_G = 0.006; // End of section 4.1, in [2]
        const U S_I = 0.5e-3; // TODO: patient specific
        const U G = x[fn_G];
        const U Q = x[fn_Q];
        const U P = _P(x);
        const U EGP = 1.16; // mmol/min Table II, [1]
        const U CNS = 0.3; // mmol/min Table II, [1]
        const U V_G = 13.3; // L, Table II, [1]

        U dGdt = 0.0;
        // G' = -p_G G(t)
        //	 - S_I G(t) (Q(t) / (1 + a_G Q(t)))
        //	 + (P(t) + EGP -CNS)/V_G
        dGdt += -p_G * G;
        dGdt += -S_I*G * Q_frac(x);
        dGdt += (P + EGP - CNS)/V_G;

        return dGdt;
    }

    auto Q_dot(const _state_type &x)
    {
        const U I = x[fn_I];
        const U Q = x[fn_Q];
        const U n_C = 0.0075; // 1/min, Table II, [1]

        return n_I * (I - Q) - n_C * Q_frac(x);
    }

    auto I_dot(const _state_type &x)
    {
        const U n_K = 0.0542; // 1/min, Table II, [1]
        const U n_L = 0.1578; // 1/min, Table II, [1]
        const U a_I = 1.7e-3; // 1/mU, Table II, [1]
        const U V_I = 4.0; // L, Table II, [1]
        const U x_L = 0.67; // unitless, Table II, [1]

        const U u_ex = insulin_rate; // TODO: get this over the network

        const U Q = x[fn_Q];
        const U I = x[fn_I];
        const U u_en = _u_en(x);

        auto dIdt = U(0.0);
        // I' = - n_K I(t)
        //	- n_L (I(t)/(1+a_I I(t)))
        //	- n_I (I(t) - Q(t))
        //	+ u_ex(t) / V_I
        //	+ (1 - x_L) u_en / V_I
        dIdt += -n_K * I;
        dIdt += -n_L * alpha_decay(I, a_I);
        dIdt += -n_I * (I - Q);
        dIdt += u_ex / V_I;
        dIdt += (1.0 - x_L) * u_en / V_I;
        return dIdt;
    }

    auto P1_dot(const _state_type &x)
    {
        const auto D = U(0.0); // enteral feed rate TODO: get from network
        return -d1 * x[fn_P1] + D;
    }

    auto P2_dot(const _state_type &x)
    {
        return -P_min(x) + d1 * x[fn_P1];
    }

    void copy(const ChaseIcing &other)
    {
        data = other.data;
        insulin_rate = other.insulin_rate;
        dextrose_rate = other.dextrose_rate;
    }

    /* data is the container which contains the most up to date representation of the model's state
     *
     * Should only be accessed using the enum Function data type
     */
    _state_type data;
public:
    using state_type = _state_type;
    void operator() (const state_type &x, state_type &dxdt, const U t)
    {
        dxdt[fn_G] = G_dot(x);
        dxdt[fn_Q] = Q_dot(x);
        dxdt[fn_I] = I_dot(x);
        dxdt[fn_P1] = P1_dot(x);
        dxdt[fn_P2] = P2_dot(x);
    }

    auto glucose()
    {
        return data[fn_G];
    }

    auto q()
    {
        return data[fn_Q];
    }

    auto i()
    {
        return data[fn_I];
    }

    auto p1()
    {
        return data[fn_P1];
    }

    auto p2()
    {
        return data[fn_P2];
    }

    int run(U time_start
            , U time_end
            , U dt
            , U insulin_rate_mUpermin
            , U dextrose_rate_mmolpermin)
    {

        insulin_rate = insulin_rate_mUpermin;
        dextrose_rate = dextrose_rate_mmolpermin;

        /* model doesnt modify inplace, so we make copies of its data
         * for integration */
        auto x = data;
        auto step = stepper;

        integrate_const(step, *this, x, time_start, time_end, dt);

        data = x;
        stepper = step;

        return 0;
    }

    /* run the model until time_end using default rates and time step
     */
    int run(U time_end)
    {
        return run(0.0, time_end, 0.1, insulin_rate, dextrose_rate);
    }

    /* run the model with implicitly provided rates
     */
    int run(U time_start, U time_end, U dt)
    {
        return run(time_start, time_end, dt, insulin_rate, dextrose_rate);
    }

    /* insulin_rate is the exogenous IV insulin rate, in mU/min (not mmol/min as listed in [1])
     *
     * Available for public modification, but use run() for most purposes.
     */
    U insulin_rate;

    /* dextrose_rate is the exogenous IV dextrose rate, in mmol/min
     *
     * Available for public modification, but use run() for most purposes.
     */
    U dextrose_rate;

    /* stepper is the Boost integration type, we use RK4 but you may use any you like
     */
    runge_kutta4<state_type> stepper;

    /* public constructor
     */
    ChaseIcing(state_type _data) : data(_data) {};

    /* copy constructor
     */
    ChaseIcing(const ChaseIcing &other)
    {
        copy(other);
    }

    /* assignment operator
     */
    ChaseIcing& operator=(const ChaseIcing &other)
    {
        copy(other);
        return *this;
    }
};


/**
 * Copyright (c) 2022 <Daumantas Kavolis>
 *
 * 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.
 */

#pragma once

#include "config.hpp"

MSVC_WARNING_DISABLE(4619)
#include <boost/container/small_vector.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/size.hpp>
MSVC_WARNING_POP()

#include "polynomial.hpp"
#include "sequence.hpp"
#include "traits.hpp"

namespace poly {
template <class Poly, std::size_t N = SmallStorageSize>
class PolynomialSeries {
 public:
  using Traits = typename Poly::Traits;
  using Real = typename Traits::Real;
  using OrderType = typename Traits::OrderType;

  template <typename T>
  using small_vector = boost::container::small_vector<T, N>;

  explicit PolynomialSeries(OrderType n) { resize(n); }

  template <class Range, typename = std::enable_if_t<detail::is_range<Range>::value>>
  explicit PolynomialSeries(Range const& coefficients) {
    auto count = boost::size(coefficients);
    coefficients_.resize(count);
    polynomials_.resize(count);
    for (auto&& [index, coefficient] : coefficients | boost::adaptors::indexed()) {
      polynomials_[index].order(narrow_cast<OrderType>(index));
      coefficients_[index] = static_cast<Real>(coefficient);
    }
  }

  auto coefficients() noexcept -> view<Real> {
    return {coefficients_.data(), coefficients_.size()};
  }
  [[nodiscard]] auto coefficients() const noexcept -> view<Real const> {
    return {coefficients_.data(), coefficients_.size()};
  }
  [[nodiscard]] auto polynomials() const noexcept -> view<Poly const> {
    return {polynomials_.data(), polynomials_.size()};
  }

  auto operator[](std::size_t index) noexcept -> Real& { return coefficients_[index]; }
  [[nodiscard]] auto operator[](std::size_t index) const noexcept -> Real {
    return coefficients_[index];
  }

  auto at(std::size_t index) noexcept -> Real& { return coefficients_.at(index); }
  [[nodiscard]] auto at(std::size_t index) const noexcept -> Real {
    return coefficients_.at(index);
  }

  template <class F, typename = std::enable_if_t<std::is_invocable_r_v<Real, F, Real>>>
  [[nodiscard]] static auto project(F const& function, OrderType order) -> PolynomialSeries {
    Poly poly{order};
    view<Real const> abscissa = poly.abscissa();
    return project(abscissa | boost::adaptors::transformed(
                                  [&function](Real const& x) { return function(x); }));
  }

  template <class Range, typename = std::enable_if_t<detail::is_range<Range>::value>>
  [[nodiscard]] static auto project(Range const& y_range) -> PolynomialSeries {
    OrderType count = narrow<OrderType>(boost::size(y_range));
    if (count == 0) return PolynomialSeries(0);
    OrderType max_order = count - 1;

    Poly poly{max_order};
    view<Real const> weights = poly.weights();
    view<Real const> abscissa = poly.abscissa();

    PolynomialSeries series(max_order);

    for (auto&& [j, y] : y_range | boost::adaptors::indexed()) {
      for (auto&& [i, f] :
           polynomial_sequence<Poly>(count, abscissa[j]) | boost::adaptors::indexed()) {
        series.coefficients_[i] += f * weights[j] * y;
      }
    }

    return series;
  }

  [[nodiscard]] auto operator()(Real x) const -> Real {
    if constexpr (Traits::has_next) {
      if (size() == 0) return 0;
      Real t0 = polynomials_[0](x);
      Real f = coefficients_[0] * t0;
      if (size() == 1) return f;
      Real t1 = polynomials_[1](x);
      for (std::size_t i = 1; i < size(); i++) {
        f += coefficients_[i] * t1;
        std::swap(t0, t1);
        t1 = polynomials_[i].next(x, t0, t1);
      }
      return f;
    } else {
      Real f = 0;
      for (auto&& [index, polynomial] : polynomials_ | boost::adaptors::indexed())
        f += coefficients_[index] * polynomial(x);
      return f;
    }
  }

  [[nodiscard]] auto size() const noexcept -> std::size_t { return polynomials_.size(); }
  void resize(OrderType new_size) {
    OrderType old_size = narrow<OrderType>(size());
    polynomials_.resize(new_size);
    coefficients_.resize(new_size, 0);

    for (OrderType i = old_size; i < new_size; i++) polynomials_[i].order(i);
  }

  template <bool check = false>
  [[nodiscard]] auto weights(bounds_check<check> c = no_bounds_check) const -> view<Real const> {
    return polynomials_[size() - 1].weights(c);
  }
  template <bool check = false>
  [[nodiscard]] auto abscissa(bounds_check<check> c = no_bounds_check) const -> view<Real const> {
    return polynomials_[size() - 1].abscissa(c);
  }

  static auto domain() noexcept -> std::pair<Real, Real> { return Poly::domain(); }

 private:
  small_vector<Real> coefficients_;
  small_vector<Poly> polynomials_;
};
}  // namespace poly


#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
  Eigen::MatrixXf m(2,4);
  Eigen::VectorXf v(2);

  m << 1, 23, 6, 9,
       3, 11, 7, 2;

  v << 2,
       3;

  MatrixXf::Index index;
  // find nearest neighbour
  (m.colwise() - v).colwise().squaredNorm().minCoeff(&index);

  cout << "Nearest neighbour is column " << index << ":" << endl;
  cout << m.col(index) << endl;
}


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include "base/eigen2hdf.hpp"
#include "base/init.hpp"
#include "base/timer.hpp"
#include "fft/fft2.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

using namespace std;

const char* fname = "test_rt_truncate.h5";

typedef FFT fft_t;
typedef RT<std::complex<double>, RidgeletFrame, fft_t> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt)
{
  const char* fname = "f_rc.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  unsigned int J, rho_x, rho_y;
  double keep;
  double sigma;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("J,j", po::value<unsigned int>(&J)->default_value(3), "J")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("rttre", po::value<double>(&keep)->default_value(1), "percentage of coefficients kept")
      ("sigma", po::value<double>(&sigma)->default_value(1. / 8), "parameter")
      ("save", "save coefficients")
      ("non-smooth", "non-smooth");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << setw(20) << "J: " << J << "\n"
       << setw(20) << "rho_x: " << rho_x << "\n"
       << setw(20) << "rho_y: " << rho_y << "\n"
       << setw(20) << "Nx: " << std::pow(2, J + 2) * rho_x << "\n"
       << setw(20) << "Ny: " << std::pow(2, J + 2) * rho_y << "\n";
  RDTSCTimer timer;

  timer.start();
  RidgeletFrame frame(J, J, rho_x, rho_y);
  double time_frame_constructor = timer.stop();
  timer.print(cout, time_frame_constructor, "RidgeletFrame init");

  const unsigned int ncols = frame.Nx();  // #cols
  const unsigned int nrows = frame.Ny();  // #rows

  RT_t rt(frame);
  typedef typename RT_t::rt_coeff_t rt_coeff_t;
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  array_t F(nrows / 2, ncols / 2);
  const double sigma2 = sigma * sigma;
  if (vm.count("non-smooth")) {
    Eigen::ArrayXd x = Eigen::ArrayXd::LinSpaced(ncols / 2, 0, 1);
    Eigen::ArrayXd y = Eigen::ArrayXd::LinSpaced(nrows / 2, 0, 1);
    F = x.transpose()
            .replicate(nrows / 2, 1)
            .binaryExpr(y.replicate(1, ncols / 2), [sigma2](double x, double y) {
              return std::exp(-1 / sigma2 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
            });
    F = ((x.transpose().replicate(nrows / 2, 1) - 0.5).cwiseAbs() < sigma)
            .select(F, Eigen::ArrayXXd::Zero(nrows / 2, ncols / 2));

  } else {  // smooth
    Eigen::ArrayXd x = Eigen::ArrayXd::LinSpaced(ncols / 2, 0, 1);
    Eigen::ArrayXd y = Eigen::ArrayXd::LinSpaced(nrows / 2, 0, 1);
    F = x.transpose()
            .replicate(nrows / 2, 1)
            .binaryExpr(y.replicate(1, ncols / 2), [sigma2](double x, double y) {
              return std::exp(-1 / sigma2 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
            });
  }

  fft_t fft;
  // debug
  complex_array_t Fhh(nrows / 2, ncols / 2);
  fft.ft(Fhh, F, false);
  hid_t file;
  if (vm.count("save")) {
    file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "Fhh", Fhh);  // debug
  }
  // ----------------------------------------
  complex_array_t Fh(nrows, ncols);
  Fh.setZero();
  ftcut(Fh, nrows / 2, ncols / 2) = Fhh;
  // hf_zero(Fh); // make sure RT projection is real valued
  rca_t rca(frame);
  timer.start();
  cout << "rt.rt(...)"
       << "\n";
  rt.rt(rca.coeffs(), Fh);

  rca_t rca_copy(frame);
  rca_copy = rca;
  auto& rt_coeffs = rca.coeffs();

  int ncoeffs = 0;
  for (unsigned int i = 0; i < rt_coeffs.size(); ++i) {
    ncoeffs += rt_coeffs[i].rows() * rt_coeffs[i].cols();
  }
  cout << "dim(rt_coeffs): " << ncoeffs << "\n";
  auto time_rt = timer.stop();
  timer.print(cout, time_rt, "rt.rt");

  RCLinearize rcl(frame);

  if (keep < 1) {
    double tre = rcl.get_threshold(rca, keep);
    // apply threshold
    rcl.threshold(rca, tre);
  }

  // -------------------- Inverse transform --------------------
  complex_array_t Fh2(nrows, ncols);
  timer.start();
  cout << "rt.irt(...)"
       << "\n";
  rt.irt(Fh2, rt_coeffs);
  auto time_irt = timer.stop();
  timer.print(cout, time_irt, "rt.irt");

  array_t F2(nrows / 2, ncols / 2);
  complex_array_t Fh2_cut(nrows / 2, ncols / 2);
  Fh2_cut.setZero();
  Fh2_cut = ftcut(Fh2, nrows / 2, ncols / 2);
  // hf_zero(Fh2_cut);
  fft.ift(F2, Fh2_cut);
  auto diff = (F - F2).abs() / (F.rows() * F.cols());
  cout << "(F-F2).abs().sum(): " << diff.sum() << "\n";

  if (vm.count("save")) {
    eigen2hdf::save(file, "Fhl", Fhh);
    eigen2hdf::save(file, "Fh", Fh);
    eigen2hdf::save(file, "R", F);
    eigen2hdf::save(file, "Fh2", Fh2);
    eigen2hdf::save(file, "R2", F2);
    H5Fclose(file);
    cout << "written results to " << fname << "\n";
  }

  return 0;
}


#include <array>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <math.h>
#include <Eigen/Dense>

int print_vector_int(std::vector<int> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] <<  " " ;
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_vector_double(std::vector<double> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] <<  " " ;
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_vector_string(std::vector<std::string> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] << " ";
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_matrix_int(std::vector<std::vector<int> > A) {
   int i, j;
   for (i=0; i<A.size(); i=i+1) {
      for (j=0; j<A[i].size(); j=j+1) {
         std::cout << A[i][j] << " ";
      }
      std::cout << " " << std::endl;
   }
   std::cout << " " << std::endl;
   return 0;
}

bool fexists(const char *filename) {
   std::ifstream ifile(filename);
   return ifile.good();
}

int if_file_exist_delete (std::string filename) {
   if (fexists(filename.c_str())) {
      if (std::remove(filename.c_str()) != 0) {
          std::cout << "failed to remove " << filename << std::endl;
          exit(1);
       }
       else {
          std::cout << filename << " found and deleted " << std::endl;
       }
   }
   return 0;
}

int x_rotation_matrix (double cosx, double sinx, Eigen::Matrix3d& Rx) {

   Rx(0,0)=1;
   Rx(0,1)=0;
   Rx(0,2)=0;
   Rx(1,0)=0;
   Rx(1,1)=cosx;
   Rx(1,2)=-sinx;
   Rx(2,0)=0;
   Rx(2,1)=sinx;
   Rx(2,2)=cosx;

   std::cout << "Rx " << Rx << std::endl;

   return 0;
}

int y_rotation_matrix (double cosy, double siny, Eigen::Matrix3d& Ry) {

   Ry(0,0)=cosy;
   Ry(0,1)=0;
   Ry(0,2)=-siny;
   Ry(1,0)=0;
   Ry(1,1)=1;
   Ry(1,2)=0;
   Ry(2,0)=siny;
   Ry(2,1)=0;
   Ry(2,2)=cosy;

   std::cout << "Ry " << Ry << std::endl;

   return 0;
}

int align (Eigen::Vector3d vec_in, Eigen::MatrixXd& coords) {

   double cosx, sinx, cosy, siny;
   Eigen::Matrix3d Rx, Ry;
   Eigen::Vector3d vecz;
   Eigen::MatrixXd coords_t(coords.cols(),coords.rows());
   coords_t = coords.transpose();

   if (vec_in(0,0) == 0) {
      if (vec_in(1,0) != 0) { // if vec_in = [0,y,z]

         // find matrix for rotation about the x-axis:
         cosx = vec_in(2,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         sinx = vec_in(1,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         x_rotation_matrix(cosx, sinx, Rx);

         // rotate space about the x - axis to bring vec_in
         // in the xz-plane:
         coords_t = Rx*coords_t;
         vecz = Rx*vec_in;
      }
      else if (vec_in(1,0) == 0) {// [0,0,z]
         // all is good
         vecz = vec_in;
      }
   }
   if (vec_in(1,0) == 0) {
      if (vec_in(0,0) != 0) { // [x, 0, z]

         // find matrix for rotation about the y-axis:
         cosy = vec_in(2,0)/sqrt( pow(vec_in(0,0),2) + pow(vec_in(2,0),2) );
         siny = vec_in(0,0)/sqrt( pow(vec_in(0,0),2) + pow(vec_in(2,0),2) );
         y_rotation_matrix (cosy, siny, Ry);

         // rotate space around the y - axis, so that the
         // rotation axis lies along the positive z - axis:
         coords_t = Ry*coords_t;
         vecz = Ry*vec_in; // should have the form [0,0,z]
      }
   }
   if (vec_in(0,0) != 0 ) {
      if (vec_in(1,0) != 0) { // [x, y, z]

         Eigen::Vector3d vecxz;

         // find matrix for rotation about the x-axis:
         cosx = vec_in(2,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         sinx = vec_in(1,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         x_rotation_matrix (cosx, sinx, Rx);

         // rotate space about the x - axis to bring vec_in
         // in the xz-plane:
         std::cout << Ry.rows() << Ry.cols() << coords.rows() << coords.cols()    ;
         coords_t = Rx*coords_t;
         vecxz = Rx*vec_in;

         //find matrix for rotation about the y-axis
         cosy = vecxz(2,0)/sqrt( pow(vecxz(0,0),2) + pow(vecxz(2,0),2) );
         siny = vecxz(0,0)/sqrt( pow(vecxz(0,0),2) + pow(vecxz(2,0),2) );
         y_rotation_matrix (cosy, siny, Ry);

         // rotate space around the y - axis, so that the rotation axis lies
         // along the positive z - axis:
         std::cout << Ry.rows() << Ry.cols() << coords.rows() << coords.cols();
         coords_t = Ry*coords_t;
         vecz = Ry*vecxz; //should have the form [0,0,z]
      }
   }
   coords = coords_t.transpose();
   std::cout << "vecz: " << vecz << std::endl;
   return 0;
}

int periodic_table(std::string& lable, double& mass) {

   int k;
	std::vector<std::string> lables{"C", "S"};
	std::vector<double> masses{12.0107, 32.065};

   std::cout << "lable: " << lable << "end" << std::endl;

	for (k=0; k<lables.size(); k=k+1) {
      if (lable == lables[k]) {
         mass = masses[k];
      }
   }

   return(0);
}

int read_input_file(int& nmols, std::vector<std::string>& filenames, \
      std::vector<int>& nrings_per_molecule, \
      std::vector<std::vector<int>>& rings_atoms) {

   std::string line, temp;
   char test_char;
   int nrings_total, nfile, counts, finds_nl, rr, temp_int;

   // check input file:
   // for line n
   // if old = m new != f wrong
   // if old = f new != r wrong
   // if old = r new != r or m wrong
   // if wrong cout "there is a mistake at line n

   //----------------------------------
   // count number of molecules,
   // and the total number of rings:
   //----------------------------------

   std::ifstream inputfile;
   inputfile.open("input");
   nmols = 0;
   finds_nl = 0; // finds new line
   nrings_total = 0;
   while (!inputfile.eof()) {
      inputfile.get(test_char);
      if (finds_nl == 0) { // if this is the first character in the line
         if (test_char == 'm') { // and the first character is 'm'
            nmols = nmols + 1;
         }
         if (test_char == 'r') {
            nrings_total = nrings_total + 1;
         }
      }
      finds_nl = finds_nl + 1; // because it's probably not the
                                       // end of the line
      if (test_char == '\n') { // though, if it is the end of the line:
         finds_nl = 0;
      }
   }
   std::cout << "nrings total: " << nrings_total << std::endl;

   //---------------------------------------------
   // get the filenames of the molecules,
   // count the total numbers of rings, and the
   // number of rings per molecule:
   //---------------------------------------------

   filenames.resize(nmols);
   nrings_per_molecule.resize(nmols);
   rings_atoms.resize(nrings_total);
   nfile = -1; // because c++ array numbering starts from 0
   rr = 0; // the current ring being read
   finds_nl = 0; // finds new line
   inputfile.clear(); // To clear the EOF from previously
   inputfile.seekg(0, std::ios::beg); // set to beggining of file
   while (!inputfile.eof()) {
      inputfile.get(test_char);
      if (finds_nl == 0) { // if this is the first character in the line
         if (test_char == 'm') { // we have a new molecule
            nfile = nfile + 1;
            nrings_per_molecule[nfile] = 0; // initialize to 0 rings
         }
         if (test_char == 'f') { // the filename of the new molecule is
                                 // written here
            getline(inputfile,line); // get this line
            std::stringstream ssin(line); // break up line in string stream
            counts = 1;
            while (ssin.good()){
               ssin >> temp;
               if (counts = 2) {
                  filenames[nfile] = temp;
               }
               counts = counts + 1;
            }
            // go back by 1 character so that get reads '\n' from
            // this line which was read by getline:
            inputfile.seekg(-1, std::ios::cur);
         }
         if (test_char == 'r') {
            // there is another ring for this molecule:
            nrings_per_molecule[nfile] = nrings_per_molecule[nfile] + 1;
            // get the atoms that make up this ring
            getline(inputfile,line); // get this line
            std::stringstream ssin(line); // break up line in string stream
            counts = 1;
            while (ssin.good()){
               ssin >> temp;
               std::cout << temp << " ";
               if (counts > 1) {
                  temp_int = std::atoi(temp.c_str());
                  rings_atoms[rr].push_back(temp_int);
               }
               counts = counts + 1;
            }
            std::cout << " " << std::endl;
            // move on to next ring
            rr = rr + 1;
            // go back by 1 character so that get reads '\n' from
            // this line which was read by getline:
            inputfile.seekg(-1, std::ios::cur);
         }
      }
      finds_nl = finds_nl + 1; // because it's probably not the
                                       // end of the line
      if (test_char == '\n') { // though, if it is the end of the line:
         finds_nl = 0;
      }
   }

   inputfile.close();
   return 0;
}


/*
 * Matrix-vector multiply
 *
 * Information on Intel vector pragmas is available in the following link:
 * https://software.intel.com/en-us/cpp-compiler-developer-guide-and-reference-vector-1#58209E46-70EA-4C47-BED6-E69236C6680C
 */

// https://en.cppreference.com/w/cpp/memory/c/aligned_alloc
#include <cstdlib>  // aligned_alloc (C++11), std::aligned_alloc (C++17)
#include <cstring>
#include <ctime>
#include <iostream>
#include <vector>
#include <boost/align/aligned_allocator.hpp>
#include <cmath>
// #include "papi.h"
#include "simd.h"
#include "environ.h"
#include "utils.h"
#include "vutils.h"

///////////////////////////////////////////////////////////////////////////////
// USER CONFIGURATION
///////////////////////////////////////////////////////////////////////////////
// Square matrix dimension
const size_t N = 6;

// Precision for floating-point operations
// Valid values are: 4, 8
#define REAL_TYPE 4

///////////////////////////////////////////////////////////////////////////////
// FP and SIMD
///////////////////////////////////////////////////////////////////////////////
// Precision for floating-point operations
#if REAL_TYPE == 4
typedef float real;
typedef SIMD_FLT vreal;
#elif REAL_TYPE == 8
typedef double real;
typedef SIMD_DBL vreal;
#endif

// Number of floating-point values that fit into a SIMD register
const int SIMD_STREAMS = SIMD_WIDTH_BYTES / sizeof(real);
const int LOG2STREAMS = std::log2(SIMD_STREAMS);

///////////////////////////////////////////////////////////////////////////////
// PROGRAM
///////////////////////////////////////////////////////////////////////////////
template <typename T>
using aligned_vector = std::vector<T, boost::alignment::aligned_allocator<T, SIMD_WIDTH_BYTES>>;

void gemv(
    const size_t n,
    const size_t lda,
    const aligned_vector<real> v1,
    const aligned_vector<real> v2,
    aligned_vector<real> &dp)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < n; col++) {
            dp[row] += v1[row * lda + col] * v2[col];
        }
    }
}

void gemv_simd_tree_sum(
    const size_t n,
    const size_t lda,
    const real *v1,
    const real *v2,
    real *dp)
{
    // const real *_v1 = v1; __SIMD_ASSUME_ALIGNED__(v1);
    // const real *_v2 = v2; __SIMD_ASSUME_ALIGNED__(v2);
    const real *_v1 = (real *)__SIMD_ASSUME_ALIGNED__(v1);
    const real *_v2 = (real *)__SIMD_ASSUME_ALIGNED__(v2);

    for (size_t row = 0; row < n; row++) {
        vreal vdp;
        simd_set_zero(&vdp);
        for (size_t col = 0; col < lda; col+=SIMD_STREAMS) {
            vreal vv1 = simd_load(&_v1[row * lda + col]);
            vreal vv2 = simd_load(&_v2[col]);
            vdp = simd_fmadd(vv1, vv2, vdp);
        }

        // Binary tree sum reduction
        for (size_t i = 0; i < LOG2STREAMS - 1; i++) {
            vdp = simd_hadd(vdp, vdp);
        }

        real tdp[SIMD_STREAMS] __SIMD_ALIGN__;
        simd_store(tdp, vdp);
        // NOTE: 'dp' does need to be aligned because it is used to store a scalar value.
        if (SIMD_WIDTH_BYTES == 16) {
            // HADD from SSE3 does not interleave horizontal sums.
            dp[row] = tdp[0] + tdp[1];
        } else if (SIMD_WIDTH_BYTES == 32) {
            dp[row] = tdp[0] + tdp[SIMD_STREAMS / 2];
        }
    }
}

void print_matrix(
    const size_t n,
    const size_t m,
    const size_t lda,
    const real *v)
{
    for (size_t row = 0; row < n; row++) {
        for (size_t col = 0; col < m; col++) {
            std::cout << v[row * lda + col] << ", ";
        }
        std::cout << std::endl;
    }

}

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

    detectCPU();
    detectSIMD();

    std::cout << "Alignment: " << SIMD_WIDTH_BYTES << std::endl;
    std::cout << "Num. elems: " << SIMD_STREAMS << std::endl;
    std::cout << "Log2 SIMD: " << LOG2STREAMS << std::endl;

    // Number of elements in padded matrix column to conform with SIMD alignment
    const size_t lda = (((N * sizeof(real)) / SIMD_WIDTH_BYTES) * SIMD_WIDTH_BYTES + SIMD_WIDTH_BYTES) / sizeof(real);
    // For unaligned rows, set LDA to N
    // const size_t lda = N;

    // Create a vector of given size
    aligned_vector<real> v1(N * lda);      // matrix
    aligned_vector<real> v2(1 * lda, 1.);  // column vector, set to 1 --> add rows of matrix
    aligned_vector<real> dp(N, 0.);  // resulting column vector (dot products)

    // Zero out extra rows used for padding, to prevent floating-point exception during vector multiplication.
    // This memory elements are never modified, so set once.
    std::memset(v2.data() + N, 0, (lda - N) * sizeof(real));

    // real *v1 = NULL;
    // real *v2 = NULL;
    // real *dp = NULL;
    // scalar_malloc(&v1, SIMD_WIDTH_BYTES, N * lda);
    // scalar_malloc(&v2, SIMD_WIDTH_BYTES, 1 * lda);
    // scalar_malloc(&dp, SIMD_WIDTH_BYTES, N);

    for (size_t k = 0; k < num_matrices; k++) {

        // Initialize vector
        for (size_t row = 0; row < N; row++) {
            for (size_t col = 0; col < N; col++) {
                v1[row * lda + col] = k + 1. * (row + col);
            }
        }

#if defined(DEBUG)
        // Print matrix and vector
        std::cout << "Matrix:" << std::endl;
        print_matrix(N, lda, lda, v1.data());
        std::cout << std::endl;

        std::cout << "Vector:" << std::endl;
        print_matrix(lda, 1, 1, v2.data());
        std::cout << std::endl;
#endif

        // Matrix-vector multiply
        gemv_simd_tree_sum(N, lda, v1.data(), v2.data(), dp.data());
        gemv(N, lda, v1, v2, dp);

#if defined(DEBUG)
        // Print resulting column vector
        std::cout << "Result:" << std::endl;
        print_matrix(N, 1, 1, dp.data());
        std::cout << std::endl;
#endif
    }

    std::cout << "Matrix size: " << N << " x " << lda << std::endl;

    // Only needed if this array was allocated using 'scalar_malloc'.
    // scalar_free(&v1);
    // scalar_free(&v2);
    // scalar_free(&dp);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_RADINDEG_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup trigo_constant
 * \defgroup trigo_constant_radindeg Radindeg
 *
 * \par Description
 * Constant Radindeg : Degree in radian multiplier, \f$\frac{180}\pi\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/radindeg.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_radindeg_(A0)>::type
 *     Radindeg();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Radindeg
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Radindeg, double
                                , 57, 0x42652ee1
                                , 0x404ca5dc1a63c1f8ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Radindeg, Radindeg);
}

namespace nt2
{
  static const long double long_radindeg =  57.295779513082320876798154814105l;
}

#endif


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   CartesianToGeodeticFukushima.hpp
 * Author: jordan
 */

#ifndef CARTESIANTOGEODETICFUKUSHIMA_HPP
#define CARTESIANTOGEODETICFUKUSHIMA_HPP

#ifdef _WIN32
#define _USE_MATH_DEFINES
#include <math.h>
#else
#include <cmath>
#endif

#include <vector>
#include <Eigen/Dense>
#include "../Position.hpp"
#include "../utils/Constants.hpp"

class CartesianToGeodeticFukushima {
    /*
     * This class implements the method given in Fukushima (2006):
     *
     * Transformation from Cartesian to geodetic coordinates
     * accelerated by Halley's method
     * DOI: 10.1007/s00190-006-0023-2
     */
private:

    unsigned int numberOfIterations;

    // Ellipsoid parameters
    double a; // semi-major axis
    double e2; // first eccentricity squared

    // Derived parameters
    double b; // semi-minor axis
    double a_inverse; // 1/a
    double ec; // sqrt(1 - e*e)

public:

    CartesianToGeodeticFukushima(unsigned int numberOfIterations, double a=a_wgs84, double e2=e2_wgs84) :
    numberOfIterations(numberOfIterations), a(a), e2(e2) {
        ec = std::sqrt(1 - e2);
        b = a*ec;
        a_inverse = 1 / a;
    }

    ~CartesianToGeodeticFukushima() {};

    void ecefToLongitudeLatitudeElevation(Eigen::Vector3d & ecefPosition, Position & positionGeographic) {
        double x = ecefPosition(0);
        double y = ecefPosition(1);
        double z = ecefPosition(2);

        // Center of the Earth
        if (x == 0.0 && y == 0.0 && z == 0.0) {
            positionGeographic.setLatitude(0.0);
            positionGeographic.setLongitude(0.0);
            positionGeographic.setEllipsoidalHeight(0.0);
            return;
        }

        // Position at Poles
        if (x == 0.0 && y == 0.0 && z != 0.0) {
            if (z > 0) {
                positionGeographic.setLatitude(M_PI_2*R2D);
            } else {
                positionGeographic.setLatitude(-M_PI_2*R2D);
            }

            positionGeographic.setLongitude(0.0);
            positionGeographic.setEllipsoidalHeight(std::abs(z) - b);
            return;
        }

        double pp = x * x + y * y;
        double p = std::sqrt(pp);

        // Position at Equator
        if (z == 0.0) {
            positionGeographic.setLatitude(0.0);
            positionGeographic.setLongitude(estimateLongitude(x, y, p)*R2D);
            positionGeographic.setEllipsoidalHeight(std::sqrt(x * x + y * y) - a);
            return;
        }

        double P = p*a_inverse;
        double Z = a_inverse * ec * std::abs(z);

        //double R = std::sqrt(pp + z * z);
        std::vector<double> S(numberOfIterations + 1, 0);
        std::vector<double> C(numberOfIterations + 1, 0);

        std::vector<double> D(numberOfIterations + 1, 0);
        std::vector<double> F(numberOfIterations + 1, 0);

        std::vector<double> A(numberOfIterations + 1, 0);
        std::vector<double> B(numberOfIterations + 1, 0);

        S[0] = Z; //starter variables. See (Fukushima, 2006) p.691 equation (17)
        C[0] = ec*P; //starter variables. See (Fukushima, 2006) p.691 equation (17)
        A[0] = std::sqrt(S[0] * S[0] + C[0] * C[0]);
        B[0] = 1.5 * e2 * e2 * P * S[0] * S[0] * C[0] * C[0] * (A[0] - ec); //starter variables. See (Fukushima, 2006) p.691 equation  (18)

        unsigned int iterationNumber = 1;

        while (iterationNumber <= numberOfIterations) {

            D[iterationNumber - 1] =
                    Z * A[iterationNumber - 1] * A[iterationNumber - 1] * A[iterationNumber - 1] +
                    e2 * S[iterationNumber - 1] * S[iterationNumber - 1] * S[iterationNumber - 1];
            F[iterationNumber - 1] =
                    P * A[iterationNumber - 1] * A[iterationNumber - 1] * A[iterationNumber - 1] -
                    e2 * C[iterationNumber - 1] * C[iterationNumber - 1] * C[iterationNumber - 1];

            S[iterationNumber] =
                    D[iterationNumber - 1] * F[iterationNumber - 1] -
                    B[iterationNumber - 1] * S[iterationNumber - 1];
            C[iterationNumber] =
                    F[iterationNumber - 1] * F[iterationNumber - 1] -
                    B[iterationNumber - 1] * C[iterationNumber - 1];

            A[iterationNumber] = std::sqrt(
                    S[iterationNumber] * S[iterationNumber] +
                    C[iterationNumber] * C[iterationNumber]);

            B[iterationNumber] =
                    1.5 *
                    e2 * S[iterationNumber] *
                    C[iterationNumber] * C[iterationNumber] *
                    ((P * S[iterationNumber] - Z * C[iterationNumber]) * A[iterationNumber] -
                    e2 * S[iterationNumber] * C[iterationNumber]);

            ++iterationNumber;
        }

        double lon = estimateLongitude(x, y, p);

        double Cc = ec*C[numberOfIterations];
        double lat = estimateLatitude(z, S[numberOfIterations], Cc);
        double h = estimateHeight(z, p, A[numberOfIterations], S[numberOfIterations], Cc);

        positionGeographic.setLatitude(lat*R2D);
        positionGeographic.setLongitude(lon*R2D);
        positionGeographic.setEllipsoidalHeight(h);
    }

    double estimateLongitude(double x, double y, double p) {
        // Vermeille (2004), stable longitude calculation
        // atan(y/x) suffers when x = 0

        if (y < 0) {
            return -M_PI_2 + 2*std::atan(x/(p - y));
        }

        return M_PI_2 - 2*std::atan(x/(p + y));
    }

    double estimateLatitude(double z, double S, double Cc) {
        double lat = std::abs(std::atan(S / Cc));

        if (z < 0) {
            return -lat;
        }

        return lat;
    }

    double estimateHeight(double z, double p, double A, double S, double Cc) {
        return (p * Cc + std::abs(z) * S - b * A) / std::sqrt(Cc * Cc + S * S);
    }
};

#endif /* CARTESIANTOGEODETICFUKUSHIMA_HPP */


/*
    $ sudo apt-get install libboost-math*

    real   4m15.388s
    user    4m10.272s
    sys 0m0.588s
*/

#include <sstream>
#include <string>
#include <fstream>
#include <iostream>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;

struct Result {
    boost::numeric::ublas::matrix<int> A;
    boost::numeric::ublas::matrix<int> B;
};

int getMatrixSize(string filename) {
    string line;
    ifstream infile;
    infile.open (filename.c_str());
    getline(infile, line);
    return count(line.begin(), line.end(), '\t') + 1;
}

void printMatrix(boost::numeric::ublas::matrix<int> matrix) {
    for (unsigned int i=0; i < matrix.size1(); i++) {
        for (unsigned int j=0; j < matrix.size2(); j++) {
            cout << matrix(i, j);
            if(j+1 != matrix.size2()) {
                cout << "\t";
            }
        }
        cout << endl;
    }
}

Result read(string filename) {
    Result ab;
    string line;
    ifstream infile;
    infile.open (filename.c_str());

    // get dimension
    getline(infile, line);
    int n = getMatrixSize(filename);

    boost::numeric::ublas::matrix<int> A(n,n), B(n,n);

    // process first line
    istringstream iss(line);
    int a, i = 0, j = 0;
    while (iss >> a) {
        A(i,j) = a;
        j++;
    }
    i++;

    while (getline(infile, line) && !line.empty()) {
        istringstream iss(line);
        j = 0;
        while (iss >> a) {
            A(i,j) = a;
            j++;
        }
        i++;
    }

    i = 0;
    while (getline(infile, line)) {
        istringstream iss(line);
        j = 0;
        while (iss >> a) {
            B(i,j) = a;
            j++;
        }
        i++;
    }

    infile.close();
    ab.A = A;
    ab.B = B;
    return ab;
}

int main (int argc, char* argv[]) {
    string filename;
    if (argc < 3) {
        filename = "2000.in";
    } else {
        filename = argv[2];
    }
    Result result = read (filename);

    boost::numeric::ublas::matrix<int> C;
    C = boost::numeric::ublas::prod(result.A, result.B);
    printMatrix(C);

    return 0;
}


/**
 * \file SecondOrderSVFFilter.cpp
 */

#include "SecondOrderSVFFilter.h"

#include <cassert>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename SVFCoefficients>
  struct SecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
    typename SVFCoefficients::DataType iceq1;
    typename SVFCoefficients::DataType iceq2;

    SVFState()
    :iceq1(0), iceq2(0)
    {
    }
  };

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::SecondOrderSVFFilter(int nb_channels)
  :SVFCoefficients(nb_channels), state(new SVFState[nb_channels])
  {
  }

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::~SecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void SecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state.reset(new SVFState[nb_input_ports]);
  }

  template<typename DataType>
  void SecondOrderSVFFilter<DataType>::process_impl(int64_t size) const
  {
    assert(nb_input_ports == nb_output_ports);

    for(int j = 0; j < nb_input_ports; ++j)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[j];
      DataType* ATK_RESTRICT output = outputs[j];

      for(int64_t i = 0; i < size; ++i)
      {
        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = 2 * v1 - state[j].iceq1;
        state[j].iceq2 = 2 * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  SecondOrderSVFBaseCoefficients<DataType>::SecondOrderSVFBaseCoefficients(int nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels), cut_frequency(0), Q(1)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_cut_frequency(DataType_ cut_frequency)
  {
    this->cut_frequency = cut_frequency;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBaseCoefficients<DataType>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  SecondOrderSVFLowPassCoefficients<DataType_>::SecondOrderSVFLowPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFLowPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  SecondOrderSVFBandPassCoefficients<DataType_>::SecondOrderSVFBandPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFBandPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFHighPassCoefficients<DataType_>::SecondOrderSVFHighPassCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFHighPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  SecondOrderSVFNotchCoefficients<DataType_>::SecondOrderSVFNotchCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFNotchCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  SecondOrderSVFPeakCoefficients<DataType_>::SecondOrderSVFPeakCoefficients(int nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFPeakCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFBellCoefficients<DataType_>::SecondOrderSVFBellCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFBellCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFBellCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFLowShelfCoefficients<DataType_>::SecondOrderSVFLowShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFLowShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  SecondOrderSVFHighShelfCoefficients<DataType_>::SecondOrderSVFHighShelfCoefficients(int nb_channels)
  :Parent(nb_channels), gain(0)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType SecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFHighShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

  template class SecondOrderSVFBaseCoefficients<float>;
  template class SecondOrderSVFBaseCoefficients<double>;

  template class SecondOrderSVFLowPassCoefficients<float>;
  template class SecondOrderSVFLowPassCoefficients<double>;
  template class SecondOrderSVFBandPassCoefficients<float>;
  template class SecondOrderSVFBandPassCoefficients<double>;
  template class SecondOrderSVFHighPassCoefficients<float>;
  template class SecondOrderSVFHighPassCoefficients<double>;
  template class SecondOrderSVFNotchCoefficients<float>;
  template class SecondOrderSVFNotchCoefficients<double>;
  template class SecondOrderSVFPeakCoefficients<float>;
  template class SecondOrderSVFPeakCoefficients<double>;
  template class SecondOrderSVFBellCoefficients<float>;
  template class SecondOrderSVFBellCoefficients<double>;
  template class SecondOrderSVFLowShelfCoefficients<float>;
  template class SecondOrderSVFLowShelfCoefficients<double>;
  template class SecondOrderSVFHighShelfCoefficients<float>;
  template class SecondOrderSVFHighShelfCoefficients<double>;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<double> >;
}


#define _USE_MATH_DEFINES

#include <fstream>
#include <iostream>
#include <omp.h>
#include <cmath>
#include <cstdio>
#include <chrono>
#include <string>
#include <armadillo>
#include <vector>
#include "wigner/gaunt.hpp"

using namespace std::chrono;
using namespace arma;

static const double kB = 1.3806504e-23;         // J/K
static const double NA = 6.02214179e23;         // 1/mol
static const double EHARTREE = 4.35974434e-18;  // J/Hartree
static const double AMU = 1.660538921e-27;      // kg/amu
static const double HBAR = 1.054571726e-34;     // J.s
static const double HBAR1 = HBAR / EHARTREE;    // Hartree.s
static const double HBAR2 = HBAR * 1e20 / AMU;  // amu.Å^2/s
static const double SCH4 = 186.25; // J/mol.K

Mat<double> getHamiltonian(int lmax, double Ix, double Iy, double Iz) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number for the spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    std::cout << "Lmax = " << lmax << std::endl;
    double size = (double(1.0/3.0)*(lmax+1)*(2*lmax+1)*(2*lmax+3));
    std::cout << "Dimensions of the matrix: " << size << std::endl;
    Mat<double> H = zeros<mat>(size+1, size+1);
    //SpMat<double> H = sp_mat(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);

    double kap;
    if (A == B && A == C) {     //SPHERICAL ROTOR
        kap = 0;
    } else {
        kap = (2.0*B - (A + C)) / (A - C);
    }

    std::cout << "kappa val is " << kap << "." << std::endl;
    #pragma omp parallel
    {
        double g;
        double a;
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    unsigned long long j = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    //std::cout << j << '\t';
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                unsigned long long i = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0)
                                    + 2*mm*ell + mm + kk;
                                //if (j < i) {
                                //    continue;
                                //} else {
                                //    H(i,j) += 1;
                                //}
                                if (i == j) {
                                    try {
                                        //H(i,j) += B*el*(el+1);
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k-2 >= -el) {
                                            H(i-2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k-1))*sqrt(el*(el+1)-(k-1)*(k-2));
                                            if (abs(double(j - i+2)) != 2) {
                                                std::cout << "(i-2, j) index spacing incorrect @ (" << i-2 << ',' << j << ") --> ("
                                                    << el << ',' << m << ',' << k << ')' << std::endl;
                                            }

                                        }
                                        if (k+2 <= el) {
                                            H(i+2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            if (abs(double(i+2 - j)) != 2) {
                                                std::cout << "(i+2, j) index spacing incorrect @ (" << i+2 << ',' << j << ") --> ("
                                                    << el << ',' << m << ',' << k << ')' << std::endl;
                                            }

                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } // end parallel
    std::cout << std::endl;
    //H.print("H = ");
    return H;
}

SpMat<double> getSparseHam(int lmax, double Ix, double Iy, double Iz) {
    /* Construct the Hamiltonian Matrix
     * Inputs:  lmax;   the maximum quantum number for the spherical basis
     *             I;   the gas-phase moments of inertia [=] amu*Å^2
     * Outputs:    H;   the Hamiltonian matrix (Hermitian) [=] Hartree
     */
    std::cout << "Lmax = " << lmax << std::endl;
    double size = (double(1.0/3.0)*(lmax+1)*(2*lmax+1)*(2*lmax+3));
    std::cout << "Dimensions of the sparse matrix: " << size << std::endl;
    SpMat<double> H = sp_mat(size, size);

    // Define rotational constants:
    double B = HBAR1*HBAR2/(2.0*Iz);
    double A = HBAR1*HBAR2/(2.0*Iy);
    double C = HBAR1*HBAR2/(2.0*Ix);

    double kap;
    if (A == B && A == C) {     //SPHERICAL ROTOR
        kap = 0;
    } else {
        kap = (2.0*B - (A + C)) / (A - C);
    }
    #pragma omp parallel
    {
        double g;
        double a;
        #pragma omp for
        for (int el = 0; el < lmax+1; el++) {
            for (int m = -el; m <= el; m++) {
                for (int k = -el; k <= el; k++) {
                    unsigned long long j = (4*el*el*el/3.0) + 2*el*el + (5*el/3.0) + 2*m*el + m + k;
                    //std::cout << j << '\t';
                    for (int ell = 0; ell < lmax+1; ell++) {
                        for (int mm = -ell; mm <= ell; mm++) {
                            for (int kk = -ell; kk <= ell; kk++) {
                                unsigned long long i = (4*ell*ell*ell/3.0) + 2*ell*ell + (5*ell/3.0)
                                    + 2*mm*ell + mm + kk;
                                //if (j < i) {
                                //    continue;
                                //} else {
                                //    H(i,j) += 1;
                                //}
                                if (i == j) {
                                    try {
                                        //H(i,j) += B*el*(el+1);
                                        H(i,j) += 0.5*(A+C)*el*(el+1) + 0.5*(A-C)*kap*k*k;
                                        if (k-2 >= -el) {
                                            H(i-2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k-1))*sqrt(el*(el+1)-(k-1)*(k-2));
                                            if (isnan(H(i-2,j))) {
                                                std:: cout << "NaN @ (" << i-2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                            if (abs(double(j - i+2)) != 2) {
                                                std::cout << "Index spacing incorrect @ (" << i-2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                        }
                                        if (k+2 <= el) {
                                            H(i+2,j) += 0.25*(C-A)*sqrt(el*(el+1)-k*(k+1))*sqrt(el*(el+1)-(k+1)*(k+2));
                                            if (isnan(H(i+2,j))) {
                                                std:: cout << "NaN @ (" << i+2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                            if (abs(double(i+2 - j)) != 2) {
                                                std::cout << "Index spacing incorrect @ (" << i+2 << ',' << j << ") --> (" << el << ',' << m << ',' << k << ')' << std::endl;
                                            }
                                        }
                                    } catch (const std::exception& e) {
                                        std::cout << "Failure at index: " <<
                                            i << "\t(" << el << ',' << m << ',' <<
                                            k << ')' << std::endl;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } // end parallel
    return H;
}

std::string getDirectory(std::string sysname) {
    std::string dirname = "/Users/lancebettinson/Thesis/umrr/code/hamiltonian-cpp/data";
    if (sysname == "") {
        std::string sysname;
        std::cout << "Enter system name:" << std::endl;
        std::cin >> sysname;
    }
    return dirname+'/'+sysname;
}

Col<double> getCoefficients(std::string sysname) {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"vdat.txt";
    std::ifstream is(filename);
    if (is.fail())
    {
        std::cout << "cannot open file " << filename;
    }
    double theta, phi, v;
    std::vector<double> my_vec;
    while (is) {
        if (!(is >> theta >> phi >> v)) {
            break;
        }
        //std::cout << theta << '\t' << phi << '\t' << v << std::endl;
        my_vec.push_back(v);
    }
    Col<double> cvec = conv_to<vec>::from(my_vec);
    //cvec.print();
    is.close();
    return cvec;
}

std::vector<double> getMomentOfInertia(std::string sysname="METH-CHA") {
    std::string dirname = getDirectory(sysname);
    std::string filename = dirname+'/'+"I.txt";
    std::ifstream is(filename);
    double val;
    std::vector<double> Ivec;
    while (is) {
        if (!(is >> val)) {
            break;
        }
        Ivec.push_back(val);
    }
    return Ivec;
}

double getPartitionFunction(double T, Mat<double>& H, int sym=1) {
    /* Solve the Eigenvalues
     * Inputs:  T;  the temperature [=] K
     *          H;  the Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;

    //Col<double> eigval = eig_sym(H);
    //double Q = 0;
    ////b = 1;
    //int count = 0;
    //for (double e : eigval) {
    //    Q += exp(-b * e);
    //}
    //for (double e : eigval) {
    //    if (count == 5) {
    //        //std::cout << std::endl;
    //    } //std::cout << exp(-b*e)/Q << '\t';
    //    count++;
    //}
    //std::cout << std::endl << "Q predicted by eig_sym: " << Q/sym << std::endl;
    Mat<double> bH = -b*H;
    Mat<double> expbH = expmat_sym(bH);
    double tr = trace(expbH);
    std::cout << pow(b,-1) << std::endl;
    std::cout << std::endl << "Q predicted by eig_sym: " << tr/sym << std::endl;
    return double(tr/sym);
    //return double(Q/sym);
}

double getSparseQ(double T, SpMat<double>& H, int sym=1) {
    /* Solve the Eigenvalues for Sparse Matrix
     * Inputs:  T;  the temperature [=] K
     *          H;  the (sparse) Hamiltonian matrix [=] Hartree
     */
    double b = pow(kB * T, -1) * EHARTREE;
    //SpMat<double> bH = -b*H;

    vec eigval;
    mat eigvec;

    std::cout << "Number of rows: " << H.n_rows << std::endl;
    eigs_sym(eigval, eigvec, H, H.n_rows-1);
    double Q = 0;
    std::cout << "Eigenvalues: " << std::endl;
    for (double e : eigval) {
        std::cout << e << '\t';
        Q += exp(-b * e);
    }
    std::cout << std::endl;
    std::cout << "Q predicted by eigs_sym: " << Q/sym << std::endl;
    return double(Q/sym);
}

int main() {
    auto start = high_resolution_clock::now();

    std::string sysname = "ETH1-CHA";
    int sigma = 1;
    std::string dirname = getDirectory(sysname);

    std::vector<double> Ivec = getMomentOfInertia(sysname);
    std::cout << "Printing I:" << std::endl;
    for (double i : Ivec) {
        std::cout << i << '\t';
    }
    //std::cout << "Rotational Constant: = " << HBAR1*HBAR2/2.0/I << std::endl;
    std::cout << "kT = " << kB*300 / EHARTREE << std::endl;
    //std::cout << "Ratio = " << HBAR1*HBAR2/2.0/I *EHARTREE/(kB*300) << std::endl;
    //std::cout << "Qapprox = " << kB*300/EHARTREE / (HBAR1*HBAR2/2.0/I) << std::endl;

    Col<double> ahat = getCoefficients(sysname);
    std::cout << "Directory is: " << dirname << std::endl;

    /*
     *  Sparse Matrix Implementation
     */
    //SpMat<double> spH = getSparseHam(25, Ivec[0], Ivec[1], Ivec[2]);
    ////spH.print("My sparse Ham =");
    //if (!spH.is_hermitian()) {
    //    std::cout << "NOT HERMITIAN, CHECK" << std::endl;
    //} else {
    //    std::cout << "IS HERMITIAN, GOOD TO GO" << std::endl;
    //}
    //double spQ = getSparseQ(300, spH, 3);
    //std::cout << "spQ = " << spQ << std::endl;

    /*
     *  Dense Matrix Implementation
     */
    Mat<double> H = getHamiltonian(31, Ivec[0], Ivec[1], Ivec[2]);
    if (!H.is_hermitian()) {
        std::cout << "NOT HERMITIAN, CHECK" << std::endl;
    } else {
        std::cout << "HERMITIAN YAY" << std::endl;
    }
    //H.print("My ham:");
    double Q = getPartitionFunction(298, H, sigma);

    /*
     *  Classical Partition Function
     */
    double B = HBAR1*HBAR2/(2.0*Ivec[2]);
    double A = HBAR1*HBAR2/(2.0*Ivec[1]);
    double C = HBAR1*HBAR2/(2.0*Ivec[0]);
    std::cout << "Rotational constants / Hartree:\n" << A << '\t' << B << '\t' << C << '\t' << std::endl;
    std::cout << "kB T / Hartree:\n" << kB * 298 / EHARTREE << std::endl;
    std::cout << "Qapprox = " << sqrt(M_PI)/sigma * sqrt(pow(kB*300/EHARTREE, 3) / (A*B*C));

    std::cout << std::endl;
    std::cout << std::endl;
    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    std::cout << std::endl << duration.count()/1e6 << " secs" << std::endl;
    return 0;
}


// ----------------------------------------------------------------------------
// FILENAME: tylortest.cpp
//
// DESCRIPTION:
//    This file contians the function that used for save the running time of
//    tylor expansion of polynomial shift with original one
//
// AUTHOR: Xinlong Yi
//
// ----------------------------------------------------------------------------

#include "poly.h"
#include <boost/numeric/interval/utility_fwd.hpp>
#include <chrono>
#include <time.h>

static const int kTYLORDEGREE = 6;
static const int digit = 2; // number of digit after point
static const int digit_control = std::pow(10, digit); // controler of digit
static const double max_root = 1000;
// static const double max_root = std::pow(2, kTYLORDEGREE);

/**
 * Get random double in range min to max
 */
double rand_double(double min, double max) {
  double f = (double)rand() / RAND_MAX;
  f = min + f * (max - min);
  f = std::ceil(f * digit_control) / digit_control;
  return f;
}

/**
 * Replace "x" in polynomial with "x+h"
 * Applied Taylor Expansion to this.
 * p(x+h) = p(h) + p'(h)x + 1/2*p''(h)x^2 ... 1/(n!) * p^n(h)*x^n
 * ref: https://math.stackexchange.com/questions/694565/polynomial-shift
 *
 * @tparam n :Maximum degree of poly
 * @param poly :Polynomial
 * @param h :Number add to x
 * @return :Polynomial that replace x in poly to x+h
 */
template <int n> Poly<n> Tylor(const Poly<n> &poly, interval h) {
  if (h.lower() <= 0.0 && h.upper() >= 0.0)
    return poly;

  Poly<n> ret, tmp(poly);
  ret[0] = tmp.ValueAt(h);
  double divisor(1.0);
  for (int index = 1; index <= poly.get_degree(); ++index) {
    divisor *= index;
    tmp.Derivative_();
    ret[index] = (tmp.ValueAt(h) / divisor);
  }

  ret.set_degree();
  return ret;
}

/**
 * Replace "x" in polynomial with "x+h"
 * With original implementation. Start with x+h, then multiply them
 *
 * @tparam n :Maximum degree of poly
 * @param poly :Polynomial
 * @param h :Number add to x
 * @return :Polynomial that replace x in poly to x+h
 */
template <int n> Poly<n> Original(const Poly<n> &poly, interval h) {
  double tmp[2] = {boost::numeric::median(h), 1};
  Poly<n> tmp_poly(tmp, 2), ret, multiplier(tmp, 2);
  ret[0] = poly[0];

  /* TODO :  */
  for (int i = 1; i <= poly.get_degree(); i++) {
    ret += (poly[i] * multiplier);

    for (int j = multiplier.get_degree(); j >= 0; j--) {
      multiplier[j + 1] = multiplier[j] + h * multiplier[j + 1];
    }
    multiplier[0] *= h;
    multiplier.set_degree(multiplier.get_degree() + 1);
  }

  ret.set_degree(poly.get_degree());

  return ret;
}

int main() {
  srand(time(NULL));

  double *coeffs = new double[kTYLORDEGREE];
  for (size_t i = 0; i < kTYLORDEGREE; i++) {
    coeffs[i] = rand_double(-kTYLORDEGREE, kTYLORDEGREE);
  }

  interval h(rand_double(-max_root, max_root));

  Poly<kTYLORDEGREE> test_poly(coeffs, kTYLORDEGREE);
  std::cout << "orig " << test_poly << std::endl;

  // Tylor
  auto tylor_start = std::chrono::high_resolution_clock::now();
  auto r1 = Tylor(test_poly, h);
  auto tylor_end = std::chrono::high_resolution_clock::now();

  //    Original
  auto ori_start = std::chrono::high_resolution_clock::now();
  auto r2 = Original(test_poly, h);
  auto ori_end = std::chrono::high_resolution_clock::now();

  //     Time
  auto tylor_duration = std::chrono::duration_cast<std::chrono::nanoseconds>(
      tylor_end - tylor_start);
  auto ori_duration =
      std::chrono::duration_cast<std::chrono::nanoseconds>(ori_end - ori_start);

  std::cout << "Tylor expansion method takes " << tylor_duration.count()
            << " ns for " << kTYLORDEGREE - 1 << " degree" << std::endl;

  std::cout << "Original method takes " << ori_duration.count() << " ns for "
            << kTYLORDEGREE - 1 << " degree" << std::endl;

  std::cout << r1 << std::endl;
  std::cout << r2 << std::endl;

  interval a = 1.0, b = 3.0;
  interval tmp = a - b;
  std::cout << boost::numeric::median(tmp) << "[" << boost::numeric::width(tmp)
            << "]" << std::endl;

  return 0;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_VectorND_HPP
#define RW_MATH_VectorND_HPP

/**
 * @file VectorND.hpp
 */
#if !defined(SWIG)
#include <rw/common/InputArchive.hpp>
#include <rw/common/OutputArchive.hpp>
#include <rw/common/Serializable.hpp>
#include <rw/core/macros.hpp>

#include <Eigen/Core>
#endif

namespace rw { namespace math {

    /** @addtogroup math */
    /*@{*/

    /**
     * @brief A N-Dimensional Vector
     *
     */
    template< size_t N, class T = double > class VectorND : public rw::common::Serializable
    {
      public:
        //! The type of the internal Eigen Vector
        typedef Eigen::Matrix< T, N, 1 > EigenVectorND;

        //! Value type.
        typedef T value_type;

        /**
         * @brief Creates a N-dimensional VectorND
         */
        VectorND ()
        {
            if (N <= 0) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            _vec = EigenVectorND (N);
        }

        /**
         * @brief Construct a Vector from N arguments
         * @param args [in] a list of arguments
         */
        template< typename... ARGS > VectorND (T arg0, ARGS... args)
        {
            if (N <= 0u) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            size_t i = 1;
            ParamExpansion (i, args...);

            _vec[--i] = arg0;
        }

        /**
         * @brief construct vector from std::vector
         * @param vec [in] the vector to construct from
         */
        VectorND (const std::vector< T >& vec)
        {
            if (N <= 0u) {
                RW_THROW ("Vector to small, N must be larger than 0");
            }
            else if (vec.size () != N) {
                RW_THROW ("Wrong Size vector matrix: N of size:" << N << " and vector of size: "
                                                                 << vec.size () << "given");
            }
            for (size_t i = 0; i < N; i++) {
                _vec[i] = vec[i];
            }
        }

        /**
         * @brief Creates a 3D VectorND from Eigen type.
         *
         * @param v [in] an Eigen vector.
         */
        template< class R > VectorND (const Eigen::MatrixBase< R >& v)
        {
            if (v.cols () != 1 || v.rows () != N)
                RW_THROW ("Unable to initialize VectorND with " << v.rows () << " x " << v.cols ()
                                                                << " matrix");
            _vec = v;
        }

        /**
         * @brief The dimension of the VectorND (i.e. 3).
         * This method is provided to help support generic algorithms using
           size() and operator[].
         */
        size_t size () const { return N; }

        // ###################################################
        // #                 Math Operations                 #
        // ###################################################

        // ########## Eigen Operations

        /**
         * @brief element wise multiplication.
         * @param rhs [in] the vector being multiplied with
         * @return the resulting VectorND
         */
        template< class R > VectorND< N, T > elemMultiply (const Eigen::MatrixBase< R >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] *= rhs[i];
            }
            return ret;
        }

        /**
         * @brief element wise division.
         * @param lhs [in] vector
         * @param rhs [in] vector
         * @return the resulting VectorND
         */
        template< class R > VectorND< N, T > elemDivide (const Eigen::MatrixBase< R >& rhs) const
        {
            VectorND< N, T > ret;
            for (size_t i = 0; i < N; i++) {
                ret[i] = (*this)[i] / rhs[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > VectorND< N, T > operator- (const Eigen::MatrixBase< R >& rhs) const
        {
            return VectorND< N, T > (_vec - rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend VectorND< N, T > operator- (const Eigen::MatrixBase< R >& lhs,
                                           const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs - rhs.e ());
        }

        /**
         * @brief Vector addition.
         */
        template< class R > VectorND< N, T > operator+ (const Eigen::MatrixBase< R >& rhs) const
        {
            return VectorND< N, T > (_vec + rhs);
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R >
        friend VectorND< N, T > operator+ (const Eigen::MatrixBase< R >& lhs,
                                           const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs + rhs.e ());
        }

        // ########## VectorND Operations

        /**
         * @brief element wise division.
         * @param rhs [in] the vector being devided with
         * @return the resulting Vector3D
         */
        VectorND< N, T > elemDivide (const VectorND< N, T >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] /= rhs._vec[i];
            }
            return ret;
        }

        /**
         * @brief Elementweise multiplication.
         * @param rhs [in] vector
         * @return the element wise product
         */
        VectorND< N, T > elemMultiply (const VectorND< N, T >& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] *= rhs._vec[i];
            }
            return ret;
        }

        /**
         * @brief Vector subtraction.
         */
        VectorND< N, T > operator- (const VectorND< N, T >& rhs) const
        {
            return VectorND< N, T > (_vec - rhs._vec);
        }

        /**
         * @brief Vector addition.
         */
        VectorND< N, T > operator+ (const VectorND< N, T >& rhs) const
        {
            return VectorND< N, T > (_vec + rhs._vec);
        }

        /**
         * @brief Unary minus.
         * @brief negative version
         */
        VectorND< N, T > operator- () const { return VectorND< N, T > (_vec * (-1)); }

        // ########## Scalar Operations

        /**
         * @brief Scalar division.
         * @param rhs [in] the scalar to devide with
         * @return result of devision
         */
        VectorND< N, T > operator/ (T rhs) const { return VectorND< N, T > (_vec / rhs); }

#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar division.
         * @param lhs [in] the scalar to devide with
         * @param rhs [out] the vector beind devided
         * @return result of devision
         */
        friend VectorND< N, T > operator/ (T lhs, const VectorND< N, T >& rhs)
        {
            VectorND< N, T > ret = rhs;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = lhs / ret._vec[i];
            }
            return ret;
        }
#endif

        /**
         * @brief Scalar multiplication.
         * @param rhs [in] the scalar to multiply with
         * @return the product
         */
        VectorND< N, T > operator* (T rhs) const { return VectorND< N, T > (_vec * rhs); }

#if !defined(SWIGPYTHON)
        /**
         * @brief Scalar multiplication.
         * @param lhs [in] the scalar to multiply with
         * @param rhs [in] the Vector to be multiplied
         * @return the product
         */
        friend VectorND< N, T > operator* (T lhs, const VectorND< N, T >& rhs)
        {
            return VectorND< N, T > (lhs * rhs._vec);
        }
#endif

        /**
         * @brief Scalar subtraction.
         */
        VectorND< N, T > elemSubtract (const T& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = ret._vec[i] - rhs;
            }
            return ret;
        }

        /**
         * @brief Scalar addition.
         */
        VectorND< N, T > elemAdd (const T& rhs) const
        {
            VectorND< N, T > ret = *this;
            for (size_t i = 0; i < N; i++) {
                ret._vec[i] = ret._vec[i] + rhs;
            }
            return ret;
        }

        // ########### Math Functions

        /**
         * @brief Returns the Euclidean norm (2-norm) of the VectorND
         * @return the norm
         */
        T norm2 () const { return _vec.norm (); }

        /**
         * @brief Returns the Manhatten norm (1-norm) of the VectorND
         * @return the norm
         */
        T norm1 () const { return _vec.template lpNorm< 1 > (); }

        /**
         * @brief Returns the infinte norm (\f$\inf\f$-norm) of the VectorND
         * @return the norm
         */
        T normInf () const { return _vec.template lpNorm< Eigen::Infinity > (); }

        /**
         * @brief calculate the dot product
         * @param vec [in] the vecor to be dotted
         * @return the dot product
         */
        double dot (const VectorND< N, T >& vec) const { return _vec.dot (vec._vec); }

        /**
         * @brief normalize vector to get length 1
         * @return the normalized Vector
         */
        VectorND< N, T > normalize ()
        {
            T length = norm2 ();
            if (length != 0)
                return (*this) / length;
            else
                return VectorND< N, T > ();
        }

        // ###################################################
        // #                Acces Operators                  #
        // ###################################################
#if !defined(SWIG)
        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator() (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator() (size_t i) { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return const reference to element
         */
        const T& operator[] (size_t i) const { return _vec[i]; }

        /**
         * @brief Returns reference to VectorND element
         * @param i [in] index in the VectorND \f$i\in \{0,1,2\} \f$
         * @return reference to element
         */
        T& operator[] (size_t i) { return _vec[i]; }
#else
        ARRAYOPERATOR (T);
#endif
        /**
         * @brief Accessor for the internal Eigen VectorND.
         */
        EigenVectorND& e () { return _vec; }

        /**
           @brief Accessor for the internal Eigen VectorND.
         */
        const EigenVectorND& e () const { return _vec; }
#if !defined(SWIG)
        /**
         * @brief Streaming operator.
         * @param out [in/out] the stream to continue
         * @param v [in] the vector to stream
         * @param reference to \b out
         */
        friend std::ostream& operator<< (std::ostream& out, const VectorND< N, T >& v)
        {
            out << "Vector" << N << "D(";
            for (size_t i = 0; i < N - 1; i++) {
                out << v[i] << ", ";
            }
            out << v[N - 1] << ")";
            return out;
        }
#else
#define VECTORND(num, type) rw::math::VectorND< num, type >
        TOSTRING (VECTORND (N, T));
#undef VECTORND
#endif

        /**
         * @brief converts the vector to a std:vector
         * @return a std::vector
         */
        std::vector< T > toStdVector () const
        {
            std::vector< T > ret;
            for (size_t i = 0; i < N; i++) {
                ret.push_back (_vec[i]);
            }
            return ret;
        }

        // ###################################################
        // #             assignement Operators               #
        // ###################################################

        /**
         * @brief Scalar multiplication.
         */
        VectorND< N, T >& operator*= (double s)
        {
            _vec *= s;
            return *this;
        }

        /**
         * @brief Scalar division.
         */
        VectorND< N, T >& operator/= (double s)
        {
            _vec /= s;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        VectorND< N, T >& operator+= (const VectorND< N, T >& v)
        {
            _vec += v._vec;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        VectorND< N, T >& operator-= (const VectorND< N, T >& v)
        {
            _vec -= v._vec;
            return *this;
        }

        /**
         * @brief copy a vector from eigen type
         * @param r [in] an Eigen Vector
         */
        template< class R > VectorND< N, T >& operator= (const Eigen::MatrixBase< R >& r)
        {
            _vec = r;
            return *this;
        }

        /**
         * @brief Vector addition.
         */
        template< class R > VectorND< N, T >& operator+= (const Eigen::MatrixBase< R >& r)
        {
            _vec += r;
            return *this;
        }

        /**
         * @brief Vector subtraction.
         */
        template< class R > VectorND< N, T >& operator-= (const Eigen::MatrixBase< R >& r)
        {
            _vec -= r;
            return *this;
        }

        // ###################################################
        // #                    Comparetors                  #
        // ###################################################
        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R > bool operator== (const Eigen::MatrixBase< R >& rhs) const
        {
            return this->_vec == rhs;
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        template< class R >
        friend bool operator== (const Eigen::MatrixBase< R >& lhs, const VectorND< N, T >& rhs)
        {
            return lhs == rhs._vec;
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param ths [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R > bool operator!= (const Eigen::MatrixBase< R >& rhs) const
        {
            return !(*this == rhs);
        }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param b [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        template< class R >
        friend bool operator!= (const Eigen::MatrixBase< R >& lhs, const VectorND< N, T >& rhs)
        {
            return !(lhs == rhs);
        }

        /**
         * @brief Compare with \b rhs for equality.
         * @param rhs [in] other vector.
         * @return True if a equals b, false otherwise.
         */
        bool operator== (const VectorND< N, T >& rhs) const { return this->_vec == rhs._vec; }

        /**
         *  @brief Compare with \b rhs for inequality.
         *  @param rhs [in] other vector.
         *  @return True if a and b are different, false otherwise.
         */
        bool operator!= (const VectorND< N, T >& rhs) const { return !(*this == rhs); }

        // ###################################################
        // #                      OTHER                      #
        // ###################################################
#if !defined(SWIG)
        //! @copydoc rw::common::Serializable::write
        void write (rw::common::OutputArchive& oarchive, const std::string& id) const
        {
            oarchive.write (this->toStdVector (), id, "VectorND");
        }

        //! @copydoc rw::common::Serializable::read
        void read (rw::common::InputArchive& iarchive, const std::string& id)
        {
            std::vector< T > result (N, 0);
            iarchive.read (result, id, "VectorND");
            *this = VectorND< N, T > (result);
        }
#endif
#if !defined(SWIG)
        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVectorND () const { return this->e (); }

        /**
         * @brief implicit conversion to EigenVector
         */
        operator EigenVectorND& () { return this->e (); }
#endif
        /**
         * @brief Get zero-initialized vector.
         * @return vector.
         */
        static VectorND< N, T > zero () { return Eigen::Matrix< T, N, 1 >::Zero (); }

      private:
        void ParamExpansion (size_t& i)
        {
            if (i > N) {
                RW_THROW ("Vector to big, argc(" << i << ") != N(" << N << ")");
            }
            else if (i < N) {
                RW_THROW ("Vector to small, argc(" << i << ") != N(" << N << ")");
            }
        }

        template< typename R > void ParamExpansion (size_t& i, R arg)
        {
            ParamExpansion (++i);
            _vec[--i] = T (arg);
        }

        template< typename R, typename... ARGS >
        void ParamExpansion (size_t& i, R arg, ARGS... args)
        {
            ParamExpansion (++i, args...);

            _vec[--i] = T (arg);
        }

        EigenVectorND _vec;
    };

    /**
     * @brief Calculates the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D VectorND cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    const VectorND< ND, T > cross (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        return v1.e ().cross (v2.e ());
        // return cross(v1.e(),v2.e());
    }

    /**
     * @brief Calculates the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param dst [out] the 3D VectorND cross product @f$ \mathbf{v1} \times \mathbf{v2} @f$
     *
     * The 3D VectorND cross product is defined as:
     * @f$
     * \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c}
     *  v1_y * v2_z - v1_z * v2_y \\
     *  v1_z * v2_x - v1_x * v2_z \\
     *  v1_x * v2_y - v1_y * v2_x
     * \end{array}\right]
     * @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    void cross (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2, VectorND< ND, T >& dst)
    {
        dst = v1.e ().cross (v2.e ());
        // dst = cross(v1.m(),v2.m());
    }

    /**
     * @brief Calculates the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the dot product @f$ \mathbf{v1} . \mathbf{v2} @f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T > T dot (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        return v1.e ().dot (v2.e ());
    }

    /**
     * @brief Returns the normalized VectorND \f$\mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} \f$.
     * In case \f$ \|mathbf{v}\| = 0\f$ the zero VectorND is returned.
     * @param v [in] \f$ \mathbf{v} \f$ which should be normalized
     * @return the normalized VectorND \f$ \mathbf{n} \f$
     *
     * @relates VectorND
     */
    template< size_t ND, class T > const VectorND< ND, T > normalize (const VectorND< ND, T >& v)
    {
        // Create a copy
        VectorND< ND, T > res (v);
        res.e ().normalize ();
        return res;
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$ with n
     * determining the sign.
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     * @param n [in] @f$ \mathbf{n} @f$
     *
     * @return the angle
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    double angle (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2,
                  const VectorND< ND, T >& n)
    {
        const VectorND< ND, T > nv1 = normalize (v1);
        const VectorND< ND, T > nv2 = normalize (v2);
        const VectorND< ND, T > nn  = normalize (n);
        return atan2 (dot (nn, cross (nv1, nv2)), dot (nv1, nv2));
    }

    /**
     * @brief Calculates the angle from @f$ \mathbf{v1}@f$ to @f$ \mathbf{v2} @f$
     * around the axis defined by @f$ \mathbf{v1} \times \mathbf{v2} @f$
     * @param v1 [in] @f$ \mathbf{v1} @f$
     * @param v2 [in] @f$ \mathbf{v2} @f$
     *
     * @return the angle
     *
     * @relates VectorND
     */
    template< size_t ND, class T >
    double angle (const VectorND< ND, T >& v1, const VectorND< ND, T >& v2)
    {
        VectorND< ND, T > n = cross (v1, v2);
        return angle (v1, v2, n);
    }

    /**
     * @brief Casts VectorND<N,T> to VectorND<Q>
     * @param v [in] VectorND with type T
     * @return VectorND with type Q
     *
     * @relates VectorND
     */
    template< class Q, size_t ND, class T >
    const VectorND< ND, Q > cast (const VectorND< ND, T >& v)
    {
        VectorND< ND, Q > ret;

        for (size_t i = 0; i < v.size (); i++) {
            ret[i] = static_cast< Q > (v[i]);
        }
        return ret;
    }

    template< class T > using Vector6D = VectorND< 6, T >;

#if !defined(SWIG)
    extern template class rw::math::VectorND< 6, double >;
    extern template class rw::math::VectorND< 6, float >;
    extern template class rw::math::VectorND< 5, double >;
    extern template class rw::math::VectorND< 5, float >;
    extern template class rw::math::VectorND< 4, double >;
    extern template class rw::math::VectorND< 4, float >;
    extern template class rw::math::VectorND< 3, double >;
    extern template class rw::math::VectorND< 3, float >;
    extern template class rw::math::VectorND< 2, double >;
    extern template class rw::math::VectorND< 2, float >;
#else
#define VECTORND(num, type) rw::math::VectorND< num, type >;
    SWIG_DECLARE_TEMPLATE (Vector6Dd, VECTORND (6, double));
    SWIG_DECLARE_TEMPLATE (Vector6Df, VECTORND (6, float));
    SWIG_DECLARE_TEMPLATE (Vector5Dd, VECTORND (5, double));
    SWIG_DECLARE_TEMPLATE (Vector5Df, VECTORND (5, float));
    SWIG_DECLARE_TEMPLATE (Vector4Dd, VECTORND (4, double));
    SWIG_DECLARE_TEMPLATE (Vector4Df, VECTORND (4, float));
#undef VECTORND
#endif

    /**@}*/
}}    // namespace rw::math

#endif    // end include guard