#pragma once

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include "vector-2.hpp"
#include <Eigen/Core>
#include <cmath>

namespace perceive
{
// --------------------------------------------------------------------- Vector3

#pragma pack(push, 1)
template<typename T> class Vector3T
{
 public:
   using value_type = T;

   T x, y, z;

   constexpr Vector3T()
       : x(T(0.0))
       , y(T(0.0))
       , z(T(0.0))
   {}
   constexpr Vector3T(T x_, T y_, T z_)
       : x(x_)
       , y(y_)
       , z(z_)
   {}
   constexpr Vector3T(const float p[3])
   {
      x = p[0];
      y = p[1];
      z = p[2];
   }
   constexpr Vector3T(const double p[3])
   {
      x = p[0];
      y = p[1];
      z = p[2];
   }
   constexpr Vector3T(const Vector2T<T>& p, double z_ = 0.0)
       : x(p.x)
       , y(p.y)
       , z(z_)
   {}
   // explicit Vector3T(const Eigen::Vector3f& v) : x(v(0)), y(v(1)), z(v(2)) {}
   // explicit Vector3T(const Eigen::Vector3d& v) : x(v(0)), y(v(1)), z(v(2)) {}

   static Vector3T nan() { return Vector3T(T(NAN), T(NAN), T(NAN)); }

   Vector3T& operator=(const Vector3T& v) = default;

   Vector3T& operator=(const Eigen::Vector3d& v)
   {
      for(int i = 0; i < 3; ++i) this->operator[](i) = v(i);
      return *this;
   }

   unsigned size() const { return 3; }

   Vector3T& normalise(T epsilon = T(1e-9))
   {
      // Don't normalize if we don't have to
      T mag2 = quadrance();
      if(std::fabs(mag2 - T(1.0)) > epsilon) {
         T mag_inv = T(1.0) / std::sqrt(mag2);
         x *= mag_inv;
         y *= mag_inv;
         z *= mag_inv;
      }
      return *this;
   }

   Vector3T& normalise_line(T epsilon = T(1e-9))
   {
      T mag2 = x * x + y * y;
      if(std::fabs(mag2 - T(1.0)) > epsilon and (mag2 > T(1e-20)))
         *this *= T(1.0) / std::sqrt(mag2);
      return *this;
   }

   Vector3T& normalise_point(T epsilon = T(1e-9))
   {
      if(std::fabs(z - T(1.0)) > epsilon) *this *= T(1.0) / z;
      return *this;
   }

   Vector3T normalised(T epsilon = T(1e-9)) const
   {
      Vector3T res = *this;
      res.normalise(epsilon);
      return res;
   }
   Vector3T normalised_line(T epsilon = T(1e-9)) const
   {
      Vector3T res = *this;
      res.normalise_line(epsilon);
      return res;
   }
   Vector3T normalised_point(T epsilon = T(1e-9)) const
   {
      Vector3T res = *this;
      res.normalise_point(epsilon);
      return res;
   }

   Vector3T& normalize(T ep = T(1e-9)) { return normalise(ep); }
   Vector3T& normalize_line(T ep = T(1e-9)) { return normalise_line(ep); }
   Vector3T& normalize_point(T ep = T(1e-9)) { return normalise_point(ep); }

   Vector3T normalized(T epsilon = T(1e-9)) const
   {
      return normalised(epsilon);
   }
   Vector3T normalized_line(T ep = T(1e-9)) const
   {
      return normalised_line(ep);
   }
   Vector3T normalized_point(T ep = T(1e-9)) const
   {
      return normalised_point(ep);
   }

   T quadrance() const { return x * x + y * y + z * z; }
   inline T norm() const;
   T dot(const Vector3T& o) const { return x * o.x + y * o.y + z * o.z; }
   T distance(const Vector3T& rhs) const { return (*this - rhs).norm(); }
   Vector3T cross(const Vector3T& v) const
   {
      Vector3T o;
      o.x = y * v.z - z * v.y;
      o.y = z * v.x - x * v.z;
      o.z = x * v.y - y * v.x;
      return o;
   }
   Vector3T left_cross(const Vector3T& rhs) const { return rhs.cross(*this); }

   Vector3T& set_to(const T& a, const T& b, const T& c)
   {
      x = a;
      y = b;
      z = c;
      return *this;
   }
   Vector3T& set_to(T a[3])
   {
      set_to(a[0], a[1], a[2]);
      return *this;
   }

   T* copy_to(T a[3]) const
   {
      a[0] = x;
      a[1] = y;
      a[2] = z;
      return a;
   }

   T* ptr()
   {
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
      return &x;
   }
   const T* ptr() const { return const_cast<Vector3T<T>*>(this)->ptr(); }
   T& operator[](int idx)
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 3);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 3);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx)
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 3);
#endif
      return ptr()[idx];
   }

   const T& operator()(int idx) const
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 3);
#endif
      return ptr()[idx];
   }

   Vector3T round() const
   {
      return Vector3T(
          floor(x + T(0.499)), floor(y + T(0.499)), floor(z + T(0.499)));
   }

   Vector3T& operator*=(T scalar)
   {
      x *= scalar;
      y *= scalar;
      z *= scalar;
      return *this;
   }
   Vector3T& operator/=(T scalar)
   {
      x /= scalar;
      y /= scalar;
      z /= scalar;
      return *this;
   }
   Vector3T operator*(T scalar) const
   {
      Vector3T res(*this);
      res *= scalar;
      return res;
   }
   Vector3T operator/(T scalar) const
   {
      Vector3T res(*this);
      res /= scalar;
      return res;
   }

   Vector3T& operator+=(const Vector3T& rhs)
   {
      x += rhs.x;
      y += rhs.y;
      z += rhs.z;
      return *this;
   }
   Vector3T& operator-=(const Vector3T& rhs)
   {
      x -= rhs.x;
      y -= rhs.y;
      z -= rhs.z;
      return *this;
   }
   Vector3T operator+(const Vector3T& rhs) const
   {
      Vector3T res(*this);
      res += rhs;
      return res;
   }
   Vector3T operator-(const Vector3T& rhs) const
   {
      Vector3T res(*this);
      res -= rhs;
      return res;
   }
   Vector3T operator-() const { return Vector3T(-x, -y, -z); }

   bool operator==(const Vector3T& rhs) const
   {
      return x == rhs.x && y == rhs.y && z == rhs.z;
   }
   bool operator!=(const Vector3T& rhs) const { return !(*this == rhs); }

   bool operator<(const Vector3T& o) const noexcept
   {
      return (x != o.x) ? (x < o.x) : (y != o.y) ? (y < o.y) : (z < o.z);
   }

   bool operator>(const Vector3T& o) const noexcept
   {
      return !(*this == o) and !(*this < o);
   }

   bool operator<=(const Vector3T& o) const noexcept
   {
      return (*this == o) or (*this < o);
   }

   bool operator>=(const Vector3T& o) const noexcept { return !(*this < o); }

   bool has_nan() const { return x != x || y != y || z != z; }
   bool is_nan() const
   {
      return std::isnan(x) || std::isnan(y) || std::isnan(z);
   }
   bool is_finite() const
   {
      return std::isfinite(x) && std::isfinite(y) && std::isfinite(z);
   }
   bool is_unit_vector(T epsilon = T(1e-9)) const
   {
      return is_finite() && fabs(quadrance() - 1.0) < epsilon;
   }

   inline friend bool isfinite(const Vector3T& o) noexcept
   {
      return o.is_finite();
   }

   std::string to_string(const char* fmt = nullptr) const
   {
      if(fmt == nullptr) {
         if constexpr(std::is_floating_point<T>::value) {
            fmt = "[{:7.5f}, {:7.5f}, {:7.5f}]";
         } else {
            fmt = "[{}, {}, {}]";
         }
      }
      return format(fmt, x, y, z);
   }
   std::string to_str() const { return format("[{}, {}, {}]", x, y, z); }

   void print(const char* msg = NULL, bool newline = true) const
   {
      printf("%s%s%s%s",
             (msg == NULL ? "" : msg),
             (msg == NULL ? "" : " "),
             to_string().c_str(),
             (newline ? "\n" : ""));
      fflush(stdout);
   }

   // Treat as spherical
   T& inclination() { return x; }
   T& azimuth() { return y; }
   T& r() { return z; }
   const T& inclination() const { return x; }
   const T& azimuth() const { return y; }
   const T& r() const { return z; }

   // Homogenous point
   bool pt_at_infinity(T epsilon = T(1e-9)) const { return fabs(z) < epsilon; }

   size_t hash() const { return sdbm_hash(ptr(), sizeof(T) * size()); }

   friend std::string str(const Vector3T<T>& o) noexcept
   {
      return o.to_string();
   }
};
#pragma pack(pop)

template<typename T> Vector3T<T> operator*(float a, const Vector3T<T>& v)
{
   return v * a;
}
template<typename T> Vector3T<T> operator/(float a, const Vector3T<T>& v)
{
   return v / a;
}
template<typename T> Vector3T<T> operator*(double a, const Vector3T<T>& v)
{
   return v * a;
}
template<typename T> Vector3T<T> operator/(double a, const Vector3T<T>& v)
{
   return v / a;
}

// String shim
template<typename T> std::string str(const Vector3T<T>& v)
{
   return v.to_string();
}
template<typename T>
std::ostream& operator<<(std::ostream& out, const Vector3T<T>& v)
{
   out << v.to_string();
   return out;
}

template<> inline float Vector3T<float>::norm() const
{
   return sqrtf(quadrance());
}
template<> inline double Vector3T<double>::norm() const
{
   return sqrt(quadrance());
}
} // namespace perceive


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file richardsonextrapolation.hpp
*/

#ifndef quantlib_richardson_extrapolation_hpp
#define quantlib_richardson_extrapolation_hpp

#include <ql/types.hpp>
#include <ql/utilities/null.hpp>
#include <boost/function.hpp>

namespace QuantLib {

    //! Richardson Extrapolation
    /*! Richardson Extrapolation is a sequence acceleration technique for
      \f[
          f(\Delta h) = f_0 + \alpha\cdot (\Delta h)^n + O((\Delta h)^{n+1})
      \f]
     */

    /*! References:
        http://en.wikipedia.org/wiki/Richardson_extrapolation
     */

    class RichardsonExtrapolation {
      public:
        /*! Richardon Extrapolation
           \param f function to be extrapolated to delta_h -> 0
           \param delta_h step size
           \param n if known, n is the order of convergence
         */
        RichardsonExtrapolation(const boost::function<Real (Real)>& f,
                                Real delta_h, Real n = Null<Real>());

        /*! Extrapolation for known order of convergence
            \param t scaling factor for the step size
        */
        Real operator()(Real t=2.0) const;

        /*! Extrapolation for unknown order of convergence
            \param t first scaling factor for the step size
            \param s second scaling factor for the step size
        */
        Real operator()(Real t, Real s) const;

      private:
        const Real delta_h_;
        const Real fdelta_h_;
        const Real n_;
        const boost::function<Real (Real)> f_;
    };
}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2012 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file richardsonextrapolation.cpp
*/

#include <ql/errors.hpp>
#include <ql/math/solvers1d/brent.hpp>

#include <cmath>

namespace QuantLib {
    namespace {
        class RichardsonEqn {
          public:
            RichardsonEqn(Real fh, Real ft, Real fs, Real t, Real s)
            : fdelta_h_(fh), ft_(ft), fs_(fs), t_(t), s_(s) { }

            Real operator()(Real k) const {
                return      ft_ + (ft_-fdelta_h_)/(std::pow(t_, k)-1.0)
                        - ( fs_ + (fs_-fdelta_h_)/(std::pow(s_, k)-1.0));
            }
          private:
            const Real fdelta_h_, ft_, fs_, t_, s_;
        };

    }

    inline RichardsonExtrapolation::RichardsonExtrapolation(
        const boost::function<Real (Real)>& f, Real delta_h, Real n)
    : delta_h_(delta_h),
      fdelta_h_(f(delta_h)),
      n_(n),
      f_(f) {
    }

    inline Real RichardsonExtrapolation::operator()(Real t) const {

        QL_REQUIRE(t > 1, "scaling factor must be greater than 1");
        QL_REQUIRE(n_ != Null<Real>(), "order of convergence must be known");

        const Real tk = std::pow(t, n_);

        return (tk*f_(delta_h_/t)-fdelta_h_)/(tk-1.0);
    }

    inline Real RichardsonExtrapolation::operator()(Real t, Real s)
    const {
        QL_REQUIRE(t > 1 && s > 1, "scaling factors must be greater than 1");
        QL_REQUIRE(t > s, "t must be greater than s");

        const Real ft = f_(delta_h_/t);
        const Real fs = f_(delta_h_/s);

        const Real k = Brent().solve(RichardsonEqn(fdelta_h_, ft, fs, t, s),
                                     1e-8, 0.05, 10);

        const Real ts = std::pow(s, k);

        return (ts*fs-fdelta_h_)/(ts-1.0);
    }
}

#endif


#pragma once

// #include <Eigen/Core>
#include <Eigen/Sparse>

#include "SU2meshparser.hpp"

namespace heatdiff
{
class HeatDiffusion2d
{
public:
    HeatDiffusion2d();
    ~HeatDiffusion2d();

    void SetMeshConfiguration(std::string meshfilename);
    void GenerateGrid();
    void SetFlowConfig(double volumetric_source, double thermal_cond,
                       double thickness);
    void SetBoundaryConditions(std::string tag, std::string bc_type,
                               double temperature);
    void SetInitialConditions(double init_temperature);
    void Solve();
    void WriteResultsToVtk(std::string vtkfilename);

    // For debug
    void PrintDebug();

private:
    std::string meshfilename_;
    std::vector<CellQuad4> cells_;
    std::vector<Node2d> nodes_;

    // Calculate initial cell values
    void CalcInitialValues();
    // Cinstruct matrices
    void ConstructMatrices();
    void UpdateFaceValues();
    void CalcNodalValues();

    double thickness_;

    // For Simultaneous Linear Equation
    //! Coefficient matrix
    Eigen::SparseMatrix<double> A;
    //! Temperature vector
    Eigen::VectorXd T;
    //! Source vector
    Eigen::VectorXd B;
};
}  // namespace heatdiff


//
// Created by mmath on 7/7/17.
//

#include <cmath>
#include <deque>
#include <algorithm>
#include <functional>
#include <tuple>
#include <iostream>

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include "Utilities.hpp"
#include "FunctionApprox.hpp"

namespace pyscan {

    std::ostream& operator<<(std::ostream& object, Vec3 const& v1) {
        object << "(" << v1[0] << ", " << v1[1] << ", " << v1[2] << ")";
        return object;
    }

    bool almostEqualRelative(double A, double B, double maxRelDiff) {
        // Calculate the difference.
        double diff = fabs(A - B);
        A = fabs(A);
        B = fabs(B);
        // Find the largest
        double largest = (B > A) ? B : A;
        return diff <= largest * maxRelDiff;
    }

    bool lineIntersection(Vec2 const& dir1, double d1, Vec2 const& dir2, double d2, Vec2& v_ext) {
        /*
         * Need to test two cases.
         *
         */
        if (almostEqualRelative(dir1[0] * dir2[1], dir1[1] * dir2[0], 8 * std::numeric_limits<double>::epsilon())) {
            return false;
        }

        double denom = 1 / (dir1[0] * dir2[1] - dir1[1] * dir2[0]);

        v_ext[0] = (dir2[1] * d1 - dir1[1] * d2) * denom;
        v_ext[1] = (dir1[0] * d2 - dir2[0] * d1) * denom;
        return true;
    }

    double det3(Vec3 const& dir1, Vec3 const& dir2, Vec3 const& dir3) {
        return dir1[0] * util::det2(dir2[1], dir3[1], dir2[2], dir3[2])
               - dir1[1] * util::det2(dir2[0], dir3[0], dir2[2], dir3[2])
               + dir1[2] * util::det2(dir2[0], dir3[0], dir2[1], dir3[1]);
    }

    using namespace boost::numeric::ublas;

    matrix<double> inverse3x3(matrix<double> const& m) {
        double det = m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) -
                     m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
                     m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));

        double invdet = 1 / det;

        matrix<double> minv(3, 3);
        minv(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) * invdet;
        minv(0, 1) = (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2)) * invdet;
        minv(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * invdet;
        minv(1, 0) = (m(1, 2) * m(2, 0) - m(1, 0) * m(2, 2)) * invdet;
        minv(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * invdet;
        minv(1, 2) = (m(1, 0) * m(0, 2) - m(0, 0) * m(1, 2)) * invdet;
        minv(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * invdet;
        minv(2, 1) = (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) * invdet;
        minv(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * invdet;

        return minv;
    }

    bool lineIntersection(Vec3 const& dir1, Vec3 const& p1,
                          Vec3 const& dir2, Vec3 const& p2,
                          Vec3 const& dir3, Vec3 const& p3,
                          Vec3& v_ext) {

        double d1 = dot(dir1, p1);
        double d2 = dot(dir2, p2);
        double d3 = dot(dir3, p3);
        //equation will be of the form.
        // a1 x + b1 y + c1 z = d1
        // a2 x + b2 y + c2 z = d2
        // a3 x + b3 y + c3 z = d3

        // Compute the rank of the system and see if it is singular
        double dval = det3(dir1, dir2, dir3);
        if (util::aeq(fabs(dval), 0)) {
            return false;
        }

        matrix<double> A(3, 3);

        A(0, 0) = dir1[0], A(0, 1) = dir1[1], A(0, 2) = dir1[2],
        A(1, 0) = dir2[0], A(1, 1) = dir2[1], A(1, 2) = dir2[2],
        A(2, 0) = dir3[0], A(2, 1) = dir3[1], A(2, 2) = dir3[2];

        vector<double> v(3);
        v[0] = d1, v[1] = d2, v[2] = d3;
        auto m = inverse3x3(A);

        v_ext[0] = m(0, 0) * v[0] + m(0, 1) * v[1] + m(0, 2) * v[2];
        v_ext[1] = m(1, 0) * v[0] + m(1, 1) * v[1] + m(1, 2) * v[2];
        v_ext[2] = m(2, 0) * v[0] + m(2, 1) * v[1] + m(2, 2) * v[2];
        return true;
    }

    bool inside(Vec2 const& pos, double alpha, double rho) {
      return (alpha <= pos[0]) && (pos[0] <= 1 - alpha) &&
        (rho <= pos[1]) && (pos[1] <= 1 - rho);
    }

    bool inRange(double a, double b) {
      return (b <= a) && (a <= 1 - b);
    }

    void lineIntersectionI(Vec2 const& dir1, double d1, Vec2 const& dir2, double d2, Vec2& v_ext) {
      /*
        If there is numerical instability then we project to infinity.
      */
      if (!lineIntersection(dir1, d1, dir2, d2, v_ext)) {
        v_ext[0] = std::numeric_limits<double>::infinity();
        v_ext[1] = std::numeric_limits<double>::infinity();
      }
    }

    Vec2 projectToBoundary(Vec2 const& pos, Vec2 const& dir, double alpha, double rho) {
      /*
        Takes the pos and projects it to the rectangle defined by
        [rho, 1 rho] x [alpha, 1 - alpha]
      */
      if (inside(pos, alpha, rho))
        return pos;

      Vec2 rho_b, rho_t, alpha_l, alpha_r;
      double d1 = dot(pos, dir);
      lineIntersectionI(dir, d1, Vec2{0.0, 1.0}, rho, rho_b);
      lineIntersectionI(dir, d1, Vec2{0.0, 1.0}, 1 - rho, rho_t);
      lineIntersectionI(dir, d1, Vec2{1.0, 0.0}, alpha, alpha_l);
      lineIntersectionI(dir, d1, Vec2{1.0, 0.0}, 1 - alpha, alpha_r);
      if (pos[1] < rho && inRange(rho_b[0], alpha))
          return rho_b;
      else if (1 - rho < pos[1] && inRange(rho_t[0], alpha))
        return rho_t;
      else if (pos[0] < alpha && inRange(alpha_l[1], rho))
        return alpha_l;
      else if (1 - alpha < pos[0] && inRange(alpha_r[1], rho))
        return alpha_r;
      // lives in the corner and no good projections
      else if (pos[0] < alpha) {
        if (pos[1] < rho)
          return {alpha, rho};
        else
          return {alpha, 1 - rho};
      } else {
        if (pos[1] < rho)
          return {1 - alpha, rho};
        else
          return {1 - alpha, 1 - rho};
      }
    }

    double approximateHull(double eps,
                           Vec2 const& cc, Vec2 const& cl,
                           std::function<double(Vec2)> phi, //function to maximize
                           std::function<Vec2(Vec2)> lineMaxExt) {

        auto lineMaxF = [&] (Vec2 v1) {
          auto pt = lineMaxExt(v1);
          //std::cout << "line_max = " << pt << std::endl;
          return pt; // projectToBoundary(pt, v1, alpha, rho);
        };

        auto avg = [&] (Vec2 const& v1, Vec2 const& v2) {
            Vec2 v_out = v1 + v2;
            Vec2 tmp;
            double norm = 1.0 / sqrt(v_out[0] * v_out[0] + v_out[1] * v_out[1]);
            tmp[0] = v_out[0] * norm;
            tmp[1] = v_out[1] * norm;
            return tmp;
        };

        struct Frame {
            Vec2 d_cc, d_cl, p_cc, p_cl;
            Frame(Vec2 const& di, Vec2 const& dj, Vec2 const& cc, Vec2 const& cl) :
                    d_cc(di), d_cl(dj), p_cc(cc), p_cl(cl) {}
        };
        double maxRValue = 0;

        std::deque<Frame> frameStack;
        // TODO double check debug to see if there is an issue with an infinite singularity here. Might need to change start.
        //This start needs to be fixed. Compute the mi, bi, and everything explicitly
        frameStack.emplace_back(cc, cl, lineMaxF(cc), lineMaxF(cl));

//#ifndef NDEBUG
//        size_t iter_count = 0;
//#endif
        while(!frameStack.empty()) {
            Frame lf = frameStack.front();

//#ifndef NDEBUG
//            std::cout << "considering triangle" << std::endl;
//            std::cout << lf.d_cc << " " << lf.p_cc << " " << lf.d_cl << " " << lf.p_cl << std::endl;
//#endif
            frameStack.pop_front();
            double di = dot(lf.d_cc, lf.p_cc);
            double dj = dot(lf.d_cl, lf.p_cl);
            double vi = phi(lf.p_cc);
            double vj = phi(lf.p_cl);
            maxRValue = std::max({vi, vj, maxRValue});
            Vec2 p_ext;

            if (lineIntersection(lf.d_cc, di, lf.d_cl, dj, p_ext)) {
                double vw = phi(p_ext);
//#ifndef NDEBUG
//                std::cout << lf.p_cc << " " << vi << std::endl;
//                std::cout << p_ext << " " << vw << std::endl;
//                std::cout << lf.p_cl << " " << vj << std::endl;
//#endif
                //dist(lf)
                if (vw - maxRValue > eps) {

                    //This triangle is worth evaluating
//#ifndef NDEBUG
//                    std::cout << "evaluating triangle" << std::endl;
//#endif
                    Vec2 m_vec = avg(lf.d_cc, lf.d_cl);
                    auto line_max = lineMaxF(m_vec);

                    frameStack.emplace_back(lf.d_cc, m_vec, lf.p_cc, line_max);
                    frameStack.emplace_back(m_vec, lf.d_cl, line_max, lf.p_cl);
                }
            }
//#ifndef NDEBUG
//            iter_count += 1;
//            std::cout << std::endl;
//#endif
        }
//#ifndef NDEBUG
//        std::cout << "maxRValue = " << maxRValue << std::endl;
//        std::cout << iter_count << std::endl;
//#endif
        return maxRValue;
    }

    double approximateHull(double eps,
                           std::function<double(Vec2)> phi, //function to maximize
                           std::function<Vec2(Vec2)> lineMaxF) {
        return std::max(approximateHull(eps, Vec2{1, 0}, Vec2{0, -1}, phi, lineMaxF),
                        approximateHull(eps, Vec2{-1, 0}, Vec2{0, 1}, phi, lineMaxF));
    }

    /*
     * Computes the height of a triangle that has corner points p1, p2, and pt where pt is the top corner.
     * p1, p2 -- corners of the base of the triangle
     * pt -- top corner of the triangle
     * return -- the height of the triangle as a double.
     */
    double height(Vec2 const& p1, Vec2 const& p2, Vec2 const& pt) {
        double a = sqrt((p1[0] - p2[0]) *(p1[0] - p2[0]) + (p1[1] - p2[1]) *(p1[1] - p2[1]));
        double b = sqrt((p1[0] - pt[0]) *(p1[0] - pt[0]) + (p1[1] - pt[1]) *(p1[1] - pt[1]));
        double c = sqrt((p2[0] - pt[0]) *(p2[0] - pt[0]) + (p2[1] - pt[1]) *(p2[1] - pt[1]));
        double s = (a + b + c)/ 2;
        return 2 * sqrt(s * (s - a) * (s - b) * (s - c)) / a;
    }

    std::vector<Vec2> eps_core_set(double eps,
                                   Vec2 const& cc, Vec2 const& cl,
                                   std::function<Vec2(Vec2)> lineMaxF) {

            auto avg = [&] (Vec2 const& v1, Vec2 const& v2) {
                Vec2 v_out = v1 + v2;
                Vec2 tmp;
                double norm = 1.0 / sqrt(v_out[0] * v_out[0] + v_out[1] * v_out[1]);
                tmp[0] = v_out[0] * norm;
                tmp[1] = v_out[1] * norm;
                return tmp;
            };

            struct Frame {
                Vec2 d_cc, d_cl, p_cc, p_cl;
                Frame(Vec2 const& di, Vec2 const& dj, Vec2 const& cc, Vec2 const& cl) :
                        d_cc(di), d_cl(dj), p_cc(cc), p_cl(cl) {}
            };
            auto pcc = lineMaxF(cc), pcl = lineMaxF(cl);
            std::vector<Vec2> pts{ pcc, pcl };
            std::deque<Frame> frameStack;
            frameStack.emplace_back(cc, cl, pcc, pcl);
            while(!frameStack.empty()) {
                Frame lf = frameStack.front();
                frameStack.pop_front();
                double di = dot(lf.d_cc, lf.p_cc);
                double dj = dot(lf.d_cl, lf.p_cl);

                Vec2 p_ext;

                if (lineIntersection(lf.d_cc, di, lf.d_cl, dj, p_ext)) {
                    if (height(lf.p_cc, lf.p_cl, p_ext) > eps) {

                        Vec2 m_vec = avg(lf.d_cc, lf.d_cl);
                        auto line_max = lineMaxF(m_vec);
                        pts.push_back(line_max);
                        frameStack.emplace_back(lf.d_cc, m_vec, lf.p_cc, line_max);
                        frameStack.emplace_back(m_vec, lf.d_cl, line_max, lf.p_cl);
                    }
                }
            }
            return pts;
    }

    std::vector<Vec2> eps_core_set(double eps,
                        std::function<Vec2(Vec2)> lineMaxF) {
        auto core_set1 = eps_core_set(eps, Vec2{1, 0}, Vec2{0, -1},  lineMaxF)
            ,core_set2 = eps_core_set(eps, Vec2{0, 1}, Vec2{1, 0}, lineMaxF)
            ,core_set3 = eps_core_set(eps, Vec2{-1, 0}, Vec2{0, 1}, lineMaxF)
            ,core_set4 = eps_core_set(eps, Vec2{0, -1}, Vec2{-1, 0}, lineMaxF);
        core_set1.insert(core_set1.end(), core_set2.begin(), core_set2.end());
        core_set1.insert(core_set1.end(), core_set3.begin(), core_set3.end());
        core_set1.insert(core_set1.end(), core_set4.begin(), core_set4.end());
        return core_set1;
    }

    point_list_t approx_hull(point_list_t const &pts, double eps) {
        auto max_f = [&] (Vec2 direction) {
            double max_dir = -std::numeric_limits<double>::infinity();
            pt2_t curr_pt(0.0, 0.0, 0.0);
            for (auto& pt : pts) {
                double curr_dir = direction[0] * pt(0) + direction[1] * pt(1);
                if (max_dir < curr_dir) {
                    max_dir = curr_dir;
                    curr_pt = pt;
                }
            }
            return Vec2{curr_pt(0), curr_pt(1)};
        };
        std::vector<pyscan::Point<>> core_set_pts;
        {
            auto vecs = eps_core_set(eps, max_f);
            for (auto &v :vecs) {
                core_set_pts.emplace_back(v[0], v[1], 1.0);
            }
        }
        remove_duplicates(core_set_pts);
        return core_set_pts;
    }

  }


#include "TrICP.h"

#include <Eigen/LU>
#include <Eigen/SVD>

using namespace navtypes;

struct PointPair // NOLINT(cppcoreguidelines-pro-type-member-init)
{
	point_t mapPoint;
	point_t samplePoint;
	double dist;
};

void heapify(PointPair arr[], int len, int i) {
	int smallest = i;
	int l = 2 * i + 1;
	int r = 2 * i + 2;
	if (l < len && arr[l].dist < arr[smallest].dist) {
		smallest = l;
	}
	if (r < len && arr[r].dist < arr[smallest].dist) {
		smallest = r;
	}

	if (smallest != i) {
		std::swap(arr[i], arr[smallest]);
		heapify(arr, len, smallest);
	}
}

// get some of the point pairs with the least distance using heapsort
std::vector<PointPair> getMinN(PointPair* arr, int len, int minNum) {
	for (int i = len / 2 - 1; i >= 0; i--) {
		heapify(arr, len, i);
	}

	std::vector<PointPair> ret;
	for (int i = len - 1; i > len - 1 - minNum; i--) {
		ret.push_back(arr[0]);
		std::swap(arr[0], arr[i]);
		heapify(arr, i, 0);
	}

	return ret;
}

transform_t computeTransformation(const std::vector<PointPair>& pairs) {
	/*
	 * We need to find a rigid transformation that maps points in the sample to points in
	 * the map. This transformation is not a regular affine, as only rotations and translations
	 * are permitted.
	 *
	 * We can do this with the Kabsch algorithm.
	 * https://en.wikipedia.org/wiki/Kabsch_algorithm
	 */
	Eigen::Vector2d mapCentroid = Eigen::Vector2d::Zero();
	Eigen::Vector2d sampleCentroid = Eigen::Vector2d::Zero();

	for (const PointPair& pair : pairs) {
		mapCentroid += pair.mapPoint.topRows<2>();
		sampleCentroid += pair.samplePoint.topRows<2>();
	}

	mapCentroid /= pairs.size();
	sampleCentroid /= pairs.size();

	int size = pairs.size();
	Eigen::Matrix<double, Eigen::Dynamic, 2> P(size, 2);
	Eigen::Matrix<double, Eigen::Dynamic, 2> Q(size, 2);
	for (int i = 0; i < size; i++) {
		P.row(i) = pairs[i].samplePoint.topRows<2>() - sampleCentroid;
		Q.row(i) = pairs[i].mapPoint.topRows<2>() - mapCentroid;
	}

	Eigen::Matrix2d H = P.transpose() * Q;

	// computing SVD for square matrices, so no QR preconditioner needed
	Eigen::JacobiSVD<Eigen::Matrix2d, Eigen::NoQRPreconditioner> svd;
	svd.compute(H, Eigen::ComputeFullU | Eigen::ComputeFullV);

	Eigen::Matrix2d U = svd.matrixU();
	Eigen::Matrix2d V = svd.matrixV();
	double d = (V * U.transpose()).determinant() > 0 ? 1 : -1;
	Eigen::Matrix2d D = Eigen::Matrix2d::Identity();
	D.bottomRightCorner<1, 1>()(0, 0) = d;
	Eigen::Matrix2d R = V * D * U.transpose();

	Eigen::Vector2d translation = mapCentroid - R * sampleCentroid;

	transform_t trf = transform_t::Identity();
	trf.topLeftCorner<2, 2>() = R;
	trf.topRightCorner<2, 1>() = translation;
	return trf;
}

TrICP::TrICP(int maxIter, double relErrChangeThresh,
			 std::function<point_t(const point_t&)> getClosest)
	: maxIter(maxIter), relErrChangeThresh(relErrChangeThresh),
	  getClosest(std::move(getClosest)) {
}

transform_t TrICP::correct(const points_t& sample, double overlap) {
	if (sample.empty() || overlap == 0) {
		return transform_t::Identity();
	}
	int i = 0;
	double mse = 1e9;
	double oldMSE;
	points_t points = sample;
	transform_t trf = transform_t::Identity();
	int N = static_cast<int>(overlap * sample.size());
	do {
		i++;
		oldMSE = mse;
		transform_t t = iterate(points, N, mse);
		trf = t * trf;
	} while (!isDone(i, mse, oldMSE));

	return trf;
}

bool TrICP::isDone(int numIter, double mse, double oldMSE) const {
	if (mse <= 1e-9) {
		return true;
	}
	double relErrChange = fabs(mse - oldMSE) / mse;
	return numIter >= maxIter || relErrChange <= relErrChangeThresh;
}

transform_t TrICP::iterate(points_t& sample, int N, double& mse) const {
	PointPair pairs[sample.size()];
	for (size_t i = 0; i < sample.size(); i++) {
		const point_t& point = sample[i];
		point_t closestPoint = getClosest(point);
		double dist = (point - closestPoint).norm();
		PointPair pair{closestPoint, point, dist};
		pairs[i] = pair;
	}

	std::vector<PointPair> closestPairs = getMinN(pairs, sample.size(), N);

	double newS = 0;
	for (const PointPair& pair : closestPairs) {
		newS += pair.dist * pair.dist;
	}
	mse = newS / N;

	transform_t trf = computeTransformation(closestPairs);

	for (point_t& point : sample) {
		point = trf * point;
	}
	return trf;
}


#pragma once

/// @file

#include "numeric/dense_matrix.hpp"
#include <Eigen/SVD>

namespace neon
{
/// svd computes the singular value decomposition of an input matrix and
/// provides access to the left and right singular vectors and the singular
/// values.
class svd
{
public:
    svd() = default;

    virtual ~svd() = default;

    /// compute thin SVD decomposition of a matrix A
    virtual void compute(col_matrix const& A) = 0;

    /// Compute thin SVD decomposition
    /// \param A Input matrix
    /// \param n Maximum number of singular vectors
    virtual void compute(col_matrix const& A, std::int64_t const n) = 0;

    /// compute thin SVD decomposition of a matrix A with a tolerance on the singular values
    /// A singular value will be considered nonzero if its value is strictly greater than
    /// \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
    virtual void compute(col_matrix const& A, double const tolerance) = 0;

    /// \return left singular vectors (columns of the thin U matrix)
    virtual col_matrix const& left() const noexcept = 0;

    /// \return right singular vectors (columns of the thin V matrix)
    virtual col_matrix const& right() const noexcept = 0;

    /// \return singular values
    virtual vector const& values() const noexcept = 0;

    /// A least-squares solution of A*x = b
    /// linear compination of columns of A
    virtual void solve(vector& x, vector const& b) const noexcept = 0;

protected:
    col_matrix left_vectors;
    col_matrix right_vectors;
    vector singular_values;
};

/// bdc_svd first reduces the input matrix to bi-diagonal form and then performs a
/// divide-and-conquer diagonalization. Small blocks are diagonalized using class
/// JacobiSVD. Default switching size is 16.
class bdc_svd : public svd
{
public:
    bdc_svd() = default;

    bdc_svd(col_matrix const& A);

    void compute(col_matrix const& A) override;

    void compute(col_matrix const& A, std::int64_t const n) override;

    void compute(col_matrix const& A, double const tolerance) override;

    col_matrix const& left() const noexcept override;

    col_matrix const& right() const noexcept override;

    vector const& values() const noexcept override;

    void solve(vector& x, vector const& b) const noexcept override;

private:
    Eigen::BDCSVD<col_matrix> decomposition;
};

/// Implementation of the truncated Singular Value Decomposition, using
/// randomized algorithms as described in 'finding structure with randomness'
/// @cite halko2011finding.
class randomised_svd : public svd
{
public:
    randomised_svd() = default;

    randomised_svd(col_matrix const& A);

    void compute(col_matrix const& A) override;

    void compute(col_matrix const& A, std::int64_t const n) override;

    void compute(col_matrix const& A, double const tolerance) override;

    col_matrix const& left() const noexcept override;

    col_matrix const& right() const noexcept override;

    vector const& values() const noexcept override;

    void solve(vector& x, vector const& b) const noexcept override;

private:
    Eigen::BDCSVD<col_matrix> decomposition;
};
}


#ifndef EXPSUM_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP
#define EXPSUM_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP

#include <armadillo>

namespace expsum
{
///
/// Partial Lanczos bidiagonalization with full reorthogonalization
///
/// @param matvec  compute `y = a * A) * x + b * y` with matrix-free form
/// @param matvec_trans compute `y = a * A.t() * x + b * y` with matrix-free
///   form
/// @param[out] alpha  real vector of size `n` that stores diagonal part of B.
/// @param[out] beta   real vector of size `n - 1` that stores superdiagonal
///   part of B.
/// @param[out] P  on exit matrix P
/// @param[out] Q  on exit matrix Q
/// @param[out] rank  on exit rank of matrix A
/// @param[inout] tol_error on entry tolerance of residuals, and on exit an
///   esitmation of residual.
/// @param[inout] work workspace
///
template <typename MatVec, typename MatVecTrans, typename T>
void partial_lanczos_bidiagonalization(
    MatVec matvec, MatVecTrans matvec_trans,
    arma::Col<typename arma::Col<T>::pod_type>& alpha,
    arma::Col<typename arma::Col<T>::pod_type>& beta, arma::Mat<T>& P,
    arma::Mat<T>& Q, arma::uword& rank,
    typename arma::Col<T>::pod_type& tol_error, arma::Col<T>& work)
{
    using value_type = T;
    using real_type  = typename arma::Col<T>::pod_type;

    // constexpr static const value_type zero = value_type();
    // constexpr static const value_type one  = value_type(1);

    arma::uword m        = P.n_rows;
    arma::uword n        = Q.n_rows;
    arma::uword k        = P.n_cols;
    arma::uword max_rank = std::min({m, n, k});

    assert(Q.n_cols == k);
    assert(work.size() >= max_rank);
    assert(real_type() < tol_error && tol_error < real_type(1));

    auto q0 = Q.col(0);
    q0.randn();
    q0 /= arma::norm(q0);
    auto p0 = P.col(0);
    // p0 <-- A * q0
    matvec(q0, value_type(), p0);
    auto a1 = arma::norm(p0);
    if (a1 > real_type())
    {
        p0 *= real_type(1) / a1;
    }
    alpha(0) = a1;

    const real_type tol2 = tol_error * tol_error;
    // Estimation of the Frobenius norm of A
    real_type fnormA = a1 * a1;
    // Estimation of relative error
    real_type error = real_type();

    rank = 0;

    while (++rank < max_rank)
    {
        auto p1 = P.col(rank - 1);
        auto p2 = P.col(rank);
        auto q1 = Q.col(rank - 1);
        auto q2 = Q.col(rank);
        //
        // --- Recursion for right Lanczos vector
        //
        // q2 <-- A.t() * p1 - a1 * q1
        matvec_trans(p1, value_type(), q2);
        q2 -= a1 * q1;
        // Reorthogonalization
        auto tmp   = work.head(rank);
        auto viewQ = Q.head_cols(rank);
        tmp        = viewQ.t() * q2;
        q2 -= viewQ * tmp;

        auto b1 = arma::norm(q2);
        if (b1 > real_type())
        {
            q2 *= real_type(1) / b1;
        }
        beta(rank - 1) = b1;
        //
        // --- Recursion for left Lanczos vector
        //
        // p2 <-- A * p2 - b1 * p1
        matvec(q2, value_type(), p2);
        p2 -= b1 * p1;
        // Reorthogonalization
        auto viewP = P.head_cols(rank);
        tmp        = viewP.t() * p2;
        p2 -= viewP * tmp;

        const auto a2 = arma::norm(p2);
        if (a2 > real_type())
        {
            p2 *= real_type(1) / a2;
        }
        alpha(rank) = a2;

        const auto t = a2 * a2 + b1 * b1;
        //
        // Estimation of the Frobenius norm of A
        //
        // \|A\|_{F}^{2}= \sum_{K=1}^{rank(A)-1}
        //              (\alpha_{K}^{2} + \beta_{K}^{2}) + \alpha_{rank(A)}}
        //
        fnormA += t;
        error = t;
        if (t <= tol2 * fnormA)
        {
            // Converged
            break;
        }

        a1 = a2;
    }

    tol_error = sqrt(error);
    return;
}

} // namespace: expsum

#endif /* NUMERIC_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP */


//####### Test module for unit conversion ####################################

//Define Module name
 #define BOOST_TEST_MODULE "phys/unit_conversion"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <picsar_qed/math/math_constants.h>
#include <picsar_qed/physics/phys_constants.h>
#include <picsar_qed/physics/unit_conversion.hpp>

#include <array>
#include <algorithm>
#include <functional>

using namespace picsar::multi_physics::phys;
using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//Auxiliary functions for tests
template<typename RealType>
struct val_pack{
    RealType SI;
    RealType omega;
    RealType lambda;
    RealType hl;
};

template<typename RealType, quantity Quantity>
constexpr void test_to_SI(val_pack<RealType> vals,
    RealType reference_omega = -1.0,
    RealType reference_length = -1.0)
{
    const auto fact_SI =
        conv<Quantity, unit_system::SI,
            unit_system::SI, RealType>::fact();
    const auto fact_omega =
        conv<Quantity, unit_system::norm_omega,
            unit_system::SI, RealType>::fact(reference_omega);
    const auto fact_lambda =
        conv<Quantity, unit_system::norm_lambda,
            unit_system::SI, RealType>::fact(reference_length);
    const auto fact_hl =
        conv<Quantity, unit_system::heaviside_lorentz,
            unit_system::SI, RealType>::fact();

    const auto res_SI2SI = vals.SI*fact_SI;
    const auto res_omega2SI = vals.omega*fact_omega;
    const auto res_lambda2SI = vals.lambda*fact_lambda;
    const auto res_hl2SI = vals.hl*fact_hl;
    const auto all_res = std::array<RealType,4>{
        res_SI2SI, res_omega2SI, res_lambda2SI, res_hl2SI};
    for (const auto& res : all_res)
        BOOST_CHECK_SMALL((res-vals.SI)/vals.SI, tolerance<RealType>());
}

template<typename RealType, quantity Quantity>
constexpr void test_from_to(
    val_pack<RealType> vals,
    RealType reference_omega = -1.0,
    RealType reference_length = -1.0)

{
    const auto from_SI_to_all = std::array<RealType,4>
    {
        vals.SI*conv<Quantity, unit_system::SI,
            unit_system::SI, RealType>::fact(),
        vals.SI*conv<Quantity, unit_system::SI,
            unit_system::norm_omega, RealType>::fact(1.0,reference_omega),
        vals.SI*conv<Quantity, unit_system::SI,
            unit_system::norm_lambda, RealType>::fact(1.0,reference_length),
        vals.SI*conv<Quantity, unit_system::SI,
            unit_system::heaviside_lorentz, RealType>::fact(),
    };

    const auto from_omega_to_all = std::array<RealType,4>
    {
        vals.omega*conv<Quantity, unit_system::norm_omega,
            unit_system::SI, RealType>::fact(reference_omega),
        vals.omega*conv<Quantity, unit_system::norm_omega,
            unit_system::norm_omega, RealType>::fact(reference_omega, reference_omega),
        vals.omega*conv<Quantity, unit_system::norm_omega,
            unit_system::norm_lambda, RealType>::fact(reference_omega, reference_length),
        vals.omega*conv<Quantity, unit_system::norm_omega,
            unit_system::heaviside_lorentz, RealType>::fact(reference_omega),
    };

    const auto from_lambda_to_all = std::array<RealType,4>
    {
        vals.lambda*conv<Quantity, unit_system::norm_lambda,
            unit_system::SI, RealType>::fact(reference_length),
        vals.lambda*conv<Quantity, unit_system::norm_lambda,
            unit_system::norm_omega, RealType>::fact(reference_length, reference_omega),
        vals.lambda*conv<Quantity, unit_system::norm_lambda,
            unit_system::norm_lambda, RealType>::fact(reference_length, reference_length),
        vals.lambda*conv<Quantity, unit_system::norm_lambda,
            unit_system::heaviside_lorentz, RealType>::fact(reference_length, 1.0),
    };

    const auto from_hl_to_all = std::array<RealType,4>
    {
        vals.hl*conv<Quantity, unit_system::heaviside_lorentz,
            unit_system::SI, RealType>::fact(),
        vals.hl*conv<Quantity, unit_system::heaviside_lorentz,
            unit_system::norm_omega, RealType>::fact(1.0,reference_omega),
        vals.hl*conv<Quantity, unit_system::heaviside_lorentz,
            unit_system::norm_lambda, RealType>::fact(1.0,reference_length),
        vals.hl*conv<Quantity, unit_system::heaviside_lorentz,
            unit_system::heaviside_lorentz, RealType>::fact(),
    };

    const auto fact_SI = conv<Quantity, unit_system::SI,
        unit_system::SI, RealType>::fact();
    const auto fact_omega = conv<Quantity, unit_system::norm_omega,
        unit_system::SI, RealType>::fact(reference_omega);
    const auto fact_lambda = conv<Quantity, unit_system::norm_lambda,
        unit_system::SI, RealType>::fact(reference_length);
    const auto fact_hl = conv<Quantity, unit_system::heaviside_lorentz,
        unit_system::SI, RealType>::fact();

    const auto all_facts = std::array<RealType, 4>{
        fact_SI, fact_omega, fact_lambda, fact_hl};

    const auto all_data = std::array<std::array<RealType, 4>, 4>{
        from_SI_to_all,
        from_omega_to_all,
        from_lambda_to_all,
        from_hl_to_all
    };

    for (auto data: all_data)
    {
        std::transform( data.begin(), data.end(),
            all_facts.begin(), data.begin(),
            std::multiplies<RealType>());

        for (const auto& res : data){
            BOOST_CHECK_SMALL((res-vals.SI)/vals.SI, tolerance<RealType>());
        }
    }
}

// ------------- Tests --------------

// ***Test energy reference for Heaviside Lorentz units
template<typename RealType>
void test_case_hl_reference_energy()
{
    BOOST_CHECK_SMALL(
        (heaviside_lorentz_reference_energy<RealType>-MeV<RealType>)/MeV<RealType>,
        tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_heaviside_lorentz_ref_energy )
{
    test_case_hl_reference_energy<double>();
    test_case_hl_reference_energy<float>();
}

// ***Test electron rest energy in Heaviside Lorentz units
template<typename RealType>
void test_case_hl_electron_rest_energy()
{
    constexpr auto exp = static_cast<RealType>(
        electron_mass<double>*light_speed<double>*light_speed<double>/
        MeV<double>);
    constexpr auto res = heaviside_lorentz_electron_rest_energy<RealType>;
    BOOST_CHECK_SMALL((res-exp)/exp, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_heaviside_lorentz_electron_rest_energy )
{
    test_case_hl_electron_rest_energy<double>();
    test_case_hl_electron_rest_energy<float>();
}

// ***Test Schwinger field in Heaviside Lorentz units
template<typename RealType>
void test_case_hl_schwinger_field()
{
    constexpr auto exp = static_cast<RealType>(
        schwinger_field<double>*conv<quantity::E,
            unit_system::SI, unit_system::heaviside_lorentz, double>::fact());
    constexpr auto res = heaviside_lorentz_schwinger_field<RealType>;
    BOOST_CHECK_SMALL((res-exp)/exp, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_heaviside_lorentz_schwinger_field )
{
    test_case_hl_schwinger_field<double>();
    test_case_hl_schwinger_field<float>();
}

// ***Test elementary charge in Heaviside Lorentz units
template<typename RealType>
void test_case_hl_elementary_charge()
{
    constexpr auto exp = static_cast<RealType>(
        elementary_charge<double>*conv<quantity::charge,
            unit_system::SI, unit_system::heaviside_lorentz, double>::fact());
    constexpr auto res = heaviside_lorentz_elementary_charge<RealType>;
    BOOST_CHECK_SMALL((res-exp)/exp, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_heaviside_lorentz_elementary_charge )
{
    test_case_hl_elementary_charge<double>();
    test_case_hl_elementary_charge<float>();
}

// ***Test mass conversion to SI and all to all
template<typename RealType>
void test_case_mass()
{
    constexpr auto mass_SI = electron_mass<RealType>;
    constexpr auto mass_omega = static_cast<RealType>(1.0);
    constexpr auto mass_lambda = static_cast<RealType>(1.0);
    constexpr auto mass_hl = electron_mass<RealType>*light_speed<RealType>*
        light_speed<RealType>/heaviside_lorentz_reference_energy<RealType>;
    constexpr auto all_masses = val_pack<RealType>{mass_SI, mass_omega, mass_lambda, mass_hl};

    test_to_SI<RealType, quantity::mass>(all_masses);
    test_from_to<RealType, quantity::mass>(all_masses);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_mass )
{
    test_case_mass<double>();
    test_case_mass<float>();
}

// ***Test charge conversion to SI and all to all
template<typename RealType>
void test_case_charge()
{
    constexpr auto charge_SI = elementary_charge<RealType>;
    constexpr auto charge_omega = static_cast<RealType>(1.0);
    constexpr auto charge_lambda = static_cast<RealType>(1.0);
    constexpr auto charge_hl = sqrt_4_pi_fine_structure<RealType>;
    constexpr auto all_charges = val_pack<RealType>{charge_SI, charge_omega, charge_lambda, charge_hl};

    test_to_SI<RealType, quantity::charge>(all_charges);
    test_from_to<RealType, quantity::charge>(all_charges);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_charge )
{
    test_case_charge<double>();
    test_case_charge<float>();
}

// ***Test velocity conversion to SI and all to all
template<typename RealType>
void test_case_velocity()
{
    constexpr auto velocity_SI = light_speed<RealType>;
    constexpr auto velocity_omega = static_cast<RealType>(1.0);
    constexpr auto velocity_lambda = static_cast<RealType>(1.0);
    constexpr auto velocity_hl = static_cast<RealType>(1.0);
    constexpr auto all_velocities = val_pack<RealType>{velocity_SI, velocity_omega, velocity_lambda, velocity_hl};

    test_to_SI<RealType, quantity::velocity>(all_velocities);
    test_from_to<RealType, quantity::velocity>(all_velocities);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_velocity )
{
    test_case_velocity<double>();
    test_case_velocity<float>();
}

// ***Test momentum conversion to SI and all to all
template<typename RealType>
void test_case_momentum()
{
    constexpr auto momentum_SI = electron_mass<RealType>*light_speed<RealType>;
    constexpr auto momentum_omega = static_cast<RealType>(1.0);
    constexpr auto momentum_lambda = static_cast<RealType>(1.0);
    constexpr auto momentum_hl = static_cast<RealType>(
        electron_mass<double>*light_speed<double>*light_speed<double>/
        heaviside_lorentz_reference_energy<double>);
    constexpr auto all_momenta = val_pack<RealType>{momentum_SI, momentum_omega, momentum_lambda, momentum_hl};

    test_to_SI<RealType, quantity::momentum>(all_momenta);
    test_from_to<RealType, quantity::momentum>(all_momenta);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_momentum )
{
    test_case_momentum<double>();
    test_case_momentum<float>();
}

// ***Test energy conversion to SI and all to all
template<typename RealType>
void test_case_energy()
{
    constexpr auto energy_SI = GeV<RealType>;
    constexpr auto energy_omega = static_cast<RealType>(
        GeV<double>/electron_mass<double>/light_speed<double>/light_speed<double>);
    constexpr auto energy_lambda = static_cast<RealType>(
        GeV<double>/electron_mass<double>/light_speed<double>/light_speed<double>);
    constexpr auto energy_hl = static_cast<RealType>(
        GeV<double>/heaviside_lorentz_reference_energy<double>);
    constexpr auto all_energies = val_pack<RealType>{energy_SI, energy_omega, energy_lambda, energy_hl};

    test_to_SI<RealType, quantity::energy>(all_energies);
    test_from_to<RealType, quantity::energy>(all_energies);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_energy )
{
    test_case_energy<double>();
    test_case_energy<float>();
}

// ***Test length conversion to SI and all to all
template<typename RealType>
void test_case_length()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr auto length_SI = reference_length;
    constexpr auto length_omega = static_cast<RealType>(2.0* pi<double>);
    constexpr auto length_lambda = static_cast<RealType>(1.0);
    constexpr auto length_hl = static_cast<RealType>(
        heaviside_lorentz_reference_energy<double>*reference_length/
        reduced_plank<double>/light_speed<double>);

    constexpr auto all_lenghts = val_pack<RealType>{length_SI, length_omega, length_lambda, length_hl};

    test_to_SI<RealType, quantity::length>(
        all_lenghts, reference_omega, reference_length);
    test_from_to<RealType, quantity::length>(
        all_lenghts, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_length)
{
    test_case_length<double>();
    test_case_length<float>();
}

// ***Test area conversion to SI and all to all
template<typename RealType>
void test_case_area_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr auto area_SI = reference_length*reference_length;
    constexpr auto area_omega = static_cast<RealType>(4.0* pi<double>*pi<double>);
    constexpr auto area_lambda = static_cast<RealType>(1.0);
    constexpr auto area_hl = static_cast<RealType>(
        heaviside_lorentz_reference_energy<double>*
        heaviside_lorentz_reference_energy<double>*
        reference_length*reference_length/
        reduced_plank<double>/reduced_plank<double>/
        light_speed<double>/light_speed<double>);

    constexpr auto all_areas = val_pack<RealType>{area_SI, area_omega, area_lambda, area_hl};

    test_to_SI<RealType, quantity::area>(
        all_areas, reference_omega, reference_length);
    test_from_to<RealType, quantity::area>(
        all_areas, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_area_to_SI )
{
    test_case_area_to_SI<double>();
    test_case_area_to_SI<float>();
}

// ***Test volume conversion to SI and all to all
template<typename RealType>
void test_case_volume_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr auto volume_SI = reference_length*reference_length*reference_length;
    constexpr auto volume_omega = static_cast<RealType>(8.0*
        pi<double>*pi<double>*pi<double>);
    constexpr auto volume_lambda = static_cast<RealType>(1.0);
    constexpr auto volume_hl = static_cast<RealType>(
        heaviside_lorentz_reference_energy<double>*
        heaviside_lorentz_reference_energy<double>*
        heaviside_lorentz_reference_energy<double>*
        reference_length*reference_length*reference_length/
        reduced_plank<double>/reduced_plank<double>/reduced_plank<double>/
        light_speed<double>/light_speed<double>/light_speed<double>);

    constexpr auto all_volumes = val_pack<RealType>{volume_SI, volume_omega, volume_lambda, volume_hl};

    test_to_SI<RealType, quantity::volume>(
        all_volumes, reference_omega, reference_length);
    test_from_to<RealType, quantity::volume>(
        all_volumes, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_volume_to_SI )
{
    test_case_volume_to_SI<double>();
    test_case_volume_to_SI<float>();
}

// ***Test time conversion to SI and all to all
template<typename RealType>
void test_case_time_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr auto time_SI = static_cast<RealType>(reference_length/light_speed<double>);
    constexpr auto time_omega = static_cast<RealType>(2.0*pi<double>);
    constexpr auto time_lambda = static_cast<RealType>(1.0);
    constexpr auto time_hl = static_cast<RealType>(
        (reference_length/light_speed<double>)*
        heaviside_lorentz_reference_energy<double>/
        reduced_plank<double>);

    constexpr auto all_times = val_pack<RealType>{time_SI, time_omega, time_lambda, time_hl};

    test_to_SI<RealType, quantity::time>(
        all_times, reference_omega, reference_length);
    test_from_to<RealType, quantity::time>(
        all_times, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_time_to_SI )
{
    test_case_time_to_SI<double>();
    test_case_time_to_SI<float>();
}

// ***Test rate conversion to SI and all to all
template<typename RealType>
void test_case_rate_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr auto rate_SI = static_cast<RealType>(light_speed<double>/reference_length);
    constexpr auto rate_omega = static_cast<RealType>(1/(2.0*pi<double>));
    constexpr auto rate_lambda = static_cast<RealType>(1.0);
    constexpr auto rate_hl = static_cast<RealType>(
        (light_speed<double>/reference_length)*
        reduced_plank<double>/
        heaviside_lorentz_reference_energy<double>);

    constexpr auto all_rates = val_pack<RealType>{rate_SI, rate_omega, rate_lambda, rate_hl};

    test_to_SI<RealType, quantity::rate>(
        all_rates, reference_omega, reference_length);
    test_from_to<RealType, quantity::rate>(
        all_rates, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_rate_to_SI )
{
    test_case_rate_to_SI<double>();
    test_case_rate_to_SI<float>();
}

// ***Test E conversion to SI and all to all
template<typename RealType>
void test_case_E_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr double ref_field_omega = reference_omega*
        (electron_mass<double>*light_speed<double>/elementary_charge<double>);
    constexpr double ref_field_length = electron_mass<double>*
        light_speed<double>*light_speed<double>/elementary_charge<double>/reference_length;
    constexpr double ref_field_hl =
        heaviside_lorentz_reference_energy<double>*
        heaviside_lorentz_reference_energy<double>*
        sqrt_4_pi_fine_structure<double>/
        elementary_charge<double>/reduced_plank<double>/light_speed<double>;

    constexpr auto E_SI = schwinger_field<RealType>;
    constexpr auto E_omega = static_cast<RealType>(
        schwinger_field<double>/ref_field_omega);
    constexpr auto E_lambda = static_cast<RealType>(schwinger_field<double>/
        ref_field_length);
    constexpr auto E_hl = static_cast<RealType>(schwinger_field<double>/
        ref_field_hl);

    constexpr auto all_E = val_pack<RealType>{E_SI, E_omega, E_lambda, E_hl};

    test_to_SI<RealType, quantity::E>(
        all_E, reference_omega, reference_length);
    test_from_to<RealType, quantity::E>(
        all_E, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_E_to_SI )
{
    test_case_E_to_SI<double>();
    test_case_E_to_SI<float>();
}

// ***Test B conversion to SI and all to all
template<typename RealType>
void test_case_B_to_SI()
{
    constexpr auto reference_length = static_cast<RealType>(800.0e-9);
    constexpr auto reference_omega = static_cast<RealType>(
        2.0*pi<double>*light_speed<double>/reference_length);

    constexpr double ref_field_omega = reference_omega*
        (electron_mass<double>/elementary_charge<double>);
    constexpr double ref_field_length = electron_mass<double>*
        light_speed<double>/elementary_charge<double>/reference_length;
    constexpr double ref_field_hl =
        heaviside_lorentz_reference_energy<double>*
        heaviside_lorentz_reference_energy<double>*
        sqrt_4_pi_fine_structure<double>/
        elementary_charge<double>/reduced_plank<double>/
        light_speed<double>/light_speed<double>;

    constexpr double mag_schwinger = schwinger_field<double>/light_speed<double>;

    constexpr auto B_SI = static_cast<RealType>(mag_schwinger);
    constexpr auto B_omega = static_cast<RealType>(
        mag_schwinger/ref_field_omega);
    constexpr auto B_lambda = static_cast<RealType>(mag_schwinger/
        ref_field_length);
    constexpr auto B_hl = static_cast<RealType>(mag_schwinger/
        ref_field_hl);

    constexpr auto all_B = val_pack<RealType>{B_SI, B_omega, B_lambda, B_hl};

    test_to_SI<RealType, quantity::B>(
        all_B, reference_omega, reference_length);
    test_from_to<RealType, quantity::B>(
        all_B, reference_omega, reference_length);
}

BOOST_AUTO_TEST_CASE( picsar_unit_conv_B_to_SI )
{
    test_case_B_to_SI<double>();
    test_case_B_to_SI<float>();
}


// simple Boost usage example for gdb

#include <iostream>
#include <boost/math/common_factor_rt.hpp>

int main()
{
    using namespace boost::math;
    int result = gcd_evaluator<int>()(50, 125);
    std::cout << "GCD of 50 and 125 is " << result << "\n";
}


#include "rand_state.cpp"
#include "entropy.cpp"
#include <armadillo>
#include <iostream>

using namespace std;
using namespace arma;

int main()
{
  int n_q = 10;
  int k = 3;
  for (int d=1; d<100; d++)
    {
      cx_dvec psi = scrambled_1d(n_q, d);
      cout << "depth=" << d << endl;
      cout << "von Neumann entropy=" << vNentropy(k, psi) << endl;
      cout << "diagonal entropy=" << diagentropy(k, psi) << endl;
    }

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <complex>
#include <cmath>

using Eigen::MatrixXcd;
using Eigen::VectorXd;
using Eigen::VectorXcd;

#define AU_L 5.2917721067e-11 // m
#define AU_T 2.41888432651e-17 // s
#define AU_E 4.35974465e-18 // J
#define EV 1.6021766208e-19 // J
#define AU2ANG (AU_L / 1e-10)
#define AU2EV (AU_E / EV)
#define PI 3.141592653589793238462643383279502884L

double simps(VectorXcd f, int size, double dx)
{
    double so = 0.0, se = 0.0;
    for (int i = 1; i < size - 2; i++)
    {
        if(i % 2 == 1)
        {
            so = so + ((double)real(f[i]));
        }
        else
        {
            se = se + ((double)real(f[i]));
        }
    }
    return dx / 3 * (((double)real(f[0])) + ((double)real(f[size-1])) + 4 * so + 2 * se);
}

int main()
{
    // constantes do problema
    const double E0 = 150.0; // eV
    const double delta_x = 1.0; // angstron
    const double x0 = -20.0; // angstron

    // otimizando
    double L = 100.0; // angstron
    int N = 256;
    double dt = 1e-19; // s

    double L_au = L / AU2ANG;
    double dt_au = dt / AU_T;
    double E0_au = E0 / AU2EV;
    double delta_x_au = delta_x / AU2ANG;
    double x0_au = x0 / AU2ANG;
    double k0_au = sqrt(2.0 * E0_au);

    double dx = L / ((double)(N-1));
    double dx_au = L_au / ((double)(N-1));
    VectorXd x_au(N), x_aux(N);
    for (int i = 0; i < N; i++)
    {
        x_au[i] = -L_au / 2.0 + dx_au * ((double)i);
    }

    // crank-nicolson
    MatrixXcd B(N,N);
    MatrixXcd C(N,N);
    MatrixXcd D(N,N);

    std::complex<double> alpha = - dt_au * (1i / (2.0 * dx_au * dx_au)) / 2.0;
    std::complex<double> beta = 1.0 - dt_au * (- 1i / (dx_au * dx_au)) / 2.0;
    std::complex<double> gamma = 1.0 + dt_au * (- 1i / (dx_au * dx_au)) / 2.0;
    for (int i = 0; i < N; i++)
    {
        if (i > 0)
        {
            B(i,i-1) = alpha;
            C(i,i-1) = -alpha;
        }
        if (i < N - 1) {

            B(i,i+1) = alpha;
            C(i,i+1) = -alpha;
        }
        B(i,i) = beta;
        C(i,i) = gamma;
    }
    D = B.inverse() * C;

    // pacote de onda
    double d2 = pow(delta_x_au, 2);
    double PN = 1.0 / pow(2.0 * PI * d2, 1.0 / 4.0);
    VectorXcd psi(N), psi_aux(N), psi_s(N);
    for (int i = 0; i < N; i++)
    {
        psi[i] = 1i * k0_au * x_au[i] - pow(x_au[i] - x0_au, 2) / (4.0 * d2);
        psi[i] = PN * exp(psi[i]);
    }
    psi_s = psi.conjugate();

    double A0 = simps(psi_s.cwiseProduct(psi), N, dx_au);
    double A = 0.0;
    double xm = x0_au, xm2, xm3;
    double var_norma = 0.0;
    int contador = 0;

    while (xm < -x0_au)
    {
        contador++;
        psi = D * psi;
        psi_s = psi.conjugate();
        A = simps(psi_s.cwiseProduct(psi), N, dx_au);
        xm = simps(psi_s.cwiseProduct(x_au.cwiseProduct(psi)), N, dx_au) / A;
        //std::cout << "x0=" << x0_au * AU2ANG << " & A/A0=" << (100.0 * A / A0) << " & X=" << xm * AU2ANG << " & C=" << contador << "\n";
    }

    var_norma = 100.0 * A / A0;
    x_aux = x_au.cwiseProduct(x_au);
    xm2 = simps(psi_s.cwiseProduct(x_aux.cwiseProduct(psi)), N, dx_au) / A;
    x_aux = x_au.cwiseProduct(x_aux);
    xm3 = simps(psi_s.cwiseProduct(x_aux.cwiseProduct(psi)), N, dx_au) / A;
    double desvpad = sqrt(abs(xm2 - pow(xm, 2)));
    double skewness = (xm3 - 3.0 * xm * pow(desvpad, 2) - pow(xm, 3)) / pow(desvpad, 3);

    std::cout << "L=" << L << " & N=" << N << " & dt=" << dt << " & A/A0=" << var_norma << " & X=" << xm * AU2ANG << " & S=" << desvpad * AU2ANG << " & G=" << skewness << " & C=" << contador << " & T=" << (((double)contador) * dt) << "\n";
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:	 NonDWASABIBayesCalibration
//- Description: Derived class for WASABI-based Bayesian inference
//- Owner:       Tim Wildey
//- Checked by:
//- Version:

#ifndef NOND_WASABI_BAYES_CALIBRATION_H
#define NOND_WASABI_BAYES_CALIBRATION_H

#include "NonDBayesCalibration.hpp"
#include "GaussianKDE.hpp"
// for uniform PDF
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/normal.hpp>
// for uniform samples (uniform_real is deprecated)
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>

namespace Dakota {

/// WASABI - Weighted Adaptive Surrogate Approximations for Bayesian Inference

/** This class performs Bayesian calibration using the WASABI approach
    */

class NonDWASABIBayesCalibration: public NonDBayesCalibration
{
public:

  //
  //- Heading: Constructors and destructor
  //

  /// standard constructor
  NonDWASABIBayesCalibration(ProblemDescDB& problem_db, Model& model);
  /// destructor
  ~NonDWASABIBayesCalibration();

  //
  //- Heading: Static callback functions required by WASABI
  //

  /// initializer for problem size characteristics in WASABI
  static void
  problem_size (int &chain_num, int &cr_num, int &gen_num, int &pair_num,
		int &par_num);

  /// Filename and data initializer for WASABI
  static void
  problem_value (std::string *chain_filename, std::string *gr_filename,
		 double &gr_threshold, int &jumpstep, double limits[],
		 int par_num, int &printstep, std::string *restart_read_filename,
		 std::string *restart_write_filename);

  /// Compute the prior density at specified point zp
  static double prior_density (int par_num, double zp[]);

  // NOTE: Memory is freed inside the dream core
  /// Sample the prior and return an array of parameter values
  //static double* prior_sample (int par_num);

  void prior_sample ( RealVector & sample);

  void compute_responses(RealMatrix & samples, RealMatrix & responses);

protected:

  //
  //- Heading: Virtual function redefinitions
  //

  /// redefined from DakotaNonD
  void quantify_uncertainty();
  // redefined from DakotaNonD
  void print_results(std::ostream& s);

  /// Extract a subset of samples for posterior eval according to the
  /// indices in points_to_keep
  void extract_selected_posterior_samples(const std::vector<int> &points_to_keep,
				   const RealMatrix &samples_for_posterior_eval,
				   const RealVector &posterior_density,
				   RealMatrix &posterior_data ) const;

  /// Export posterior_data to file
  void export_posterior_samples_to_file( const std::string filename,
					 const RealMatrix &posterior_data) const;

  //
  //- Heading: Data

  /// The mean of the multivariate Gaussian distribution of the obs. data
  RealVector dataDistMeans;
  /// The covariance of the multivariate Gaussian distribution of the obs. data
  RealVector dataDistCovariance;
  /// The filename of the file containing the data that with density estimator
  /// defines the distribution of the obs. data
  std::string dataDistFilename;
  /// The type of covariance data provided ("diagonal","matrix")
  std::string dataDistCovType;
  /// The filename of the import file containing samples at which the
  /// posterior will be evaluated
  std::string posteriorSamplesImportFile;
  /// Format of imported posterior samples file
  unsigned short posteriorSamplesImportFormat;
  /// The filename of the export file containing an arbitrary set of samples and
  /// their corresponding density values
  std::string exportPosteriorDensityFile;
  /// The filename of the export file containing samples from the posterior and
  /// their corresponding density values
  std::string exportPosteriorSamplesFile;
  /// Format of imported posterior samples and values file
  unsigned short exportFileFormat;
  /// Flag specifying whether to generate random samples from the posterior
  bool generateRandomPosteriorSamples;
  /// Flag specifying whether to evaluate the posterior density at a
  /// set of samples
  bool evaluatePosteriorDensity;

  /// lower bounds on calibrated parameters
  RealVector paramMins;
  /// upper bounds on calibrated parameters
  RealVector paramMaxs;

  /// uniform prior PDFs for each variable
  std::vector<boost::math::uniform> priorDistributions;
  /// random number engine for sampling the prior
  boost::mt19937 rnumGenerator;
  /// samplers for the uniform prior PDFs for each variable
  std::vector<boost::uniform_real<double> > priorSamplers;

private:

  //
  // - Heading: Data
  //

  /// Pointer to current class instance for use in static callback functions
  static NonDWASABIBayesCalibration* NonDWASABIInstance;

};

} // namespace Dakota

#endif


#include "TalibMeanReversion.h"
#include <profitview_util.h>
#include <SIOClient.h>
#include <Poco/Logger.h>
#include <Poco/JSON/Parser.h>
#include <boost/json.hpp>
#include <Poco/Any.h>
#include <ta_libc.h>
#include <numeric>
#include <string>
#include <vector>
#include <memory>
#include <ctime>

TalibMeanReversion::TalibMeanReversion(
	Exchange& exchange,
	int lookback,
	double reversion_level,
	int base_quantity)
: lookback_        {lookback       }
, reversion_level_ {reversion_level}
, base_quantity_   {base_quantity  }
, exchange_        {exchange       }
{
    TA_Initialize();
}

TalibMeanReversion::~TalibMeanReversion()
{
    TA_Shutdown();
}

template<typename Sequence>
double TalibMeanReversion::stdev(const Sequence& sequence) const
{
    std::vector<double> prices{ sequence.begin(), sequence.end()};

    // See: https://www.ta-lib.org/d_api/d_api.html#Output%20Size
    std::unique_ptr <TA_Real []> out{ new TA_Real[lookback_ - TA_STDDEV_Lookback(TA_INTEGER_DEFAULT, TA_REAL_DEFAULT)]};

    TA_Integer outBeg;
    TA_Integer outNbElement;
    auto code{ TA_STDDEV(0, lookback_ - 1, prices.data(), TA_INTEGER_DEFAULT, TA_REAL_DEFAULT, &outBeg, &outNbElement, &out[0])};
    return out[0];
}

void TalibMeanReversion::onTrade(const void *p, Array::Ptr &market_data)
{
	auto& logger{ Poco::Logger::get("example")};
	auto result{ market_data->getElement<std::string>(0)};

	Poco::JSON::Parser parser;
	Poco::Dynamic::Var result_json{ parser.parse(result)};

	auto result_object{ result_json.extract<Poco::JSON::Object::Ptr>()};
	auto price{ result_object->get("price").convert<double>()};

    auto symbol{ result_object->get("sym").toString()};

    profitview::util::log_trade(logger, result_object);

	time_t date_time{ result_object->get("time").convert<time_t>()};
	logger.information("Time: " + std::string{std::asctime(std::localtime(&date_time))});

    auto& [elements, prices] { counted_prices_[symbol]};

    prices.emplace_back(price);

    if(elements + 1 < lookback_) {
        ++elements; // Accumulate up to lookback_ prices
    } else {
        // These could be done on the fly but the complexity would distract
        auto mean { std::accumulate(prices.begin(), prices.end(), 0.0)/lookback_};
        double std_reversion { reversion_level_*stdev(prices)};

        prices.pop_front(); // Now we have lookback_ prices already, remove the oldest

		logger.information("Mean: " + std::to_string(mean));
		logger.information("Standard reversion: " + std::to_string(std_reversion));

        if(boost::json::value* headers{ result_.if_contains("headers")};
           result_.empty() || // Before the first trade, result_ will be empty and therefore...
           (headers &&        // ... headers will be nullptr
            // If there are headers to check, check that rate-limit hasn't been hit
            std::stol(headers->as_object()["x-ratelimit-reset"].as_string().c_str()) < std::time(nullptr)))
        {
            if(price > mean + std_reversion) { // Well greater than the normal volatility
                // so sell, expecting a reversion to the mean
                result_ = exchange_.new_order(symbol, Side::sell, base_quantity_, OrderType::market);
            }
            else if(price < mean - std_reversion) { // Well less than the normal volatility
                // so buy, expecting a reversion to the mean
                result_ = exchange_.new_order(symbol, Side::buy, base_quantity_, OrderType::market);
            }
        }
    }
}


#include "benchmark_functions.hpp"

#include <boost/python.hpp>
#include <cmath>
#include <numeric>

namespace optimization
{

namespace
{
template <typename Type>
std::vector<Type> list_to_vector(const boost::python::list &l)
{
    std::vector<Type> out(len(l));
    for (auto i = 0u; i < len(l); ++i)
        out.emplace_back(boost::python::extract<Type>(l[i]));

    return out;
}
}

double bent_cigar(const std::vector<double> &x)
{
    return std::pow(x[0], 2) +
           std::pow(10, 6) * std::accumulate(std::next(x.begin()), x.end(), 0., [](double acc, double el) {
               return acc + std::pow(el, 2);
           });
}

double bent_cigar_pywrapper(const boost::python::list &x)
{
    return bent_cigar(list_to_vector<double>(x));
}

double rosenbrock(const std::vector<double> &x)
{
    double result = 0;

    for (auto i = 0u; i < x.size() - 1; ++i)
        result += 100 * std::pow(std::pow(x[i], 2) - std::pow(x[i + 1], 2), 2) + std::pow(x[i] - 1, 2);

    return result;
}

double rosenbrock_pywrapper(const boost::python::list &x)
{
    return rosenbrock(list_to_vector<double>(x));
}

double rastrigin(const std::vector<double> &x)
{
    return std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
        return acc + std::pow(el, 2) - 10. * std::cos(2 * std::acos(-1) * el) + 10.;
    });
}

double rastrigin_pywrapper(const boost::python::list &x)
{
    return rastrigin(list_to_vector<double>(x));
}

double zakharov(const std::vector<double> &x)
{
    const double by_half = std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
        return el * 0.5;
    });

    return std::accumulate(x.begin(), x.end(), 0., [](double acc, double el) {
               return acc + std::pow(el, 2);
           }) +
           std::pow(by_half, 2.) + std::pow(by_half, 4.);
}

double zakharov_pywrapper(const boost::python::list &x)
{
    return zakharov(list_to_vector<double>(x));
}

double objective_function(function f, const std::vector<double> &x)
{
    switch (f)
    {
    case function::bent_cigar:
        return bent_cigar(x);
    case function::rosenbrock:
        return rosenbrock(x);
    case function::rastrigin:
        return rastrigin(x);
    case function::zakharov:
        return zakharov(x);
    }

    throw std::runtime_error("Wrong enum value for function");
}

} // optimization


/*!
  @author Shin'ichiro Nakaoka
*/

#include "LuaUtil.h"
#include "../EigenUtil.h"
#include <boost/format.hpp>

using namespace std;
using boost::format;
using namespace cnoid;

namespace cnoid {

void exportLuaEigenTypes(sol::table& module)
{
    module.new_usertype<Vector3>(
        "Vector3",
        "new", sol::factories(
            []() { return make_shared_aligned<Vector3>(); },
            [](double x, double y, double z) { return make_shared_aligned<Vector3>(x, y, z); }),
        sol::call_constructor, sol::factories(
            [](sol::table self){ return make_shared_aligned<Vector3>(); },
            [](sol::table self, double x, double y, double z){ return make_shared_aligned<Vector3>(x, y, z); }),
        sol::meta_function::index, [](Vector3& self, int index) { return self[index]; },
        sol::meta_function::new_index, [](Vector3& self, int index, double value) { self[index] = value; },
        sol::meta_function::unary_minus, [](Vector3& self) { return make_shared_aligned<Vector3>(-self); },
        sol::meta_function::addition, [](Vector3& self, Vector3& other) { return make_shared_aligned<Vector3>(self + other); },
        sol::meta_function::subtraction, [](Vector3& self, Vector3& other) { return make_shared_aligned<Vector3>(self - other); },
        sol::meta_function::multiplication, [](Vector3& self, Vector3& other) { return self.dot(other); },
        "dot", [](Vector3& self, Vector3& other) { return self.dot(other); },
        "cross", [](Vector3& self, Vector3& other) { return make_shared_aligned<Vector3>(self.cross(other)); },
        "toString", [](Vector3& self) { return (format("{ %1%, %2%, %3% }") % self[0] % self[1] % self[2]).str(); }
        );
}

}


// chebyshev.cpp: evaulation of Chebyshev polynomials for filter design
//
// Copyright (C) 2017-2021 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <hprblas>

// define a true 256-bit IEEE floating point type
constexpr size_t bits_in_octand = 113 + 128;
using cpp_bin_float_octand = boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<bits_in_octand, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::expression_template_option::et_off>;
// define the floating point types (single, double, quad, octand)
using sp = boost::multiprecision::cpp_bin_float_single;
using dp = boost::multiprecision::cpp_bin_float_double;
using qp = boost::multiprecision::cpp_bin_float_quad;
using op = cpp_bin_float_octand;

// generate the Chebyshev nodes in the interval (-1, 1)
template<typename Vector>
void chebyshev_nodes(Vector& args, Vector& v) {
	typedef typename Vector::value_type value_type;
	size_t nrOfNodes = size(args);
	if (size(v) != size(args)) {
		std::cerr << "chebyshev_nodes: vectors must be the same size\n";
		return;
	}
	value_type pi = value_type(3.14159265358979323846);  // TODO: this is limited to native type accuracy
	for (size_t k = 1; k <= nrOfNodes; ++k) {
		value_type numerator = (2 * k - 1) * pi;
		value_type denominator = 2 * nrOfNodes;
		value_type arg = numerator / denominator; // better would be to use cospi arguments, so we side step radian round-off
		args[k] =arg;
		v[k] = cos(arg);
	}
}

template<typename Vector>
void dumpPair(const Vector& args, const Vector& nodes) {
	for (size_t i = 0; i < size(args) && i < size(nodes); ++i) {
		std::cout << args[i] << " : " << nodes[i] << std::endl;
	}
}

template<typename Scalar>
std::ostream& operator<<(std::ostream& ostr, const std::vector<Scalar>& vec) {
	ostr << "[ ";
	for (auto v : vec) {
		ostr << v << " ";
	}
	ostr << "]";
	return ostr;
}

int main(int argc, char** argv)
try {
	using namespace sw::universal;

	using Real = double; // qp;
	mtl::vec::dense_vector<Real> args(10), nodes(10);
	chebyshev_nodes(args, nodes);
	std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
	dumpPair(args, nodes);

	mtl::vec::cos(args);

	std::cout << nodes << std::endl;

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}

/*
 BOOST implementation of operator<<() for cpp_bin_float backend

 class number {
 ...
   //
   // String conversion functions:
   //
   std::string str(std::streamsize digits = 0, std::ios_base::fmtflags f = std::ios_base::fmtflags(0))const
   {
	  return m_backend.str(digits, f);
   }
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
   ar & m_backend;
   }
...
}

template <class Backend, expression_template_option ExpressionTemplates>
inline std::ostream& operator << (std::ostream& os, const number<Backend, ExpressionTemplates>& r)
{
	std::streamsize d = os.precision();
	std::string s = r.str(d, os.flags());
	std::streamsize ss = os.width();
	if (ss > static_cast<std::streamsize>(s.size()))
	{
		char fill = os.fill();
		if ((os.flags() & std::ios_base::left) == std::ios_base::left)
			s.append(static_cast<std::string::size_type>(ss - s.size()), fill);
		else
			s.insert(static_cast<std::string::size_type>(0), static_cast<std::string::size_type>(ss - s.size()), fill);
	}
	return os << s;
}

 template <unsigned Digits, digit_base_type DigitBase, class Allocator, class Exponent, Exponent MinE, Exponent MaxE>
std::string cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::str(std::streamsize dig, std::ios_base::fmtflags f) const
{
   if(dig == 0)
	  dig = std::numeric_limits<number<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE> > >::max_digits10;

   bool scientific = (f & std::ios_base::scientific) == std::ios_base::scientific;
   bool fixed = !scientific && (f & std::ios_base::fixed);

   std::string s;

   if(exponent() <= cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::max_exponent)
   {
	  // How far to left-shift in order to demormalise the mantissa:
	  boost::intmax_t shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1;
	  boost::intmax_t digits_wanted = static_cast<int>(dig);
	  boost::intmax_t base10_exp = exponent() >= 0 ? static_cast<boost::intmax_t>(std::floor(0.30103 * exponent())) : static_cast<boost::intmax_t>(std::ceil(0.30103 * exponent()));
	  //
	  // For fixed formatting we want /dig/ digits after the decimal point,
	  // so if the exponent is zero, allowing for the one digit before the
	  // decimal point, we want 1 + dig digits etc.
	  //
	  if(fixed)
		 digits_wanted += 1 + base10_exp;
	  if(scientific)
		 digits_wanted += 1;
	  if(digits_wanted < -1)
	  {
		 // Fixed precision, no significant digits, and nothing to round!
		 s = "0";
		 if(sign())
			s.insert(static_cast<std::string::size_type>(0), 1, '-');
		 boost::multiprecision::detail::format_float_string(s, base10_exp, dig, f, true);
		 return s;
	  }
	  //
	  // power10 is the base10 exponent we need to multiply/divide by in order
	  // to convert our denormalised number to an integer with the right number of digits:
	  //
	  boost::intmax_t power10 = digits_wanted - base10_exp - 1;
	  //
	  // If we calculate 5^power10 rather than 10^power10 we need to move
	  // 2^power10 into /shift/
	  //
	  shift -= power10;
	  cpp_int i;
	  int roundup = 0; // 0=no rounding, 1=tie, 2=up
	  static const unsigned limb_bits = sizeof(limb_type) * CHAR_BIT;
	  //
	  // Set our working precision - this is heuristic based, we want
	  // a value as small as possible > cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count to avoid large computations
	  // and excessive memory usage, but we also want to avoid having to
	  // up the computation and start again at a higher precision.
	  // So we round cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count up to the nearest whole number of limbs, and add
	  // one limb for good measure.  This works very well for small exponents,
	  // but for larger exponents we add a few extra limbs to max_bits:
	  //
#ifdef BOOST_MP_STRESS_IO
	  boost::intmax_t max_bits = cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count + 32;
#else
	  boost::intmax_t max_bits = cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count + ((cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count % limb_bits) ? (limb_bits - cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count % limb_bits) : 0) + limb_bits;
	  if(power10)
		 max_bits += (msb(boost::multiprecision::detail::abs(power10)) / 8) * limb_bits;
#endif
	  do
	  {
		 boost::int64_t error = 0;
		 boost::intmax_t calc_exp = 0;
		 //
		 // Our integer result is: bits() * 2^-shift * 5^power10
		 //
		 i = bits();
		 if(shift < 0)
		 {
			if(power10 >= 0)
			{
			   // We go straight to the answer with all integer arithmetic,
			   // the result is always exact and never needs rounding:
			   BOOST_ASSERT(power10 <= (boost::intmax_t)INT_MAX);
			   i <<= -shift;
			   if(power10)
				  i *= pow(cpp_int(5), static_cast<unsigned>(power10));
			}
			else if(power10 < 0)
			{
			   cpp_int d;
			   calc_exp = boost::multiprecision::cpp_bf_io_detail::restricted_pow(d, cpp_int(5), -power10, max_bits, error);
			   shift += calc_exp;
			   BOOST_ASSERT(shift < 0); // Must still be true!
			   i <<= -shift;
			   cpp_int r;
			   divide_qr(i, d, i, r);
			   roundup = boost::multiprecision::cpp_bf_io_detail::get_round_mode(r, d, error, i);
			   if(roundup < 0)
			   {
#ifdef BOOST_MP_STRESS_IO
				  max_bits += 32;
#else
				  max_bits *= 2;
#endif
				  shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
				  continue;
			   }
			}
		 }
		 else
		 {
			//
			// Our integer is bits() * 2^-shift * 10^power10
			//
			if(power10 > 0)
			{
			   if(power10)
			   {
				  cpp_int t;
				  calc_exp = boost::multiprecision::cpp_bf_io_detail::restricted_pow(t, cpp_int(5), power10, max_bits, error);
				  calc_exp += boost::multiprecision::cpp_bf_io_detail::restricted_multiply(i, i, t, max_bits, error);
				  shift -= calc_exp;
			   }
			   if((shift < 0) || ((shift == 0) && error))
			   {
				  // We only get here if we were asked for a crazy number of decimal digits -
				  // more than are present in a 2^max_bits number.
#ifdef BOOST_MP_STRESS_IO
				  max_bits += 32;
#else
				  max_bits *= 2;
#endif
				  shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
				  continue;
			   }
			   if(shift)
			   {
				  roundup = boost::multiprecision::cpp_bf_io_detail::get_round_mode(i, shift - 1, error);
				  if(roundup < 0)
				  {
#ifdef BOOST_MP_STRESS_IO
					 max_bits += 32;
#else
					 max_bits *= 2;
#endif
					 shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
					 continue;
				  }
				  i >>= shift;
			   }
			}
			else
			{
			   // We're right shifting, *and* dividing by 5^-power10,
			   // so 5^-power10 can never be that large or we'd simply
			   // get zero as a result, and that case is already handled above:
			   cpp_int r;
			   BOOST_ASSERT(-power10 < INT_MAX);
			   cpp_int d = pow(cpp_int(5), static_cast<unsigned>(-power10));
			   d <<= shift;
			   divide_qr(i, d, i, r);
			   r <<= 1;
			   int c = r.compare(d);
			   roundup = c < 0 ? 0 : c == 0 ? 1 : 2;
			}
		 }
		 s = i.str(0, std::ios_base::fmtflags(0));
		 //
		 // Check if we got the right number of digits, this
		 // is really a test of whether we calculated the
		 // decimal exponent correctly:
		 //
		 boost::intmax_t digits_got = i ? static_cast<boost::intmax_t>(s.size()) : 0;
		 if(digits_got != digits_wanted)
		 {
			base10_exp += digits_got - digits_wanted;
			if(fixed)
			   digits_wanted = digits_got;  // strange but true.
			power10 = digits_wanted - base10_exp - 1;
			shift = (int)cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinE, MaxE>::bit_count - exponent() - 1 - power10;
			if(fixed)
			   break;
			roundup = 0;
		 }
		 else
			break;
	  }
	  while(true);
	  //
	  // Check whether we need to round up: note that we could equally round up
	  // the integer /i/ above, but since we need to perform the rounding *after*
	  // the conversion to a string and the digit count check, we might as well
	  // do it here:
	  //
	  if((roundup == 2) || ((roundup == 1) && ((s[s.size() - 1] - '0') & 1)))
	  {
		 boost::multiprecision::detail::round_string_up_at(s, static_cast<int>(s.size() - 1), base10_exp);
	  }

	  if(sign())
		 s.insert(static_cast<std::string::size_type>(0), 1, '-');

	  boost::multiprecision::detail::format_float_string(s, base10_exp, dig, f, false);
   }
   else
   {
	  switch(exponent())
	  {
	  case exponent_zero:
		 s = sign() ? "-0" : f & std::ios_base::showpos ? "+0" : "0";
		 boost::multiprecision::detail::format_float_string(s, 0, dig, f, true);
		 break;
	  case exponent_nan:
		 s = "nan";
		 break;
	  case exponent_infinity:
		 s = sign() ? "-inf" : f & std::ios_base::showpos ? "+inf" : "inf";
		 break;
	  }
   }
   return s;
}
 */