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

#ifndef SMOOTH__FEEDBACK__MPC_FUNC_HPP_
#define SMOOTH__FEEDBACK__MPC_FUNC_HPP_

/**
 * @file
 * @brief Functions for Model-Predictive Control (MPC) on Lie groups.
 */

#include <Eigen/Core>

#include <cassert>
#include <chrono>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

#include "common.hpp"
#include "qp.hpp"

namespace smooth::feedback {

/**
 * @brief Optimal control problem defintiion.
 *
 * @tparam G State space Lie group.
 * @tparam U Input space Lie group.
 *
 * The optimal control problem is
 * \f[
 *   \begin{cases}
 *    \min_{u(\cdot)} & \int_{0}^T \left( (g(t) \ominus g_{des}(t))^T Q (g(t) \ominus g_{des}(t))^T
 *     + (u(t) \ominus u_{des}(t))^T R (u(t) \ominus u_{des}(t))^T \right)
 *     + (g(T) \ominus g_{des}(T))^T Q_T (g(T) \ominus g_{des}(T))               \\
 *    \text{s.t.}    & g(0) = x_0,                                               \\
 *                    & l_G \leq A_G (g(t) \ominus c_G) \leq u_G,                \\
 *                    & l_U \leq A_U (u(t) \ominus c_U) \leq u_U,
 *   \end{cases}
 * \f]
 * where the cost matrices must be positive semi-definite.
 */
template<LieGroup G, Manifold U>
struct OptimalControlProblem
{
  /// State tangent dimension
  static constexpr Eigen::Index Nx = Dof<G>;
  /// Input tangent dimension
  static constexpr Eigen::Index Nu = Dof<U>;

  /// Time horizon
  double T{1};
  /// Initial state
  G x0{Default<G>()};

  /// Desired input trajectory
  std::function<U(double)> udes = [](double) -> U { return Default<U>(); };
  /// Desired state trajectory
  std::function<G(double)> gdes = [](double) -> G { return Default<G>(); };

  /// Input bounds
  ManifoldBounds<U> ulim{};
  /// State bounds
  ManifoldBounds<G> glim{};

  /// MPC weights struct
  struct Weights
  {
    /// Running state cost
    Eigen::Matrix<double, Nx, Nx> Q = Eigen::Matrix<double, Nx, Nx>::Identity();
    /// Final state cost
    Eigen::Matrix<double, Nx, Nx> QT = Eigen::Matrix<double, Nx, Nx>::Identity();
    /// Running input cost
    Eigen::Matrix<double, Nu, Nu> R = Eigen::Matrix<double, Nu, Nu>::Identity();
  };

  /// MPC weights values
  Weights weights{};
};

/**
 * @brief Struct to define a linearization point.
 */
template<LieGroup G, Manifold U>
struct LinearizationInfo
{
  /**
   * @brief state linearization trajectory with first derivative
   * \f$ g_{lin}: \mathbb{R} \rightarrow (G, \mathbb{R}^{\dim \mathfrak{g}}) \f$
   */
  std::function<std::pair<G, Tangent<G>>(double)> g = [](double) -> std::pair<G, Tangent<G>> {
    return {Default<G>(), Tangent<G>::Zero()};
  };

  /**
   * @brief input linearization trajectory \f$ u_{lin}(t) :  \mathbb{R} \rightarrow U \f$
   */
  std::function<U(double)> u = [](double) -> U { return Default<U>(); };

  /**
   * @brief Domain of validity of state linearization
   *
   *  Defines an upper bound \f$ \bar a \f$ s.t. the linearization is valid for \f$ g \f$ s.t.
   * \f[
   *   -\bar a \leq g \ominus_r g_{lin} \leq \bar a
   * \f]
   * holds component-wise.
   */
  Eigen::Matrix<double, Dof<G>, 1> g_domain =
    Eigen::Matrix<double, Dof<G>, 1>::Constant(std::numeric_limits<double>::infinity());
};

/**
 * @brief Allocate QP sparsity pattern (part 1 of ocp_to_qp()).
 *
 * Variables: [x_1, ..., x_K, u_0, ..., u_{K-1}]
 *
 * Constraints:
 *  - Dynamics constraints            (K * Nx)
 *  - Input constraints               (Nu_ineq * Nu)
 *  - State constraints               (Nx_ineq * Nx)
 *  - State linearization constraints (K * Nu)        [optional]
 *
 * @param[in] pbm OptimalControlProblem definition.
 * @param[in] K number of time discretization steps
 * @param[out] qp quadratic program definition with allocated matrices.
 * @param[in] lin_con set to true to allocate K * Nu state linearization constraints
 */
template<LieGroup G, Manifold U>
  requires(Dof<G> > 0 && Dof<U> > 0)
void ocp_to_qp_allocate(
  const OptimalControlProblem<G, U> & pbm,
  std::size_t K,
  QuadraticProgramSparse<double> & qp,
  bool lin_con = false)
{
  // problem info
  static constexpr int Nx = Dof<G>;
  static constexpr int Nu = Dof<U>;

  static_assert(Nx > 0, "State space dimension must be static");
  static_assert(Nu > 0, "Input space dimension must be static");

  const uint32_t n_eq     = K * Nx;
  const uint32_t NU_iq    = K * pbm.ulim.A.rows();
  const uint32_t NX_iq    = K * pbm.glim.A.rows();
  const uint32_t NXLIN_iq = lin_con ? K * Nu : 0;

  const uint32_t nvar = K * Nx + K * Nu;
  const uint32_t ncon = n_eq + NU_iq + NX_iq + NXLIN_iq;

  // Matrix sizes
  qp.P.resize(nvar, nvar);
  qp.q.resize(nvar);

  qp.A.resize(ncon, nvar);
  qp.l.resize(ncon);
  qp.u.resize(ncon);

  // SPARSITY PATTERN FOR P

  Eigen::Matrix<int, -1, 1> Pp(nvar);
  for (auto k = 0u; k != K; ++k) {
    for (auto i = 0u; i != Nu; ++i) { Pp[k * Nu + i] = 1 + i; }
  }

  for (auto k = 0u; k != K; ++k) {
    for (auto i = 0u; i != Nx; ++i) { Pp[K * Nu + k * Nx + i] = 1 + i; }
  }

  // SPARSITY PATTERN FOR A

  Eigen::Matrix<int, -1, 1> Ap(ncon);
  int Arow = 0;

  // state constraint k = 0
  Ap.segment(Arow, Nx).setConstant(1 + Nu);
  Arow += Nx;

  // state constraints k = 1, ... K
  Ap.segment(Arow, (K - 1) * Nx).setConstant(1 + Nu + Nx);
  Arow += (K - 1) * Nx;

  // input constraints
  if (NU_iq > 0) { Ap.segment(Arow, NU_iq).setConstant(Nu); }
  Arow += NU_iq;

  // state constraints
  if (NX_iq > 0) { Ap.segment(Arow, NX_iq).setConstant(Nx); }
  Arow += NX_iq;

  // state linearization constraints
  if (NXLIN_iq > 0) { Ap.segment(Arow, NXLIN_iq).setConstant(1); }

  qp.P.reserve(Pp);
  qp.A.reserve(Ap);
}

/**
 * @brief Fill QP matrices (part 2 of ocp_to_qp()).
 */
template<LieGroup G, Manifold U, typename Dyn, diff::Type DT = diff::Type::Default>
  requires(Dof<G> > 0 && Dof<U> > 0)
void ocp_to_qp_fill(
  const OptimalControlProblem<G, U> & pbm,
  std::size_t K,
  const Dyn & f,
  const LinearizationInfo<G, U> & lin,
  QuadraticProgramSparse<double> & qp)
{
  using std::placeholders::_1;

  static constexpr int Nx = Dof<G>;
  static constexpr int Nu = Dof<U>;
  const int NU            = K * Nu;

  static_assert(Nx > 0, "State space dimension must be static");
  static_assert(Nu > 0, "Input space dimension must be static");

  const bool lin_con = lin.g_domain.minCoeff() < std::numeric_limits<double>::infinity();

  const uint32_t Nu_iq    = pbm.ulim.A.rows();
  const uint32_t Nx_iq    = pbm.glim.A.rows();
  const uint32_t Nxlin_iq = lin_con ? Nx : 0;

  const double dt = pbm.T / static_cast<double>(K);

  ////////////////////
  /// FILL A, l, u ///
  ////////////////////

  assert(static_cast<uint32_t>(qp.A.rows()) == K * (Nx + Nu_iq + Nx_iq + Nxlin_iq));
  assert(static_cast<uint32_t>(qp.A.cols()) == K * (Nx + Nu));
  assert(static_cast<uint32_t>(qp.l.rows()) == K * (Nx + Nu_iq + Nx_iq + Nxlin_iq));
  assert(static_cast<uint32_t>(qp.u.rows()) == K * (Nx + Nu_iq + Nx_iq + Nxlin_iq));

  int Arow = 0;

  for (auto k = 0u; k != K; ++k) {
    using AT = Eigen::Matrix<double, Nx, Nx>;
    using BT = Eigen::Matrix<double, Nx, Nu>;
    using ET = Eigen::Matrix<double, Nx, 1>;

    const double t = k * dt;

    // LINEARIZATION

    const auto [xl, dxl] = lin.g(t);
    const auto ul        = lin.u(t);

    // clang-format off
    const auto [flin, df_xu] = diff::dr<1, DT>(
      [&f, &t]<typename T>(const CastT<T, G> & vx, const CastT<T, U> & vu) -> Tangent<CastT<T, G>> {
        return f(t, vx, vu);
      },
      wrt(xl, ul)
    );
    // clang-format on

    // cltv system \dot x = At x(t) + Bt u(t) + Et
    const AT At = -0.5 * ad<G>(flin) - 0.5 * ad<G>(dxl) + df_xu.template leftCols<Nx>();
    const BT Bt = df_xu.template rightCols<Nu>();
    const ET Et = flin - dxl;

    // TIME DISCRETIZATION

    const AT At2     = At * At;
    const double dt2 = dt * dt;
    const double dt3 = dt2 * dt;

    // dltv system x^+ = Ak x + Bk u + Ek by truncated taylor expansion of the matrix exponential
    const AT Ak = AT::Identity() + At * dt + At2 * dt2 / 2. + At2 * At * dt3 / 6.;
    const BT Bk = Bt * dt + At * Bt * dt2 / 2. + At2 * Bt * dt3 / 6.;
    const ET Ek = Et * dt + At * Et * dt2 / 2. + At2 * Et * dt3 / 6.;

    // DYNAMICS CONSTRANTS

    if (k == 0) {
      // x(1) - B u(0) = A x0 + E

      // identity matrix on x(1)
      for (auto i = 0u; i != Nx; ++i) {
        const Eigen::Index Ari = Nx * k + i;
        assert(qp.A.outerIndexPtr()[Ari + 1] - qp.A.outerIndexPtr()[Ari] == Nu + 1);

        // B matrix on u0
        for (auto j = 0u; j != Nu; ++j) { qp.A.coeffRef(Ari, Nu * k + j) = -Bk(i, j); }

        // Identity matrix on x1
        qp.A.coeffRef(Ari, NU + Nx * k + i) = 1;
      }

      // B matrix on u(0)
      for (auto i = 0u; i != Nx; ++i) {}

      qp.u.template segment<Nx>(Nx * k) = Ak * rminus(pbm.x0, xl) + Ek;
      qp.l.template segment<Nx>(Nx * k) = qp.u.template segment<Nx>(Nx * k);
    } else {
      // x(k+1) - A x(k) - B u(k) = E
      for (auto i = 0u; i != Nx; ++i) {
        const Eigen::Index Ari = Nx * k + i;
        assert(qp.A.outerIndexPtr()[Ari + 1] - qp.A.outerIndexPtr()[Ari] == Nu + Nx + 1);

        // B matrix on u(k)
        for (auto j = 0u; j != Nu; ++j) { qp.A.coeffRef(Ari, Nu * k + j) = -Bk(i, j); }

        // A matrix on x(k)
        for (auto j = 0u; j != Nx; ++j) { qp.A.coeffRef(Ari, NU + Nx * (k - 1) + j) = -Ak(i, j); }

        // identity matrix on x(k+1)
        qp.A.coeffRef(Ari, NU + Nx * k + i) = 1;
      }

      qp.u.template segment<Nx>(Nx * k) = Ek;
      qp.l.template segment<Nx>(Nx * k) = Ek;
    }
  }
  Arow += K * Nx;

  // INPUT CONSTRAINTS

  if (Nu_iq > 0) {
    for (auto k = 0u; k < K; ++k) {
      for (auto i = 0u; i != Nu_iq; ++i) {
        const Eigen::Index Ari = Arow + k * Nu_iq + i;
        assert(qp.A.outerIndexPtr()[Ari + 1] - qp.A.outerIndexPtr()[Ari] == Nu);

        for (auto j = 0u; j != Nu; ++j) { qp.A.coeffRef(Ari, k * Nu + j) = pbm.ulim.A(i, j); }
      }
      // clang-format off
      qp.l.segment(Arow + k * Nu_iq, Nu_iq) = pbm.ulim.l - pbm.ulim.A * rminus(lin.u(k * dt), pbm.ulim.c);
      qp.u.segment(Arow + k * Nu_iq, Nu_iq) = pbm.ulim.u - pbm.ulim.A * rminus(lin.u(k * dt), pbm.ulim.c);
      // clang-format on
    }
  }
  Arow += K * Nu_iq;

  // STATE CONSTRAINTS

  if (Nx_iq > 0) {
    for (auto k = 1u; k != K + 1; ++k) {
      for (auto i = 0u; i != Nx_iq; ++i) {
        const Eigen::Index Ari = Arow + (k - 1) * Nx_iq + i;
        assert(qp.A.outerIndexPtr()[Ari + 1] - qp.A.outerIndexPtr()[Ari] == Nx);

        for (auto j = 0u; j != Nx; ++j) {
          qp.A.coeffRef(Ari, NU + (k - 1) * Nx + j) = pbm.glim.A(i, j);
        }
      }
      // clang-format off
      qp.l.segment(Arow + (k - 1) * Nx_iq, Nx_iq) = pbm.glim.l - pbm.glim.A * rminus(lin.g(k * dt).first, pbm.glim.c);
      qp.u.segment(Arow + (k - 1) * Nx_iq, Nx_iq) = pbm.glim.u - pbm.glim.A * rminus(lin.g(k * dt).first, pbm.glim.c);
      // clang-format on
    }
  }
  Arow += K * Nx_iq;

  // STATE LINEARIZATION BOUNDS

  if (Nxlin_iq > 0) {
    for (auto k = 1u; k < K + 1; ++k) {
      for (auto i = 0u; i != Nxlin_iq; ++i) {
        const Eigen::Index Ari = Arow + (k - 1) * Nxlin_iq + i;
        assert(qp.A.outerIndexPtr()[Ari + 1] - qp.A.outerIndexPtr()[Ari] == 1);

        qp.A.coeffRef(Ari, NU + (k - 1) * Nx + i) = 1.;
      }
      qp.l.segment(Arow + (k - 1) * Nxlin_iq, Nxlin_iq) = -lin.g_domain;
      qp.u.segment(Arow + (k - 1) * Nxlin_iq, Nxlin_iq) = lin.g_domain;
    }
  }
  Arow += K * Nxlin_iq;

  ////////////////
  /// FILL P,q ///
  ////////////////

  assert(static_cast<uint32_t>(qp.P.cols()) == K * (Nx + Nu));
  assert(static_cast<uint32_t>(qp.P.cols()) == K * (Nx + Nu));
  assert(static_cast<uint32_t>(qp.q.rows()) == K * (Nx + Nu));

  // INPUT COSTS

  for (auto k = 0u; k < K; ++k) {
    for (auto i = 0u; i != Nu; ++i) {
      const Eigen::Index Pci = k * Nu + i;
      assert(
        qp.P.outerIndexPtr()[Pci + 1] - qp.P.outerIndexPtr()[Pci]
        == static_cast<decltype(qp.P)::StorageIndex>(i) + 1);

      for (auto j = 0u; j != i + 1; ++j) {
        qp.P.coeffRef(k * Nu + j, Pci) = pbm.weights.R(j, i) * dt;
      }
    }
    qp.q.segment(k * Nu, Nu) = pbm.weights.R * rminus(lin.u(k * dt), pbm.udes(k * dt)) * dt;
  }

  // STATE COSTS

  // intermediate states x(1) ... x(K-1)
  for (auto k = 1u; k < K; ++k) {
    for (auto i = 0u; i != Nx; ++i) {
      const Eigen::Index Pci = NU + (k - 1) * Nx + i;
      assert(
        qp.P.outerIndexPtr()[Pci + 1] - qp.P.outerIndexPtr()[Pci]
        == static_cast<decltype(qp.P)::StorageIndex>(i) + 1);

      for (auto j = 0u; j != i + 1; ++j) {
        qp.P.coeffRef(NU + (k - 1) * Nx + j, Pci) = pbm.weights.Q(j, i) * dt;
      }
    }
    qp.q.segment(NU + (k - 1) * Nx, Nx) =
      pbm.weights.Q * rminus(lin.g(k * dt).first, pbm.gdes(k * dt)) * dt;
  }

  // last state x(K) ~ x(T)
  for (auto i = 0u; i != Nx; ++i) {
    const Eigen::Index Pci = NU + (K - 1) * Nx + i;
    assert(
      qp.P.outerIndexPtr()[Pci + 1] - qp.P.outerIndexPtr()[Pci]
      == static_cast<decltype(qp.P)::StorageIndex>(i) + 1);

    for (auto j = 0u; j != i + 1; ++j) {
      qp.P.coeffRef(NU + (K - 1) * Nx + j, Pci) = pbm.weights.QT(j, i);
    }
  }
  qp.q.segment(NU + (K - 1) * Nx, Nx) =
    pbm.weights.QT * rminus(lin.g(pbm.T).first, pbm.gdes(pbm.T));

  qp.A.makeCompressed();
  qp.P.makeCompressed();
}

/**
 * @brief Convert OptimalControlProblem on \f$ (\mathbb{G}, \mathbb{U}) \f$ into a tangent space
 * QuadraticProgramSparse on \f$ (\mathbb{R}^{\dim \mathfrak{g}}, \mathbb{R}^{\dim \mathfrak{u}})
 * \f$.
 *
 * The OptimalControlProblem is encoded into a QuadraticProgram via linearization around
 * \f$(g_{lin}(t), u_{lin}(t))\f$ followed by time discretization. The variables of the QP are \f[
 * \begin{bmatrix} \mu_0 & \mu_1 & \ldots & \mu_{K - 1} & x_1 & x_2 & \ldots & x_K
 * \end{bmatrix}, \f] where the discrete time index \f$k\f$ corresponds to time \f$t_k = k
 * \frac{T}{K} \f$ for \f$ k = 0, 1, \ldots, K \f$.
 *
 * performance.
 * @tparam G problem state group type \f$ \mathbb{G} \f$
 * @tparam U problem input group type \f$ \mathbb{U} \f$
 * @tparam Dyn dynamics functor type
 * @tparam DT differentiation method to utilize
 *
 * @param pbm optimal control problem
 * @param K number of discretization points. More points create a larger QP, but the distance \f$
 * T / K \f$ between points should be smaller than the smallest system time constant for adequate
 * @param f dynamics \f$ f : \mathbb{R} \times \mathbb{G} \times \mathbb{U} \rightarrow
 * \mathbb{R}^{\dim \mathfrak g}\f$ s.t. \f$ \mathrm{d}^r g_t = f(t, g, u) \f$
 * @param lin linearization point
 *
 * @return QuadraticProgramSparse modeling the input optimal control problem.
 *
 * @note Given a solution \f$(x^*, \mu^*)\f$ to the QuadraticProgramSparse, the corresponding
 * solution to the OptimalControlProblem is \f$ u^*(t) = u_{lin}(t) \oplus \mu^*(t) \f$ and the
 * optimal trajectory is \f$ g^*(t) = g_{lin}(t) \oplus x^*(t) \f$.
 *
 * @note Constraints are added as \f$ A x \leq b - A (g_{lin} - c) \f$ and similarly for the
 * input. Beware of using constraints on non-Euclidean spaces.
 *
 * @note \p f must be differentiable w.r.t. \f$ g \f$ and \f$ u \f$  with the default \p
 * smooth::diff method (check \p smooth::diff::DefaultType). If using an automatic differentiation
 * method this means that it must be templated on the scalar type.
 */
template<LieGroup G, Manifold U, typename Dyn, diff::Type DT = diff::Type::Default>
QuadraticProgramSparse<double> ocp_to_qp(
  const OptimalControlProblem<G, U> & pbm,
  std::size_t K,
  const Dyn & f,
  const LinearizationInfo<G, U> & lin)
{
  bool lin_con = lin.g_domain.minCoeff() < std::numeric_limits<double>::infinity();

  QuadraticProgramSparse<double> qp;

  ocp_to_qp_allocate<G, U>(pbm, K, qp, lin_con);
  ocp_to_qp_fill<G, U>(pbm, K, f, lin, qp);

  return qp;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__MPC_FUNC_HPP_


#include "FloatingObject.hpp"
#include <algorithm>
#include <vector>
#include <mutex>
#include <shared_mutex>
#include <Eigen\Geometry>

#define NOMINMAX
#include <Windows.h>

namespace {
	const float pi = 3.14159265359;
	constexpr float AIR_DENSITY = 1.293e-9; // [kg/mm3]
}

namespace dynaman {
	FloatingObject::FloatingObject(
		const Eigen::Vector3f& positionTarget,
		const Eigen::Vector3f& lowerbound,
		const Eigen::Vector3f& upperbound,
		float radius,
		float weight
	):
		m_position(positionTarget),
		m_velocity(Eigen::Vector3f::Zero()),
		m_integral(Eigen::Vector3f::Zero()),
		m_lowerbound(lowerbound),
		m_upperbound(upperbound),
		m_weight(weight),
		m_radius(radius),
		m_is_tracked(false),
		m_lastDeterminationTime(0),
		velocityBufferSize(3),
		positionBuffer(velocityBufferSize, positionTarget),
		velocityBuffer(velocityBufferSize, Eigen::Vector3f::Zero()),
		dTBuffer(velocityBufferSize, 1),
		m_pTrajectory(std::make_shared<TrajectoryConstantState>(positionTarget)
		)
	{}

	FloatingObjectPtr FloatingObject::Create(
		const Eigen::Vector3f& posTgt,
		const Eigen::Vector3f& lowerbound,
		const Eigen::Vector3f& upperbound,
		float radius,
		float weight
	){
		return std::make_shared<FloatingObject>(posTgt, lowerbound, upperbound, radius, weight);
	}

	float FloatingObject::sphereMass()
	{
		return AIR_DENSITY * 4.0f * pi * m_radius * m_radius * m_radius / 3.0f;
	}

	float FloatingObject::radius() {
		return m_radius;
	}

	float FloatingObject::weight()
	{
		return m_weight;
	}

	float FloatingObject::totalMass()
	{
		return sphereMass() + weight();
	}

	DWORD FloatingObject::lastDeterminationTime() {
		std::lock_guard<std::mutex> lock(mtxState);
		return m_lastDeterminationTime;
	}

	Eigen::Vector3f FloatingObject::position()
	{
		std::lock_guard<std::mutex> lock(mtxState);
		return m_position;
	}

	Eigen::Vector3f FloatingObject::velocity()
	{
		std::lock_guard<std::mutex> lock(mtxState);
		return averageVelocity(this->velocityBuffer, this->dTBuffer);
	}

	Eigen::Vector3f FloatingObject::integral()
	{
		std::lock_guard<std::mutex> lock(mtxState);
		return m_integral;
	}

	Eigen::Vector3f FloatingObject::positionTarget(DWORD systime_ms)
	{
		std::shared_lock<std::shared_mutex> lock(mtxTrajectory);
		return m_pTrajectory->pos(systime_ms);
	}

	Eigen::Vector3f FloatingObject::velocityTarget(DWORD systime_ms)
	{
		std::shared_lock<std::shared_mutex> lock(mtxTrajectory);
		return m_pTrajectory->vel(systime_ms);
	}

	Eigen::Vector3f FloatingObject::accelTarget(DWORD systime_ms)
	{
		std::shared_lock<std::shared_mutex> lock(mtxTrajectory);
		return m_pTrajectory->accel(systime_ms);
	}

	void FloatingObject::getStates(
		Eigen::Vector3f& pos,
		Eigen::Vector3f& vel,
		Eigen::Vector3f& integ
	) {
		std::lock_guard<std::mutex> lock(mtxState);
		pos = m_position;
		vel = averageVelocity(this->velocityBuffer, this->dTBuffer);
		integ = m_integral;
	}

	void FloatingObject::updateStates(DWORD determinationTime, const Eigen::Vector3f& positionNew)
	{
		std::lock_guard<std::mutex> lock(mtxState);
		float dt = (float)(determinationTime - m_lastDeterminationTime) / 1000.f; // [sec]
		m_velocity = (positionNew - m_position) / dt;
		dTBuffer.push_back(dt);
		dTBuffer.pop_front();
		positionBuffer.push_back(positionNew);
		positionBuffer.pop_front();
		velocityBuffer.push_back(m_velocity);
		velocityBuffer.pop_front();
		if (isTracked())
		{
			m_integral += (0.5f * (positionNew + m_position) - positionTarget()) * dt;
		}
		m_position = positionNew;
		m_lastDeterminationTime = determinationTime;
	}

	void FloatingObject::resetIntegral() {
		std::lock_guard<std::mutex> lock(mtxState);
		m_integral.setZero();
	}

	void FloatingObject::getStatesTarget(
		Eigen::Vector3f& posTgt,
		Eigen::Vector3f& velTgt,
		Eigen::Vector3f& accelTgt,
		DWORD time
	) {
		std::lock_guard<std::shared_mutex> lock(mtxTrajectory);
		posTgt = m_pTrajectory->pos(time);
		velTgt = m_pTrajectory->vel(time);
		accelTgt = m_pTrajectory->accel(time);
	}

	void FloatingObject::updateStatesTarget(
		const Eigen::Vector3f& _positionTarget,
		const Eigen::Vector3f& _velocityTarget,
		const Eigen::Vector3f& _accelTarget)
	{
		auto constTrajPtr = TrajectoryConstantState::Create(_positionTarget, _velocityTarget, _accelTarget);
		setTrajectory(constTrajPtr);
	}

	bool FloatingObject::isTracked() {
		std::lock_guard<std::mutex> lock(mtxTrack);
		return m_is_tracked;
	}

	void FloatingObject::setTrackingStatus(bool is_tracked) {
		std::lock_guard<std::mutex> lock(mtxTrack);
		this->m_is_tracked = is_tracked;
	}

	Eigen::Vector3f FloatingObject::averageVelocity(
		std::deque<Eigen::Vector3f> velocityBuffer,
		std::deque<float> intervalBuffer
	) {
		auto itrVel = velocityBuffer.begin();
		auto itrDT = intervalBuffer.begin();
		Eigen::Vector3f distSum(0.f, 0.f, 0.f);
		float period = 0;
		while (itrVel != velocityBuffer.end())
		{
			distSum += (*itrDT) * (*itrVel);
			period += *itrDT;
			itrDT++;
			itrVel++;
		}
		return distSum /= period;
	}

	Eigen::Vector3f FloatingObject::averagePosition() {
		Eigen::Vector3f posAverage(0, 0, 0);
		std::lock_guard<std::mutex> lock(mtxState);
		for (auto itr = positionBuffer.begin(); itr != positionBuffer.end(); itr++) {
			posAverage += *itr;
		}
		return posAverage / positionBuffer.size();
	}

	bool FloatingObject::isInsideWorkspace()
	{
		auto pos = position();
		Eigen::Vector3f v0 = pos - lowerbound();
		Eigen::Vector3f v1 = pos - upperbound();
		return (v0.x() * v1.x() <= 0) && (v0.y() * v1.y() <= 0) && (v0.z() * v1.z() <= 0);
	}

	void FloatingObject::setTrajectory(std::shared_ptr<Trajectory> newTrajectoryPtr)
	{
		std::lock_guard<std::shared_mutex> lock(mtxTrajectory);
		m_pTrajectory = newTrajectoryPtr;
	}

	Eigen::Vector3f FloatingObject::lowerbound() {
		return m_lowerbound;
	}

	Eigen::Vector3f FloatingObject::upperbound() {
		return m_upperbound;
	}

	std::string FloatingObject::logCtrlHeader() const {
		return "sys_time,x,y,z,vx,vy,vz,ix,iy,iz,xTgt,yTgt,zTgt,vxTgt,vyTgt,vzTgt,axTgt,ayTgt,azTgt";
	}

	std::ofstream& operator<<(std::ostream& ofs, FloatingObjectPtr pObject) {
		DWORD time = pObject->lastDeterminationTime();
		Eigen::Vector3f pos, vel, integ, posTgt, velTgt, accelTgt;
		pObject->getStates(pos, vel, integ);
		pObject->getStatesTarget(posTgt, velTgt, accelTgt, time);
		ofs
			<< time << ","
			<< pos.x() << "," << pos.y() << "," << pos.z() << ","
			<< vel.x() << "," << vel.y() << "," << vel.z() << ","
			<< integ.x() << "," << integ.y() << "," << integ.z() << ","
			<< posTgt.x() << "," << posTgt.y() << "," << posTgt.z() << ","
			<< velTgt.x() << "," << velTgt.y() << "," << velTgt.z() << ","
			<< accelTgt.x() << "," << accelTgt.y() << "," << accelTgt.z();
	}
}


#ifndef _CHEBSPEC_CPP_
#define _CHEBSPEC_CPP_

#include <armadillo>

#include "solvspec.cpp"
#include "../include/chebspec.h"

using namespace std;
using namespace arma;

const double PI = 3.141592653589793;

int ChebSpec::N = -1;
int ChebSpec::L = 0;
vector<double> ChebSpec::x = vector<double>();
vector<vector<double> > ChebSpec::Tm = vector<vector<double> >();
vector<vector<double> > ChebSpec::TmInt = vector<vector<double> >();
vector<vector<double> > ChebSpec::D = vector<vector<double> >();
vector<vector<double> > ChebSpec::D2 = vector<vector<double> >();

ChebSpec::ChebSpec(){}

void ChebSpec::setPotential(const vector<double> &XX, const vector<double> &VV)
{
  SolvSpec::setPotential(XX, VV);
  a = XX[0];
  b = XX[XX.size() - 1];
  scal = pow((b - a) / 2, 2.0);
}

void ChebSpec::findSpectrum(int nEigen)
{
  if(PotVals.size() < 2)
  {
    cout << "Please use the setPotential() function before using this one." << endl;
    return;
  }
  // compute V
  V.resize(L);
  for(int i = 0; i < L; i++)
  {
    V[i] = scal * potFunc.interp(0.5 * ((b - a) * x[i] + b + a));
  }
  // compute Ehat
  Ehat.resize(L);
  for(int i = 0; i < L; i++)
  {
    Ehat[i].resize(L);
    for(int j = 0; j < L; j++)
    {
      Ehat[i][j] = D2[i][j] * V[j];
    }
  }
  // compute UE and US
  UE.resize(L);
  US.resize(L);
  for(int i = 0; i < L; i++)
  {
    UE[i].resize(L);
    US[i].resize(L);
    for(int j = 0; j < L; j++)
    {
      UE[i][j] = 0.5 * (x[i] + 1) * D2[N][j];
      US[i][j] = 0.5 * (x[i] + 1) * Ehat[N][j];
    }
  }
  // compute the A and B matrices, remove the first/last row/column
  int Nr = N - 1;
  mat A(Nr, Nr);
  mat B(Nr, Nr);

  //lapack routines use the column-major order!
  for(int i = 0; i < Nr; i++)
  {
    for(int j = 0; j < Nr; j++)
    {
      // define A
      A(i, j) = -US[i + 1][j + 1] + Ehat[i + 1][j + 1];
      if(i == j)   // add the identity matrix
      {
        A(i, j) = A[i + j * Nr] - 1;
      }
      // now B
      B(i, j) = D2[i + 1][j + 1] - UE[i + 1][j + 1];
    }
  }

  mat B1A = inv(B) * A;
  cx_vec cxeigval;
  cx_mat cxeigvec;

  // diagonalize
  eig_gen(cxeigval, cxeigvec, B1A);
  mat eigvec = real(cxeigvec);
  vec eigval = real(cxeigval);
  // we need to sort this
  for(int i = 0; i < Nr; i++)
  {
    for(int j = 0; j < Nr; j++)
    {
      if(eigval(i) < eigval(j))
      {
        double temp = eigval(i);
        eigval(i) = eigval(j);
        eigval(j) = temp;
        eigvec.swap_cols(i, j);
      }
    }
  }
  // now we just need to put everything in our internal format
  // finally safelly add all the modes found to the spectrum (already in a nice way)
  spectrum.clear();
  spectrum.potential.push_back(X); spectrum.potential.push_back(PotVals);
  for (int i = 0; i < nEigen; i++)
  {
    // build the wavefunction
    vector<Point> wf;
    for(int j = 0; j < Nr; j++)
    {
      wf.push_back(Point(0.5 * ((b - a) * x[j] + b + a), eigvec(j,i)));
    }
    // normalization loop
    double c = 0;
    for(int j = 0; j < Nr; j++)
    {
      c += D[N][j + 1] * wf[j].y * wf[j].y * 0.5 * (b - a);
    }
    // set all the wavefunctions start growing positive from the left
    double s = 1;
    // get the sign of the derivative, this is important since it may be the case
    // that the routine return the same eigenvector with a different sign, in that case
    // the overall coefficient we would like to fit would have change the sign
    for(int j = 4; j < Nr; j++)
    {
      // filter the data and compute the derivative
      double der = (25/12)*int(1e3 * wf[j].y)-4*int(1e3 * wf[j-1].y)+3*int(1e3 * wf[j-2].y)-(4/3) * int(1e3 * wf[j-3].y)+(1/4)* int(1e3 * wf[j-4].y);
      der /= 1e3;
      if(abs(der) > 1e-2)
      {
        s = der / abs(der);
        break;
      }
    }
    for(int j = 0; j < Nr; j++)
    {
      wf[j].y = s * wf[j].y / sqrt(c);
    }
    Mode m(eigval(i) / scal, wf);
    spectrum.addMode(m);
  }
}

void ChebSpec::showMatrix(double* A, int Nr)
{
  cout << endl;
  for(int i = 0; i < Nr; i++)
  {
    for(int j = 0; j < Nr; j++)
    {
      cout << A[i + j * Nr] << " ";
    }
    cout << endl;
  }
}

ChebSpec::~ChebSpec(){}

// [[Rcpp::export]]
void chebSetN(int n)
{
  if(ChebSpec::N == n)
    return;
  ChebSpec::N = n;
  ChebSpec::L = n + 1;
  cout << "computing chebyshev matrices, N = " << ChebSpec::N << endl;
  // initialize x
  ChebSpec::x.clear();
  ChebSpec::x.resize(ChebSpec::L);
  for(int i = 0; i < ChebSpec::L; i++)
  {
    ChebSpec::x[i] = -cos(PI * i / ChebSpec::N);
  }
  // Tm, the matrix of the Chebyshev polynomial values in the grid. Checked.
  ChebSpec::Tm.clear();
  ChebSpec::Tm.resize(ChebSpec::L);
  for(int i = 0; i < ChebSpec::L; i++)
  {
    ChebSpec::Tm[i].resize(ChebSpec::L);
    for(int j = 0; j < ChebSpec::L; j++)
    {
      if(i == 0)      // first
      {
        ChebSpec::Tm[i][j] = 1;
      }
      else if(i == 1) // second
      {
        ChebSpec::Tm[i][j] = ChebSpec::x[j];
      }
      else
      {
        ChebSpec::Tm[i][j] = 2 * ChebSpec::x[j] * ChebSpec::Tm[i - 1][j] - ChebSpec::Tm[i - 2][j];
      }
    }
  }

  // TmIntInt, the matrix of the integrated from -1 Chebyshev polynomial values in the grid. Checked.
  ChebSpec::TmInt.clear();
  ChebSpec::TmInt.resize(ChebSpec::L);
  for(int i = 0; i < ChebSpec::L; i++)
  {
    ChebSpec::TmInt[i].resize(ChebSpec::L);
    for(int j = 0; j < ChebSpec::L; j++)
    {
      if(i == 0)      // first
      {
        ChebSpec::TmInt[i][j] = ChebSpec::x[j] + 1;
      }
      else if(i == 1) // second
      {
        ChebSpec::TmInt[i][j] =  0.5 * (ChebSpec::x[j] * ChebSpec::x[j] - 1);
      }
      else if(i < ChebSpec::L - 1)
      {
        ChebSpec::TmInt[i][j] = ChebSpec::Tm[i + 1][j] / (2. * (i + 1)) - ChebSpec::Tm[i - 1][j] / (2. * (i - 1)) + pow(-1, i + 1.) / (pow(i, 2.) - 1);
      }
      else   // case i = L - 1, right extreme
      {
        ChebSpec::TmInt[i][j] = (2 * ChebSpec::x[j] * ChebSpec::Tm[i][j] - ChebSpec::Tm[i - 1][j]) / (2.0 * (i + 1)) - ChebSpec::Tm[i - 1][j] / (2.0 * (i - 1)) + pow(-1, i + 1) / (pow(i, 2) - 1);
      }
    }
  }

  // D matrix: the integral from -1 to x. Checked.
  ChebSpec::D.clear();
  ChebSpec::D.resize(ChebSpec::L);
  for(int i = 0; i < ChebSpec::L; i++)
  {
    ChebSpec::D[i].resize(ChebSpec::L);
    for(int j = 0; j < ChebSpec::L; j++)
    {
      double s = 0;
      for(int k = 0; k < ChebSpec::L; k++)
      {
        double f = 1;
        if(k == 0 || k == ChebSpec::L - 1) // prime '' sum
        {
          f = 0.5;
        }
        s += f * ChebSpec::Tm[k][j] * ChebSpec::TmInt[k][i];
      }

      ChebSpec::D[i][j] = (2. / ChebSpec::N) * s;
      // prime '' sum
      if(j == 0 || j == ChebSpec::L - 1)
        ChebSpec::D[i][j] = 0.5 * ChebSpec::D[i][j];
    }
  }
  // finally the D2 = D*D matrix
  // check, the sum of the last row has to be 2 and 0 the sum of the first
  ChebSpec::D2.clear();
  ChebSpec::D2.resize(ChebSpec::L);
  for(int i = 0; i < ChebSpec::L; i++)
  {
    ChebSpec::D2[i].resize(ChebSpec::L);
    for(int j = 0; j < ChebSpec::L; j++)
    {
      double s = 0;
      for(int k = 0; k < ChebSpec::L; k++)
      {
        s += ChebSpec::D[i][k] * ChebSpec::D[k][j];
      }
      ChebSpec::D2[i][j] = s;
    }
  }

  // check, the sum of the last row has to be 2
  double s = 0;
  for(int k = 0; k < ChebSpec::L; k++)
  {
    s += ChebSpec::D2[0][k];
  }
}

;
#endif


/**
 * @example ublas/test/vector_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/ublas/vector_math.h>
#include <usml/ublas/test/vector_test_support.h>

using namespace usml::ublas;
using namespace vector_test;

BOOST_AUTO_TEST_SUITE(vector_test)

/**
 * @ingroup ublas_test
 * @{
 */

/**
 * Run a real valued vector through the standard math routines.  Tests include:
 *
 *  - addition of vector to itself
 *  - add scalar to vector
 *  - subtract vector from scalar
 *  - divide scalar by vector
 *  - limiting functions: min, max, floor, and ceil
 *  - conversion routines for degrees/radians and latitude/colatitude
 *  - algebraic functions: abs(), abs2(), arg(), sqrt()
 *  - trig functions: cos(), cosh(), sin(), sinh(), tan(), tanh()
 *  - exponential functions: exp(), log(), log10()
 *
 * Warning: Some compilers, at low optimization levels, fail to properly
 * chain multiple ublas operations into a single evaluation.
 * When this happens, the result is equal to the first operation.
 * These tests have been designed to catch this flaw.
 */
BOOST_AUTO_TEST_CASE( real_vector_test ) {

    vector<double> rvect(7);
    for ( size_t n=0; n < rvect.size(); ++n ) {
        rvect(n) = (double) (n+1);
    }

    cout << "=== vector_test: real_vector_test ===" << endl;
    cout << "a:         " << rvect << endl;
    cout << "a+a:       " << rvect + rvect << endl;
    USML_VECTOR_REAL_TESTER( rvect+rvect, rvect, add_helper<double> ) ;
    cout << "a*2.1:     " << rvect*2.1 << endl;
    USML_VECTOR_REAL_TESTER( rvect*2.1, rvect, scalar2_multiply_helper<double> ) ;
    cout << "a+2.1:     " << rvect+2.1 << endl;
    USML_VECTOR_REAL_TESTER( rvect+2.1, rvect, scalar2_add_helper<double> ) ;
    cout << "2.1-a:     " << 2.1-rvect << endl;
    USML_VECTOR_REAL_TESTER( 2.1-rvect, rvect, scalar1_minus_helper<double> ) ;
    cout << "2.1/a:     " << 2.1/rvect << endl;
    USML_VECTOR_REAL_TESTER( 2.1/rvect, rvect, scalar1_divide_helper<double> ) ;
    cout << "a*a:       " << rvect * rvect << endl;
    USML_VECTOR_REAL_TESTER( rvect*rvect, rvect, multiply_helper<double> ) ;
    cout << "a/a:       " << rvect / rvect << endl;
    USML_VECTOR_REAL_TESTER( rvect/rvect, rvect, divide_helper<double> ) ;
    cout << "max(a,3.0):   " << max(rvect,3.0) << endl;
    USML_VECTOR_REAL_TESTER( max(rvect,3.0), rvect, max_helper<double> ) ;
    cout << "min(a,3.0):   " << min(rvect,3.0) << endl;
    USML_VECTOR_REAL_TESTER( min(rvect,3.0), rvect, min_helper<double> ) ;
    cout << "floor(a+2.1): " << floor( rvect+2.1 ) << endl;
    USML_VECTOR_REAL_TESTER( floor( rvect+2.1 ), rvect, floor_helper<double> ) ;
    cout << "ceil(a+2.1):  " << ceil( rvect+2.1 ) << endl;
    USML_VECTOR_REAL_TESTER( ceil( rvect+2.1 ), rvect, ceil_helper<double> ) ;
    cout << "to_degrees(to_radians(a+2.1)):     " << to_degrees(to_radians(rvect+2.1)) << endl;
    USML_VECTOR_REAL_TESTER( to_degrees(to_radians(rvect+2.1)), rvect, scalar2_add_helper<double> ) ;
    cout << "to_latitude(to_colatitude(a+2.1)): " << to_latitude(to_colatitude(rvect+2.1)) << endl;
    USML_VECTOR_REAL_TESTER( to_latitude(to_colatitude(rvect+2.1)), rvect, scalar2_add_helper<double> ) ;

    cout << "abs(a):    " << abs(rvect) << endl;
    USML_VECTOR_REAL_TESTER( abs(rvect), rvect, std::abs<double> ) ;
    cout << "abs2(a):   " << abs2(rvect) << endl;
    USML_VECTOR_REAL_TESTER( abs2(rvect), rvect, abs2_helper<double> ) ;
    cout << "arg(a):    " << arg(rvect) << endl;
    USML_VECTOR_REAL_TESTER( arg(rvect), rvect, zero_helper<double> ) ;
    cout << "sqrt(a):   " << sqrt(rvect) << endl;
    USML_VECTOR_REAL_TESTER( sqrt(rvect), rvect, std::sqrt<double> ) ;

    cout << "cos(a):    " << cos(rvect) << endl;
    USML_VECTOR_REAL_TESTER( cos(rvect), rvect, std::cos<double> ) ;
    cout << "cosh(a):   " << cosh(rvect) << endl;
    USML_VECTOR_REAL_TESTER( cosh(rvect), rvect, std::cosh<double> ) ;
    cout << "sin(a):    " << sin(rvect) << endl;
    USML_VECTOR_REAL_TESTER( sin(rvect), rvect, std::sin<double> ) ;
    cout << "sinh(a):   " << sinh(rvect) << endl;
    USML_VECTOR_REAL_TESTER( sinh(rvect), rvect, std::sinh<double> ) ;
    cout << "tan(a):    " << tan(rvect) << endl;
    USML_VECTOR_REAL_TESTER( tan(rvect), rvect, std::tan<double> ) ;
    cout << "tanh(a):   " << tanh(rvect) << endl;
    USML_VECTOR_REAL_TESTER( tanh(rvect), rvect, std::tanh<double> ) ;

    cout << "exp(a):    " << exp(rvect) << endl;
    USML_VECTOR_REAL_TESTER( exp(rvect), rvect, std::exp<double> ) ;
    cout << "log(a):    " << log(rvect) << endl;
    USML_VECTOR_REAL_TESTER( log(rvect), rvect, std::log<double> ) ;
    cout << "log10(a):  " << log10(rvect) << endl;
    USML_VECTOR_REAL_TESTER( log10(rvect), rvect, std::log10<double> ) ;
}

/**
 * Run a complex valued vector through the standard math routines.  Tests include:
 *
 *  - addition of vector to itself
 *  - add scalar to vector
 *  - subtract vector from scalar
 *  - divide scalar by vector
 *  - min, max, floor, and ceil clipping using a scalar
 *  - conversion routines for degrees/radians and latitude/colatitude
 *  - algebraic functions: abs(), abs2(), arg(), sqrt()
 *  - trig functions: cos(), cosh(), sin(), sinh(), tan(), tanh()
 *  - exponential functions: exp(), log(), log10()
 *
 * Warning: Some compilers, at low optimization levels, fail to properly
 * chain multiple ublas operations into a single evaluation.
 * When this happens, the result is equal to the first operation.
 * These tests have been designed to catch this flaw.
 */
BOOST_AUTO_TEST_CASE( complex_vector_test ) {

    vector< std::complex<double> > cvect(7);
    for ( size_t n=0; n < cvect.size(); ++n ) {
        cvect(n) = std::complex<double>(n+1,1.0);
    }

    cout << "=== vector_test: complex_vector_test ===" << endl;
    cout << "a:         " << cvect << endl;
    cout << "a+a:       " << cvect + cvect << endl;
    USML_VECTOR_COMPLEX_TESTER( cvect+cvect, cvect, add_helper<complex<double> > ) ;
    cout << "a+2.1:     " << cvect+2.1 << endl;
    USML_VECTOR_COMPLEX_TESTER( cvect+2.1, cvect, scalar2_add_helper<complex<double> > ) ;
    cout << "2.1-a:     " << 2.1-cvect << endl;
    USML_VECTOR_COMPLEX_TESTER( 2.1-cvect, cvect, scalar1_minus_helper<complex<double> > ) ;

    cout << "abs(a):    " << abs(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( abs(cvect), cvect, abs_helper<complex<double> > ) ;
    cout << "abs2(a):   " << abs2(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( abs2(cvect), cvect, abs2_helper<complex<double> > ) ;
    cout << "arg(a):    " << arg(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( arg(cvect), cvect, arg_helper<double> ) ;
    cout << "sqrt(a):   " << sqrt(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( sqrt(cvect), cvect, std::sqrt<double> ) ;

    cout << "cos(a):    " << cos(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( cos(cvect), cvect, std::cos<double> ) ;
    cout << "cosh(a):   " << cosh(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( cosh(cvect), cvect, std::cosh<double> ) ;
    cout << "sin(a):    " << sin(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( sin(cvect), cvect, std::sin<double> ) ;
    cout << "sinh(a):   " << sinh(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( sinh(cvect), cvect, std::sinh<double> ) ;
    cout << "tan(a):    " << tan(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( tan(cvect), cvect, std::tan<double> ) ;
    cout << "tanh(a):   " << tanh(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( tanh(cvect), cvect, std::tanh<double> ) ;

    cout << "exp(a):    " << exp(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( exp(cvect), cvect, std::exp<double> ) ;
    cout << "log(a):    " << log(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( log(cvect), cvect, std::log<double> ) ;
    cout << "log10(a):  " << log10(cvect) << endl;
    USML_VECTOR_COMPLEX_TESTER( log10(cvect), cvect, std::log10<double> ) ;
}

/**
 * Test all of the real and complex combinations of the pow() function.
 */
BOOST_AUTO_TEST_CASE( pow_vector_test ) {

    vector<double> rvect(3);
    vector< std::complex<double> > cvect(3);
    std::complex<double> cmplx(2.5,3.5);

    for ( size_t n=0; n < cvect.size(); ++n ) {
        rvect(n) = (double) (n+1);
        cvect(n) = std::complex<double>(n+1,1.0);
    }

    cout << "=== vector_test: pow_vector_test ===" << endl;
    cout << "N:      " << rvect << endl;
    cout << "M:      " << cvect << endl;
    cout << "C:      " << cmplx << endl;

    cout << "N^3:    " << pow(rvect,3) << endl;
    USML_VECTOR_POW_SCALAR2_TESTER( pow(rvect,3), rvect, 3 ) ;
    cout << "N^2.5:  " << pow(rvect,2.5) << endl;
    USML_VECTOR_POW_SCALAR2_TESTER( pow(rvect,2.5), rvect, 2.5 ) ;
    cout << "2^N:    " << pow( 2.0, rvect ) << endl;
    USML_VECTOR_POW_SCALAR1_TESTER( pow(2.0,rvect), 2.0, rvect ) ;

//    cout << "M^3:    " << pow(cvect,3.0) << endl;
//    USML_VECTOR_POW_SCALAR2_TESTER( pow(cvect,3), cvect, 3 ) ;
    cout << "M^2.5:  " << pow(cvect,2.5) << endl;
    USML_VECTOR_POW_SCALAR2_TESTER( pow(cvect,2.5), cvect, 2.5 ) ;
    cout << "2^M:    " << pow(2.0,cvect) << endl;
    USML_VECTOR_POW_SCALAR1_TESTER( pow(2.0,cvect), 2.0, cvect ) ;

    cout << "N^N:    " << pow(rvect,rvect) << endl;
    USML_VECTOR_POW_TESTER( pow(rvect,rvect), rvect, rvect ) ;
    cout << "M^M:    " << pow(cvect,cvect) << endl;
    USML_VECTOR_POW_TESTER( pow(cvect,cvect), cvect, cvect ) ;
    cout << "N^M:    " << pow(rvect,cvect) << endl;
    USML_VECTOR_POW_TESTER( pow(rvect,cvect), rvect, cvect ) ;
    cout << "M^N:    " << pow(cvect,rvect) << endl;
    USML_VECTOR_POW_TESTER( pow(cvect,rvect), cvect, rvect ) ;
}

/**
 * Test the generation of real valued and analytic signals from
 * a vector of arguments.
 *
 * Assume that testers will visually inspect the results.
 */
BOOST_AUTO_TEST_CASE( signal_vector_test ) {

    vector<double> rvect(3);
    for ( size_t n=0; n < rvect.size(); ++n ) {
        rvect(n) = (double) (n+1);
    }

    cout << "=== vector_test: signal_vector_test ===" << endl;
    cout << "input:  " << rvect << endl;
    cout << "signal: " << signal(rvect) << endl;
    USML_VECTOR_REAL_TESTER( signal(rvect), rvect, signal_helper<double> ) ;
    cout << "signal: " << asignal(rvect) << endl;
    USML_VECTOR_CR_TESTER( asignal(rvect), rvect, asignal_helper<double> ) ;
}

/**
 * Run trig routines forward and backward to check algorithms.
 */
BOOST_AUTO_TEST_CASE( realInverse_vector_test ) {

    vector<double> rvect(3);
    for ( size_t n=0; n < rvect.size(); ++n ) {
        rvect(n) = 0.1 * (double) (n+1);
    }

    cout << "=== vector_test: realInverse_vector_test ===" << endl;
    cout << "acos(cos(t)):    " << acos(cos(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( acos(cos(rvect)), rvect, identity<double> ) ;
    cout << "acosh(cosh(t)):  " << acosh(cosh(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( acosh(cosh(rvect)), rvect, identity<double> ) ;
    cout << "asin(sin(t)):    " << asin(sin(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( asin(sin(rvect)), rvect, identity<double> ) ;
    cout << "asinh(sinh(t)):  " << asinh(sinh(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( asinh(sinh(rvect)), rvect, identity<double> ) ;
    cout << "atan(tan(t)):    " << atan(tan(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( atan(tan(rvect)), rvect, identity<double> ) ;
    cout << "atanh(tanh(t)):  " << atanh(tanh(rvect)) << endl;
    USML_VECTOR_REAL_TESTER( atanh(tanh(rvect)), rvect, identity<double> ) ;
}

/**
 * Run complex trig routines forward and backward to check algorithms.
 */
BOOST_AUTO_TEST_CASE( complexInverse_vector_test ) {

    vector< std::complex<double> > cvect(3);
    for ( size_t n=0; n < cvect.size(); ++n ) {
        cvect(n) = 0.1 * std::complex<double>(n+1,1.0);
    }

    cout << "=== vector_test: complexInverse_vector_test ===" << endl;
    cout << "acos(cos(t)):    " << acos(cos(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( acos(cos(cvect)), cvect, identity<complex<double> > ) ;
    cout << "acosh(cosh(t)):  " << acosh(cosh(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( acosh(cosh(cvect)), cvect, identity<complex<double> > ) ;
    cout << "asin(sin(t)):    " << asin(sin(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( asin(sin(cvect)), cvect, identity<complex<double> > ) ;
    cout << "asinh(sinh(t)):  " << asinh(sinh(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( asinh(sinh(cvect)), cvect, identity<complex<double> > ) ;
    cout << "atan(tan(t)):    " << atan(tan(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( atan(tan(cvect)), cvect, identity<complex<double> > ) ;
    cout << "atanh(tanh(t)):  " << atanh(tanh(cvect)) << endl;
    USML_VECTOR_COMPLEX_TESTER( atanh(tanh(cvect)), cvect, identity<complex<double> > ) ;
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

// Written by Simon Praetorius

#ifndef ITL_PREONLY_INCLUDE
#define ITL_PREONLY_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>

namespace itl
{

  /// Solver that simply applies a preconditioner to the rhs vector
  template <typename Matrix, typename Vector, typename Preconditioner, typename Iteration>
  int preonly(const Matrix& A, Vector& x, const Vector& b, const Preconditioner& P, Iteration& iter)
  {
    if (size(b) == 0)
      throw mtl::logic_error("empty rhs vector");

    typedef typename mtl::Collection<Vector>::value_type Scalar;

    // simple richardson iteration
    Vector r(b - A*x);
    Scalar res = two_norm(r);
    for (; !iter.finished(res); ++iter)
    {
      x += Vector(solve(P, r));
      r = b - A*x;
      res = two_norm(r);
    }
    return iter;
  }

} // namespace itl

#endif // ITL_PREONLY_INCLUDE


#define BOOST_TEST_MODULE unary
#include <boost/test/included/unit_test.hpp>
#include "exprtest.hpp"

EXPRTEST(unary1, "-(2)",  -2)
EXPRTEST(unary2, "-(-2)",  2)
EXPRTEST(unary3, "+(-2)", -2)
EXPRTEST(unary4, "+(+2)",  2)
EXPRTEST(unary5, "!(1)",   0)
EXPRTEST(unary6, "!(0)",   1)


#pragma once

// Armadillo
#include <armadillo>

// C++ standard library
#include <functional>

namespace mant {
  double brent(
      std::function<double(
          double)> objectiveFunction,
      double lowerBound,
      double upperBound,
      arma::uword maximalNumberOfIterations);
}


#ifndef EXPONENTIAL_HISTOGRAM_HPP
#define EXPONENTIAL_HISTOGRAM_HPP

#include <stdexcept>
#include <string>
#include <boost/lexical_cast.hpp>
#include <iostream>

#include <sam/BaseSlidingWindow.hpp>

namespace sam {

template <typename T>
class ExponentialHistogram: public BaseSlidingWindow<T>
{
public:
  static size_t const MAX_SIZE;

private:

  // Determines number of buckets.  If there are k/2 + 2 buckets
  // of the same size (k + 2 buckets if the bucket size equals 1),
  // the oldest two buckets are combined.
  size_t k;

  // The number of levels.  The first level has k+2 slots.
  // All other levels have k/2 + 2 slots.  The ith level (starting at 0)
  // has slots that represent 2^i numbers.
  size_t numLevels;

  // The data structure that holds the data of the sliding window.
  T** data;

  // Points to where data should be added
  size_t* ends;

  // An array of booleans that keeps track of which levels need to be merged
  bool* needToMerge;

  // If all the storage in one level has been used, this is set to true.
  // There is different processing depending on if we have seen the entire level
  // or not.
  bool* onePass;

  T total = 0;

  int numItems = 0;

public:
  ExponentialHistogram(size_t N, size_t k) : BaseSlidingWindow<T>(N)
  {
    if (N == 0) {
      throw std::out_of_range("Cannot specify 0 as the size of window");
    }

    if (N >= MAX_SIZE) {
      throw std::out_of_range("Specified N > MAX_SIZE which is " +
                              boost::lexical_cast<std::string>(MAX_SIZE));
    }

    this->k = k;

    // Sets numLevels
    determineNumLevels();

    data = new T*[numLevels];

    // The first level has k + 2 elements
    data[0] = new T[k + 2];
    for (int i = 1; i < numLevels; i++) {
      data[i] = new T[k/2 + 2];
    }

    onePass = new bool[numLevels];
    for (int i = 0; i < numLevels; i++) onePass[i] = false;

    needToMerge = new bool[numLevels];
    for (int i = 0; i < numLevels; i++) needToMerge[i] = false;

    ends = new size_t[numLevels];
    for (int i = 0; i < numLevels; i++) ends[i] = 0;

  }

  virtual ~ExponentialHistogram() {
    for(int i = 0; i < numLevels; i++) {
      delete[] data[i];
    }
    delete[] data;
    delete[] onePass;
    delete[] ends;
  }

  /**
   * Add the specified item to the window.  If the window is full,
   * the item at the end is dropped.
   */
  void add(T item) {
    //update the global total
    total = total + item;

    //update the number of items represented
    numItems++;

    // Add the item to the data structure.
    add(item, 0);

    /*for (int i = 0; i < k + 2; i++) {
      std::cout << data[0][i] << " ";
    }
    std::cout << std::endl;
    for (int i = 1; i < numLevels; i++) {
      for (int j = 0; j < k/2 + 2; j++) {
        std::cout << data[i][j] << " ";
      }
      std::cout << std::endl;
    }*/

  }

  /**
   * Returns the number of levels.  The ith level represents
   * 2^i items aggregated together.  There are k+2 values in the
   * 0th level, and k/2 + 1 values for levels > 0.
   */
  size_t getNumLevels() {
    return numLevels;
  }

  T getTotal() {
    return total;
  }

  /**
   * Returns the total number of numbers that can be represented by
   * the histogram.
   */
  size_t getNumSlots() {
    return getNumSlots(numLevels, k);
  }

  /**
   * Returns the number of items currently being represented by the
   * exponential histogram.
   */
  size_t getNumItems() {
    return numItems;
  }

  static size_t getNumSlots(long N, int k)
  {
    int size = 1;
    int total = 0;
    total = size * (k + 2);
    for (int i = 1; i < N; i++) {
      size = size * 2;
      total = total + size * (k/2 + 2);
    }
    return total;

  }

private:

  void add(T item, size_t level) {
    //std::cout << "Adding item " << item << " to level " << level << std::endl;
    if (level < numLevels) {
      // Going through the level for the first time.
      // We can just add items without worrying about overwriting values
      // or the need to merge.
      if (!onePass[level])
      {
        data[level][ends[level]] = item;
        incrementEnd(level);
        // we passed through the level once
        if (ends[level] == 0) {
          onePass[level] = true;
          needToMerge[level] = true;
        }
      }
      // We have gone through the level at least once.  We have to worry about
      // writing over values and the need to merge values to send to the level
      // above.
      else {

        // Adding an item will force a merger
        if (needToMerge[level]) {
          // index of the first item to merge.
          //std::cout << ends[level] << " " << endPlusOne(level) << std::endl;
          size_t first = data[level][ends[level]];

          // index of the second item to merge.
          size_t second = data[level][endPlusOne(level)];

          // Adding merged item to the next level
          add(first + second, level + 1);

          // Adding the new item to the now open space
          data[level][ends[level]] = item;
          data[level][endPlusOne(level)] = -1;

          // The next addition won't require a merger since we cleared out
          // two spaces.
          needToMerge[level] = false;

          incrementEnd(level);
        }
        // Still have space; no merger needed.
        else {
          data[level][ends[level]] = item;
          incrementEnd(level);
          needToMerge[level] = true;
        }
      }
    }
    // If there isn't another level, we update the total and drop the item.
    else
    {
      numItems -= pow(2, level);
      total = total - item;
    }
  }

  /**
   * Returns the index of the end incremented by 1 for the
   * specified level.
   */
  size_t endPlusOne(size_t level) {
    size_t tempEnd = ends[level] + 1;
    if (((level == 0) && (tempEnd >= (k + 2))) ||
        ((level > 0) && (tempEnd >= (k/2 + 2))))
    {
      return 0;
    }
    return tempEnd;
  }

  /**
   * Increments the end index for the specified level.
   */
  void incrementEnd(size_t level) {
    ends[level]++;
    if (((level == 0) && (ends[level] >= (k + 2))) ||
        ((level > 0) && (ends[level] >= (k/2 + 2))))
    {
      ends[level] = 0;
    }
  }

  // Determines the number of bins necessary for the sliding window
  // of size N.
  void determineNumLevels() {
    size_t total = 0;
    numLevels = 1;

    // first level has k + 2 slots, each representing one number
    total = k + 2;

    while (total <= this->N) {
      total = total + (k/2 + 2) * pow(2, numLevels);
      numLevels++;
    }
  }

};

template <typename T>
size_t const ExponentialHistogram<T>::MAX_SIZE = 10000000;

}
#endif


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing modf capabilities

    Computes the integer and  fractional parts of the input

    @par Semantic:

    @code
    std::pair<T,T> p = modf(x);
    @endcode

     is similar to:

    @code
    T t = trunc(x);
    T f = frac(x);
    @endcode

    @see frac,  trunc

  **/
  Value modf(Value const & x, Value & y);

  //@overload
  std::pair<Value, Value> modf(Value const & x);
} }
#endif

#include <boost/simd/function/scalar/modf.hpp>
#include <boost/simd/function/simd/modf.hpp>

#endif


/* Copyright (c) 2016 - 2019, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef MATERIAL_PROPERTY_HH
#define MATERIAL_PROPERTY_HH

#include <types.hh>
#include <utils.hh>

#include <deal.II/base/function_parser.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/la_vector.h>

#include <boost/property_tree/ptree.hpp>

#include <array>
#include <limits>
#include <unordered_map>

namespace adamantine
{
/**
 * This class stores the material properties for all the materials
 */
template <int dim>
class MaterialProperty
{
public:
  /**
   * Constructor.
   * \param[in] database requires the following entries:
   *   - n_materials: unsigned int in \f$(0,\infty)\f$
   *   - material_X: property tree associated with material_X
   *   where X is a number
   *   - material_X.Y: property_tree where Y is either liquid, powder, or
   *   solid [optional]
   *   - material_X.Y.Z: string where Z is either density, specific_heat,
   *   or thermal_conductivity, describe the behavior of the property as a
   *   function of the temperatur (e.g. "2.*T") [optional]
   *   - material.X.A: A is either solidus, liquidus, or latent_heat
   *   [optional]
   */
  MaterialProperty(
      MPI_Comm const &communicator,
      dealii::parallel::distributed::Triangulation<dim> const &tria,
      boost::property_tree::ptree const &database);

  /**
   * Return the value of the given property, for a given cell and a given field
   * state.
   */
  template <typename NumberType>
  double
  get(typename dealii::Triangulation<dim>::active_cell_iterator const &cell,
      Property prop,
      dealii::LA::distributed::Vector<NumberType> const &field_state) const;

  /**
   * Return the average temperature on every cell given the enthalpy.
   */
  template <typename NumberType>
  dealii::LA::distributed::Vector<NumberType> enthalpy_to_temperature(
      dealii::DoFHandler<dim> const &enthalpy_dof_handler,
      dealii::LA::distributed::Vector<NumberType> const &enthalpy);

  /**
   * Reinitialize the DoFHandler associated with MaterialProperty and resize the
   * state vectors.
   */
  void reinit_dofs();

  /**
   * Update the material state, i.e, the ratio of liquid, powder, and solid.
   */
  template <typename NumberType>
  void
  update_state(dealii::DoFHandler<dim> const &enthalpy_dof_handler,
               dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &enthalpy);

  /**
   * Get the array of material state vectors. The order of the different state
   * vectos is given by the MaterialState enum. Each entry in the vector
   * correspond to a cell in the mesh and has a value between 0 and 1. The sum
   * of the states for a given cell is equal to 1.
   */
  std::array<dealii::LA::distributed::Vector<double>,
             static_cast<unsigned int>(MaterialState::SIZE)> &
  get_state();

  double get_state_ratio(
      typename dealii::Triangulation<dim>::active_cell_iterator const &cell,
      MaterialState material_state) const;

  /**
   * Return \f$ -\frac{H_{liquidus}}{\rho C_P} + T_{liquidus} \f$
   */
  double get_liquid_beta(
      typename dealii::Triangulation<dim>::active_cell_iterator const &cell)
      const;

  /**
   * Return \f$ \frac{T_{liquidus}-T_{solidus}}{\mathcal{L}} \f$
   */
  double get_mushy_alpha(
      typename dealii::Triangulation<dim>::active_cell_iterator const &cell)
      const;

  /**
   * Return \f$ -H_{solidus} \frac{T_{liquidus}-T_{solidus}}{\mathcal{L}} +
   * T_{solidus} \f$
   */
  double get_mushy_beta(
      typename dealii::Triangulation<dim>::active_cell_iterator const &cell)
      const;

  /**
   * Return the underlying the DoFHandler.
   */
  dealii::DoFHandler<dim> const &get_dof_handler() const;

private:
  /**
   * Maximum different number of states a given material can be.
   */
  static unsigned int constexpr _n_material_states =
      static_cast<unsigned int>(MaterialState::SIZE);

  /**
   * Number of properties defined.
   */
  static unsigned int constexpr _n_properties =
      static_cast<unsigned int>(Property::SIZE);

  /**
   * Set the values in _state from the values of the user index of the
   * Triangulation.
   */
  void set_state();

  /**
   * Fill the _properties map.
   */
  void fill_properties(boost::property_tree::ptree const &database);

  /**
   * If the density and the specific heat do not depend on the temperature, the
   * relationship between the temperature and the enthalpy can be written by
   * piecewise function \f$ T = \alpha H + \beta \f$. This function computes the
   * constants \f$ \alpha \f$ and \f$ \beta \f$.
   */
  void compute_constants();

  /**
   * Return the index of the dof associated to the cell.
   */
  double get_dof_index(
      typename dealii::Triangulation<dim>::active_cell_iterator const &cell)
      const;

  /**
   * Compute the average of the enthalpy on every cell.
   */
  template <typename NumberType>
  dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> compute_average_enthalpy(
      dealii::DoFHandler<dim> const &enthalpy_dof_handler,
      dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &enthalpy) const;

  /**
   * MPI communicator.
   */
  MPI_Comm _communicator;
  /**
   * Map of \f$ -\frac{H_{liquidus}}{\rho C_P} + T_{liquidus} \f$ for each
   * material.
   */
  std::unordered_map<dealii::types::material_id, double> _liquid_beta;
  /**
   * Map of \f$ \frac{T_{liquidus}-T_{solidus}}{\mathcal{L}} \f$ for each
   * material.
   */
  std::unordered_map<dealii::types::material_id, double> _mushy_alpha;
  /**
   * Map of \f$ -H_{solidus} \frac{T_{liquidus}-T_{solidus}}{\mathcal{L}} +
   * T_{solidus} \f$ for each material.
   */
  std::unordered_map<dealii::types::material_id, double> _mushy_beta;
  /**
   * Map that stores functions describing the properties of the material.
   */
  std::unordered_map<
      dealii::types::material_id,
      std::array<
          std::array<std::unique_ptr<dealii::FunctionParser<1>>, _n_properties>,
          _n_material_states>>
      _properties;
  /**
   * Array of vector describing the ratio of each state in each cell. Each
   * vector corresponds to a state defined in the MaterialState enum.
   */
  std::array<dealii::LA::distributed::Vector<double>, _n_material_states>
      _state;
  /**
   * Discontinuous piecewise constant finite element.
   */
  dealii::FE_DGQ<dim> _fe;
  /**
   * DoFHandler associated to the _state array.
   */
  dealii::DoFHandler<dim> _mp_dof_handler;
};

template <int dim>
inline std::array<dealii::LA::distributed::Vector<double>,
                  static_cast<unsigned int>(MaterialState::SIZE)> &
MaterialProperty<dim>::get_state()
{
  return _state;
}

template <int dim>
inline double MaterialProperty<dim>::get_state_ratio(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell,
    MaterialState material_state) const
{
  double const mp_dof_index = get_dof_index(cell);
  unsigned int const mat_state = static_cast<unsigned int>(material_state);

  return _state[mat_state][mp_dof_index];
}

template <int dim>
inline double MaterialProperty<dim>::get_liquid_beta(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell) const
{
  dealii::types::material_id material_id = cell->material_id();
  auto const liquid_beta = _liquid_beta.find(material_id);
  ASSERT(liquid_beta != _liquid_beta.end(), "Material not found.");

  return liquid_beta->second;
}

template <int dim>
inline double MaterialProperty<dim>::get_mushy_alpha(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell) const
{
  dealii::types::material_id material_id = cell->material_id();
  auto const mushy_alpha = _mushy_alpha.find(material_id);
  ASSERT(mushy_alpha != _mushy_alpha.end(), "Material not found.");

  return mushy_alpha->second;
}

template <int dim>
inline double MaterialProperty<dim>::get_mushy_beta(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell) const
{
  dealii::types::material_id material_id = cell->material_id();
  auto const mushy_beta = _mushy_beta.find(material_id);
  ASSERT(mushy_beta != _mushy_beta.end(), "Material not found.");

  return mushy_beta->second;
}

template <int dim>
inline double MaterialProperty<dim>::get_dof_index(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell) const
{
  // Get a DoFCellAccessor from a Triangulation::active_cell_iterator.
  dealii::DoFAccessor<dim, dealii::DoFHandler<dim>, false> dof_accessor(
      &_mp_dof_handler.get_triangulation(), cell->level(), cell->index(),
      &_mp_dof_handler);
  std::vector<dealii::types::global_dof_index> mp_dof(1.);
  dof_accessor.get_dof_indices(mp_dof);

  return mp_dof[0];
}

template <int dim>
inline dealii::DoFHandler<dim> const &
MaterialProperty<dim>::get_dof_handler() const
{
  return _mp_dof_handler;
}
} // namespace adamantine

#endif


#include "ViewerInh.h"
#include <igl/collapse_edge.h>
#include <igl/edge_flaps.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/adjacency_list.h>
#include <Eigen/Core>
#include <set>
#include <math.h>
#include "igl/circulation.h"

void ViewerInh::init_ds_for_data() {
    int num_of_meshes = static_cast<int>(data_list.size());
    data_edges.resize(static_cast<unsigned long>(num_of_meshes));
    for (int i = 0; i < num_of_meshes; i++) {
        auto ds = new edges;                                            // Create new data structure
        data_edges[i] = reset(data_list[i], ds);
    }
}

edges *ViewerInh::reset(igl::opengl::ViewerData obj, edges* ds){
    Eigen::MatrixXd V = obj.V;                                                         // Get mesh's vertexes
    Eigen::MatrixXi F = obj.F;                                                         // Get mesh's faces
    igl::edge_flaps(F,ds->E,ds->EMAP,ds->EF,ds->EI);                                   // Init the edge related field in the data structure
    ds->Qit.resize(static_cast<unsigned long>(ds->E.rows()));
    ds->C.resize(ds->E.rows(),V.cols());
    ds->Q.clear();
    ds->EQ = get_Q(V,F,obj.F_normals);

    for(int e = 0; e < ds->E.rows(); e++){
        double cost;
        Eigen::RowVectorXd p(1,3);
        calc_edges_cost(e,V,F,ds->E,ds->EMAP,ds->EF, ds->EI, cost, p, ds->EQ);         // Calculate cost for each edge
        ds->Qit[e] = ds->Q.insert(std::pair<double,int>(cost,e)).first;                // Save the edge cost in Q and in Q's iterator- Qit
        ds->C.row(e)   = p;                                                            // Save the optimal vertex p in C at index e
    }
    ds->num_collapsed = 0;
    return ds;
}

 std::vector<Eigen::Matrix4d> ViewerInh::get_Q(Eigen::MatrixXd &V, Eigen::MatrixXi &F, Eigen::MatrixXd &F_normals){
    std::vector<std::vector<int> > vertex_to_vertices;
    std::vector<std::vector<int> > vertex_to_faces_index;
    igl::adjacency_list(F, vertex_to_vertices);
    igl::vertex_triangle_adjacency(V,F,vertex_to_vertices, vertex_to_faces_index);     // Get the faces connected to each vertex

    std::vector<Eigen::Matrix4d> Q;                                                    // Create vector Q of error quadrics
    Q.resize(V.rows());                                                                // Set the size of Q to be as the numbers of vertecies

    for(int v = 0; v < V.rows(); v++) {                                                // For each vertex v in V
        std::vector<int> faces_index_of_vector = vertex_to_vertices[v];                // Get the faces connected to v
        Eigen::Vector4d vertex(V.row(v)(0), V.row(v)(1), V.row(v)(2), 1);              // Get v in vector of size 4
        Eigen::Matrix4d sum_of_Kp;
        sum_of_Kp << 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;                                  // Init 4X4 matrix to insert to Q

        for(int f: faces_index_of_vector) {                                            // For each face connected to v
            Eigen::RowVectorXd p = F_normals.row(f).normalized();                      // p = (a, b, c) -> a^2 + b^2 + c^2 = 1
            double d = (-vertex(0) * p(0))+ (-vertex(1) * p(1)) + (-vertex(2) * p(2)); // d = -ax - by - cz
            Eigen::Vector4d p_with_d(p(0), p(1), p(2), d);                             // p_with_d = (a, b, c, d)
            Eigen::Matrix4d Kp = p_with_d * p_with_d.transpose();                      // Kp
            sum_of_Kp += Kp;                                                           // Add Kp to sum_of_KP
        }
        Q[v] = sum_of_Kp;                                                              // Push sum_of_KP to Q in index v
    }
     return Q;
}

 void ViewerInh::calc_edges_cost(const int e,
                                 const Eigen::MatrixXd & V,
                                 const Eigen::MatrixXi & F,
                                 const Eigen::MatrixXi & E,
                                 const Eigen::VectorXi & EMAP,
                                 const Eigen::MatrixXi & EF,
                                 const Eigen::MatrixXi & EI,
                                 double & cost,
                                 Eigen::RowVectorXd & p,
                                 std::vector<Eigen::Matrix4d> & Q){
        Eigen::RowVectorXd p_(1,3);
        int v1 = E(e,0);                    // Index (in V) of the first vertex in edge e
        int v2 = E(e,1);                    // Index (in V) of the second vertex in edge e
        Eigen::Matrix4d Q1 = Q[v1];         // Error-quadric of v1
        Eigen::Matrix4d Q2 = Q[v2];         // Error-quadric of v2
        Eigen::Matrix4d Q_tag = (Q1 + Q2);  // Error-quadric Q

        Eigen::Vector4d vertex1(V.row(v1)(0), V.row(v1)(1), V.row(v1)(2), 1); // V.row(v1) as vector of size 4
        Eigen::Vector4d vertex2(V.row(v2)(0), V.row(v2)(1), V.row(v2)(2), 1); // V.row(v2) as vector of size 4

        double lowest_cost;          // Init the lowest cost of the edge

        // Create matrix of Q(1,1)-Q(3,4) and (0,0,0,1) at the bottom
        Eigen::Matrix4d v_tag_1;
        v_tag_1 <<
                Q_tag(0,0),Q_tag(0,1),Q_tag(0,2),Q_tag(0,3),
                Q_tag(1,0),Q_tag(1,1),Q_tag(1,2),Q_tag(1,3),
                Q_tag(2,0),Q_tag(2,1),Q_tag(2,2),Q_tag(2,3),
                0,           0,           0,        1;

        if(v_tag_1.determinant() != 0) {                            // Check if v_tag_1 is inversable
            Eigen::Vector4d v_tag_2(0, 0, 0, 1);
            Eigen::Vector4d v_tag = v_tag_1.inverse() * v_tag_2;    // Get new vertex v_tag to collapse e into
            lowest_cost = v_tag.transpose() * Q_tag * v_tag;        // Get the cost of e
            p_ << v_tag(0), v_tag(1), v_tag(2);                      // Set the optimal vertex p to be v_tag
        }

        else{                                                       // If v_tag_1 is not inverse
            Eigen::Vector4d midpoint = (vertex1 + vertex2) * 0.5;   // Calculate mitdpoint of e
            double cost1 = vertex1.transpose()  * Q_tag * vertex1;  // Calculate cost of v1
            double cost2 = vertex2.transpose()  * Q_tag * vertex2;  // Calculate cost of v2
            double cost3 = midpoint.transpose() * Q_tag * midpoint; // Calculate cost of mitdpoint

            lowest_cost = std::min(cost1, cost2);                   // Set cost of e as min(cost1,cost2,cost3)
            lowest_cost = std::min(lowest_cost, cost3);             //

            if (lowest_cost == cost1)                               //
                p_ << vertex1(0), vertex1(1), vertex1(2);           //
            else if (lowest_cost == cost2)                          // Set the optimal vertex p to the vertex with lowest cost
                p_ << vertex2(0), vertex2(1), vertex2(2);           //
            else                                                    //
                p_ << midpoint(0), midpoint(1), midpoint(2);        //
        }
     p = p_;
     cost = lowest_cost;
//     std::cout << e << ") cost " << cost << " at point " << p(0) << ", "<< p(1) << ", "<< p(2) << "\n"<< std::endl;
}

bool ViewerInh::my_collapse(Eigen::MatrixXd & V,
                             Eigen::MatrixXi & F,
                             Eigen::MatrixXd &F_normals,
                             edges *ds){
    //a. Takes out the lowest cost edge from queue.
    if(ds->Q.empty())
    {
        return false;
    }
    std::pair<double,int> q = *(ds->Q.begin());
    ds->Q.erase(ds->Q.begin());
    int e = q.second;
    double cost = q.first;
    ds->Qit[e] = ds->Q.end();
    std::vector<int> N  = igl::circulation(e, true,ds->EMAP,ds->EF,ds->EI);
    std::vector<int> Nd = igl::circulation(e,false,ds->EMAP,ds->EF,ds->EI);
    N.insert(N.begin(),Nd.begin(),Nd.end());

    //b. Deletes edge
    const auto & kill_edge = [&ds](const int e)
    {
        ds->E(e,0) = 0;
        ds->E(e,1) = 0;
        ds->EF(e,0) = 0;
        ds->EF(e,1) = 0;
        ds->EI(e,0) = 0;
        ds->EI(e,1) = 0;
    };

    const int eflip = ds->E(e,0)>ds->E(e,1);
    // source and destination
    const int s = eflip?ds->E(e,1):ds->E(e,0);
    const int d = eflip?ds->E(e,0):ds->E(e,1);
    const std::vector<int> nV2Fd = igl::circulation(e,!eflip,ds->EMAP,ds->EF,ds->EI);
    int e1,f1,e2,f2;

    const int m = F.rows();
    for(int side = 0;side<2;side++)
    {
        const int f = ds->EF(e,side);
        const int v = ds->EI(e,side);
        const int sign = (eflip==0?1:-1)*(1-2*side);
        // next edge emanating from d
        const int e1_ = ds->EMAP(f+m*((v+sign*1+3)%3));
        // prev edge pointing to s
        const int e2_ = ds->EMAP(f+m*((v+sign*2+3)%3));
        if(! (ds->E(e1_,0) == d || ds->E(e1_,1) == d))return false;
        if(! (ds->E(e2_,0) == s || ds->E(e2_,1) == s))return false;
        // face adjacent to f on e1, also incident on d
        const bool flip1 = ds->EF(e1_,1)==f;
        const int f1_ = flip1 ? ds->EF(e1_,0) : ds->EF(e1_,1);
        if(! (f1_!=f))return false;
        if(! (F(f1_,0)==d || F(f1_,1)==d || F(f1_,2) == d))return false;
        // across from which vertex of f1_ does e1 appear?
        const int v1 = flip1 ? ds->EI(e1_,0) : ds->EI(e1_,1);
        // Kill e1_
        kill_edge(e1_);
        // Kill f
        F(f,0) = 0;
        F(f,1) = 0;
        F(f,2) = 0;
        // map f1_'s edge on e1 to e2_
        if(! (ds->EMAP(f1_+m*v1) == e1_))return false;
        ds->EMAP(f1_+m*v1) = e2_;
        // side opposite f2, the face adjacent to f on e2_, also incident on s
        const int opp2 = (ds->EF(e2_,0)==f?0:1);
        if(! (ds->EF(e2_,opp2) == f))return false;
        ds->EF(e2_,opp2) =  f1_;
        ds->EI(e2_,opp2) = v1;
        // remap e2 from d to s
        ds->E(e2_,0) = ds->E(e2_,0)==d ? s : ds->E(e2_,0);
        ds->E(e2_,1) = ds->E(e2_,1)==d ? s : ds->E(e2_,1);
        if(side==0)
        {
            e1 = e1_;
            f1 = f;
        }else
        {
            e2 = e1_;
            f2 = f;
        }
    }

    //c. Deletes faces
    for(auto f : nV2Fd)
    {
        for(int v = 0;v<3;v++)
        {
            if(F(f,v) == d)
            {
                const int flip1 = (ds->EF(ds->EMAP(f+m*((v+1)%3)),0)==f)?1:0;
                const int flip2 = (ds->EF(ds->EMAP(f+m*((v+2)%3)),0)==f)?0:1;
                if(! (ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) == d ||
                                ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) == s)) return false;
                ds->E(ds->EMAP(f+m*((v+1)%3)),flip1) = s;
                if(! (ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) == d ||
                                ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) == s)) return false;
                ds->E(ds->EMAP(f+m*((v+2)%3)),flip2) = s;
                F(f,v) = s;
                break;
            }
        }
    }
    // Finally, "remove" this edge and its information
    kill_edge(e);

    //d. Merges vertices to a new vertex v̅
    Eigen::RowVectorXd v = ds->C.row(e);
    V.row(s) = v;
    V.row(d) = v;
//    ds->EQ = get_Q(V,F,F_normals);

    // Erase the two, other collapsed edges
    ds->Q.erase(ds->Qit[e1]);
    ds->Qit[e1] = ds->Q.end();
    ds->Q.erase(ds->Qit[e2]);
    ds->Qit[e2] = ds->Q.end();
    // update local neighbors
    // loop over original face neighbors
    for(auto n : N)
    {
        if(F(n,0) != 0 ||
           F(n,1) != 0 ||
           F(n,2) != 0)
        {
            for(int v = 0;v<3;v++)
            {
                // get edge id
                const int ei = ds->EMAP(v*F.rows()+n);
                // erase old entry
                ds->Q.erase(ds->Qit[ei]);
                // compute cost and potential placement
                double cost;
                Eigen::RowVectorXd place;
                calc_edges_cost(ei,V,F,ds->E,ds->EMAP,ds->EF,ds->EI,cost,place,ds->EQ);
                // Replace in queue
                ds->Qit[ei] = ds->Q.insert(std::pair<double,int>(cost,ei)).first;
                ds->C.row(ei) = place;
            }
        }
    }

    std::cout << "edge " << e << ", cost = " << cost << "  new v position (" << v(0) << ","<< v(1) << ","<< v(2) << ")\n"<< std::endl;
return true;
}

void ViewerInh::simplify(int num_to_collapse){
    edges *ds = data_edges[selected_data_index];
    Eigen::MatrixXd V = data_list[selected_data_index].V;
    Eigen::MatrixXi F = data_list[selected_data_index].F;
    Eigen::MatrixXd F_normals = data_list[selected_data_index].F_normals;
    int curr_collapsed = 0;

    const auto & collapse = [&ds, &V,  &F, &F_normals](){
            return my_collapse(V, F, F_normals,ds);
    };

    bool something_collapsed = false;
    while(curr_collapsed < num_to_collapse)
    {
        if(collapse()){
            ds->num_collapsed++;
            curr_collapsed++;
            something_collapsed = true;
        }else{
           break;
        }
    }

    if(something_collapsed){
        data_list[selected_data_index].clear();
        data_list[selected_data_index].set_mesh(V, F);
        data_list[selected_data_index].set_face_based(true);
    }

    data_edges[selected_data_index] = ds;
}

void ViewerInh::simplification(){
    long num_of_edges = data_edges[selected_data_index]->E.rows() - data_edges[selected_data_index]->num_collapsed;
    if (num_of_edges < 8)
        return;
    const int max_iter = std::ceil(num_of_edges * 0.05);
    simplify(max_iter);
}


#include<iostream>
#include<cstdlib>
#include<complex>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/lapack/geev.hpp>
#include "utils.h"

using std::cout;
using std::endl;
using std::vector;
using std::complex;

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

void geev(int);
template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>& );

int main(){
    cout << "I'm testing uBlas." << endl;

    int n = 5;
    geev(n);

}
void geev(int n){
    cout << "\nCalculating eigenvalues using LAPACK's geev." << endl;
    ublas::matrix<double, ublas::column_major> A(n,n);
    Hessenberg(A);
    print_m(A);

    ublas::vector<complex<double> > values(n);
    ublas::matrix<complex<double>, ublas::column_major>* Vectors_left = 0;
    ublas::matrix<complex<double>, ublas::column_major> Vectors_right(n,n);

    lapack::geev(A, values, Vectors_left, &Vectors_right, lapack::optimal_workspace());
    print_v(values, "values"); cout << endl;
    print_m(Vectors_right, "Vectors_right"); cout << endl;

    Hessenberg(A);
    cout << "A*x = l*x." << endl;
    for( int i = 0; i < Vectors_right.size2(); ++i ){
        ublas::vector<complex<double> > tmp(n);
        tmp = ublas::prod( A, column(Vectors_right, i) );
        cout << tmp - values(i)*column(Vectors_right,i) << endl;
    }

}
template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>& H){
    T k = 1;
    for( unsigned int i = 0; i < H.size1(); ++i ){
        for( unsigned int j = i; j <= H.size2(); ++j ){
            if( j > 0 ){
                H(i,j-1) = k;
                k += 1;
            }
        }
    }
}


#include "vmcsolver.h"
#include "lib.h"

#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

VMCSolver::VMCSolver() :
    nDimensions(3),
    charge(2),
    nParticles(2),
    h(0.001),
    h2(1000000),
    idum(-1),
    alpha(0.5*charge),
    nCycles(1000000),
    timestep(0.05),
    D(0.5)
{
}

void VMCSolver::runMonteCarloIntegration()
{
  rOld = zeros<mat>(nParticles, nDimensions);
  rNew = zeros<mat>(nParticles, nDimensions);
  QForceOld = zeros<mat>(nParticles, nDimensions);
  QForceNew = zeros<mat>(nParticles, nDimensions);

  double waveFunctionOld = 0;
  double waveFunctionNew = 0;

  double energySum = 0;
  double energySquaredSum = 0;

  double deltaE;

  // initial trial positions
  for(int i = 0; i < nParticles; i++) {
    for(int j = 0; j < nDimensions; j++) {
      rOld(i,j) = GaussianDeviate(&idum)*sqrt(timestep);
    }
  }
  rNew = rOld;

  // loop over Monte Carlo cycles
  for(int cycle = 0; cycle < nCycles; cycle++) {

    // Store the current value of the wave function
    waveFunctionOld = waveFunction(rOld);
    QuantumForce(rOld, QForceOld); QForceOld = QForceOld*h/waveFunctionOld;
    // New position to test
    for(int i = 0; i < nParticles; i++) {
      for(int j = 0; j < nDimensions; j++) {
	rNew(i,j) = rOld(i,j) + GaussianDeviate(&idum)*sqrt(timestep)+QForceOld(i,j)*timestep*D;
      }
      //  for the other particles we need to set the position to the old position since
      //  we move only one particle at the time
      for (int k = 0; k < nParticles; k++) {
	if ( k != i) {
	  for (int j=0; j < nDimensions; j++) {
	    rNew(k,j) = rOld(k,j);
	  }
	}
      }
      // Recalculate the value of the wave function and the quantum force
      waveFunctionNew = waveFunction(rNew);
      QuantumForce(rNew,QForceNew) = QForceNew*h/waveFunctionNew;
      //  we compute the log of the ratio of the greens functions to be used in the
      //  Metropolis-Hastings algorithm
      GreensFunction = 0.0;
      for (int j=0; j < nDimensions; j++) {
	GreensFunction += 0.5*(QForceOld(i,j)+QForceNew(i,j))*
	  (D*timestep*0.5*(QForceOld(i,j)-QForceNew(i,j))-rNew(i,j)+rOld(i,j));
      }
      GreensFunction = exp(GreensFunction);

      // The Metropolis test is performed by moving one particle at the time
      if(ran2(&idum) <= GreensFunction*(waveFunctionNew*waveFunctionNew) / (waveFunctionOld*waveFunctionOld)) {
	for(int j = 0; j < nDimensions; j++) {
	  rOld(i,j) = rNew(i,j);
	  QForceOld(i,j) = QForceNew(i,j);
	  waveFunctionOld = waveFunctionNew;
	}
      } else {
	for(int j = 0; j < nDimensions; j++) {
	  rNew(i,j) = rOld(i,j);
	  QForceNew(i,j) = QForceOld(i,j);
	}
      }
      // update energies
      deltaE = localEnergy(rNew);
      energySum += deltaE;
      energySquaredSum += deltaE*deltaE;
    }
  }
  double energy = energySum/(nCycles * nParticles);
  double energySquared = energySquaredSum/(nCycles * nParticles);
  cout << "Energy: " << energy << " Energy (squared sum): " << energySquared << endl;
}

double VMCSolver::localEnergy(const mat &r)
{
    mat rPlus = zeros<mat>(nParticles, nDimensions);
    mat rMinus = zeros<mat>(nParticles, nDimensions);

    rPlus = rMinus = r;

    double waveFunctionMinus = 0;
    double waveFunctionPlus = 0;

    double waveFunctionCurrent = waveFunction(r);

    // Kinetic energy

    double kineticEnergy = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = 0; j < nDimensions; j++) {
            rPlus(i,j) += h;
            rMinus(i,j) -= h;
            waveFunctionMinus = waveFunction(rMinus);
            waveFunctionPlus = waveFunction(rPlus);
            kineticEnergy -= (waveFunctionMinus + waveFunctionPlus - 2 * waveFunctionCurrent);
            rPlus(i,j) = r(i,j);
            rMinus(i,j) = r(i,j);
        }
    }
    kineticEnergy = 0.5 * h2 * kineticEnergy / waveFunctionCurrent;

    // Potential energy
    double potentialEnergy = 0;
    double rSingleParticle = 0;
    for(int i = 0; i < nParticles; i++) {
        rSingleParticle = 0;
        for(int j = 0; j < nDimensions; j++) {
            rSingleParticle += r(i,j)*r(i,j);
        }
        potentialEnergy -= charge / sqrt(rSingleParticle);
    }
    // Contribution from electron-electron potential
    double r12 = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = i + 1; j < nParticles; j++) {
            r12 = 0;
            for(int k = 0; k < nDimensions; k++) {
                r12 += (r(i,k) - r(j,k)) * (r(i,k) - r(j,k));
            }
            potentialEnergy += 1 / sqrt(r12);
        }
    }

    return kineticEnergy + potentialEnergy;
}

double VMCSolver::waveFunction(const mat &r)
{
    double argument = 0;
    for(int i = 0; i < nParticles; i++) {
        double rSingleParticle = 0;
        for(int j = 0; j < nDimensions; j++) {
            rSingleParticle += r(i,j) * r(i,j);
        }
        argument += sqrt(rSingleParticle);
    }
    return exp(-argument * alpha);
}

double VMCSolver::QuantumForce(const mat &r, mat &QForce)
{
    mat rPlus = zeros<mat>(nParticles, nDimensions);
    mat rMinus = zeros<mat>(nParticles, nDimensions);

    rPlus = rMinus = r;

    double waveFunctionMinus = 0;
    double waveFunctionPlus = 0;

    double waveFunctionCurrent = waveFunction(r);

    // Kinetic energy

    double kineticEnergy = 0;
    for(int i = 0; i < nParticles; i++) {
        for(int j = 0; j < nDimensions; j++) {
            rPlus(i,j) += h;
            rMinus(i,j) -= h;
            waveFunctionMinus = waveFunction(rMinus);
            waveFunctionPlus = waveFunction(rPlus);
            QForce(i,j) =  (waveFunctionPlus-waveFunctionMinus);
            rPlus(i,j) = r(i,j);
            rMinus(i,j) = r(i,j);
        }
    }
}


#ifndef TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP
#define TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP

#include <cmath>

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

#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/london.hpp>
#include <triumf/superconductivity/pippard.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace superconductivity {

// nonlocal effects on superconductivity in the Meissner-Ochsenfeld state
namespace nonlocal {

//
template <typename T = double>
T B_z_pippard(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for Pippard's model
  auto integrand = [&, K](T q) {
    // need to check if this is a reasaonble cutoff...
    auto kappa = q > 1e-5 ? 1.0
                          : (3.0 / 2.0) * ((1.0 + q * q) * std::atan(q) - q) /
                                (q * q * q);
    return q / (q * q + K * kappa);
  };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

// "extreme anomalous limit"
template <typename T = double>
auto B_z_extreme_anomalous_limit(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for the "extreme anomalous limit"
  auto integrand = [&, K](T q) { return q / (q * q + K / q); };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

// london
template <typename T = double>
auto B_z_london(const T z, const T B_0, const T K) {

  // return the applied field if z isn't below the surface
  if (z <= 0.0) {
    return B_0;
  }

  // integrand for the sine transform
  // using the Kernel for the "extreme anomalous limit"
  auto integrand = [&, K](T q) { return q / (q * q + K); };

  // create the integrator w/ default tolerance and evaluation levels
  // (root_epsilon and eight levels for type double).
  static boost::math::quadrature::ooura_fourier_sin<T> integrator =
      boost::math::quadrature::ooura_fourier_sin<T>();

  // evaluate the integral, which returns a pair
  // (first = integral, second = relative error)
  std::pair<T, T> result = integrator.integrate(integrand, z);

  // return the integral multiplied by the prefactors to get B vs. z
  return boost::math::constants::two_div_pi<T>() * B_0 * result.first;
}

} // namespace nonlocal

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_NONLOCAL_HPP


#include <cmath>
#include <cstdio>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() {
	int fib = 42;

    MatrixXd m(2, 2);
    m << 0, 0, 0, 0;
    EigenSolver<MatrixXd> mSolver(m);
    cout << mSolver.eigenvectors() << "\n";

	MatrixXcd T;

	T = MatrixXcd(3, 3);

	MatrixXd F(2, 2);
	F(0, 0) = 0;
	F(0, 1) = 1;
	F(1, 0) = 1;
	F(1, 1) = 1;
	EigenSolver<MatrixXd> eigensolver(F);

	MatrixXcd D = eigensolver.eigenvalues().asDiagonal();
	D(0, 0) = pow(D(0, 0), fib);
	D(1, 1) = pow(D(1, 1), fib);

	MatrixXcd P = eigensolver.eigenvectors();
	MatrixXcd Pinv = P.inverse();
	MatrixXcd S = (P * D * Pinv);

	cout << F << endl
		<< P << endl
		<< Pinv << endl
		<< D << endl
		<< S << endl;

	cout << "fib(" << fib << "):\n";
	cout << S << endl;
	printf("%.0f\n", round(real(S(1, 1))));

	return 0;
}


//////////////////////////////////////////////////////////////////////////////////
// statistics::survival::response::right_truncated::detail::logit_log.hpp 		//
//                                                                              //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                   //
//  Software License, Version 1.0. (See accompanying file                       //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)            //
//////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_LOGIT_LOG_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_LOGIT_LOG_HPP_ER_2009
#include <boost/statistics/detail/math/function/log_shift.hpp>
#include <boost/statistics/detail/math/function/logit_shift.hpp>
#include <boost/statistics/detail/survival/response/right_truncated/mean.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace survival{
namespace response{

template<typename T0,typename T1,typename T,typename B,typename ItO>
ItO logit_log_shift(
    const T0& c0,
    const T1& c1,
    const mean_event<T,B>& m,
    ItO o
)
{
    (*o++) = statistics::detail::math::logit_shift(m.failure_time(),c0);
    (*o++) = statistics::detail::math::log_shift(m.entry_time(),c1);
    return o;
}

template<typename T0,typename T,typename B,typename ItO>
ItO logit_log_shift(
    const T0& c0,
    const mean_event<T,B>& m,
    ItO o
)
{
    return logit_log(c0,c0,m,o);
}

}// response
}// survival
}// statistics
}// boost

#endif


#include <iostream>
#include <bitset>
#include <boost/dynamic_bitset.hpp>

using namespace std;

int bulbSwitch(int num){
    if(num<=0) return 0;
    boost::dynamic_bitset<> map(num);
    int i,j;
    for(j=1;j<=num;++j){
        for(i=j;i<=num;++i){
            if(i%j==0) map.flip(i-1);
        }
        //flag=!flag;
    }
    cout<<map.count()<<endl;
}

int main(int argc,char *argv[])
{
    //const int num = 3;
    //bitset<num> map;
    //vector<bool> map(false,num);
    int num;
    cin>>num;
    boost::dynamic_bitset<> map(num);

    int i,j;
    for(j=1;j<=num;++j){
        for(i=j;i<=num;++i){
            if(i%j==0) map.flip(i-1);
        }
        //flag=!flag;
        //cout<<map<<endl;
    }
    cout<<map.count()<<endl;
    return 0;
}


/**
 * @file stabrk3_main.cc
 * @brief NPDE homework StabRK3 code
 * @author Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iomanip>
#include <iostream>
#include <vector>

#include "stabrk3.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  double T = 1.0;
  Eigen::Vector2d y0(100.0, 1.0);
  Eigen::Vector2d yT_reference = StabRK3::predPrey(y0, T, 16384);
  std::cout << "Solution Computed by predPrey(): "
            << yT_reference.transpose().format(CSVFormat) << std::endl;

  // Vector of number of steps (for convergence study)
  std::vector<unsigned int> N_list = {4,   8,   16,   32,   64,   128,
                                      256, 512, 1024, 2048, 4096, 8192};

  // Compute approximations and take last one as reference
  std::vector<Eigen::Vector2d> yT_list = StabRK3::simulatePredPrey(N_list);

  // Compute the error table
  std::cout << std::setw(15) << "N" << std::setw(15) << "error" << std::setw(15)
            << "rate" << std::endl;
  double error_old;
  for (unsigned int j = 0; j < N_list.size(); ++j) {
    double error = (yT_list[j] - yT_reference).norm();
    std::cout << std::setw(15) << N_list[j] << std::setw(15) << error;
    if (j > 0) {
      std::cout << std::setw(15) << log2(error_old / error);
    }
    error_old = error;
    std::cout << std::endl;
  }

  return 0;
}