#define BOOST_TEST_MODULE "test_flexible_local_dihedral_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/FLP/FlexibleLocalDihedralPotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(FlexibleLocalDihedral_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k  = 1.0;
    const std::array<real_type, 7> term{{
        2.2056, 0.2183, -0.0795, 0.0451, -0.3169, 0.0165, -0.1375
    }};

    mjolnir::FlexibleLocalDihedralPotential<real_type> flpd(k, term);

    const real_type x_min = -pi;
    const real_type x_max =  pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = flpd.potential(x + h);
        const real_type pot2 = flpd.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = flpd.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(1e-5));
        BOOST_TEST(flpd.potential (x) == flpd.potential (x + 2 * pi),
                   boost::test_tools::tolerance(h));
        BOOST_TEST(flpd.derivative(x) == flpd.derivative(x + 2 * pi),
                   boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(FlexibleLocalDihedral_float)
{
    using real_type = float;
    constexpr std::size_t N   = 100;
    constexpr real_type   h   = 1e-3;
    constexpr real_type   tol = 5e-2;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k  = 1.0;
    const std::array<real_type, 7> term{{
        2.2056, 0.2183, -0.0795, 0.0451, -0.3169, 0.0165, -0.1375
    }};

    mjolnir::FlexibleLocalDihedralPotential<real_type> flpd(k, term);

    const real_type x_min = -pi;
    const real_type x_max =  pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type pot1 = flpd.potential(x + h);
        const real_type pot2 = flpd.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = flpd.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(tol));
        BOOST_TEST(flpd.potential (x) == flpd.potential (x + 2 * pi),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(flpd.derivative(x) == flpd.derivative(x + 2 * pi),
                   boost::test_tools::tolerance(tol));
    }
}


#include "transform.h"

#include <Eigen/Dense>
#include "math/rotation.h"
#include "utilities/error.h"

namespace cpt {

Transform::Transform() {
    reset();
}

void Transform::reset() {
    _scale.setOnes();
    _rotation.setIdentity();
    _translation.setZero();
}

void Transform::set_translation(const Eigen::Vector3f& trans) {
    _translation = trans;
}

void Transform::set_rotation(const Eigen::Matrix3f& rot) {
    // Check to make sure we have a valid rotation.
    CHECK_AND_THROW_ERROR(std::abs(rot.determinant() - 1.f) < 1e-6, "Invalid rotation matrix [Determinant:" <<  rot.determinant() << "].");
    _rotation = rot;
}

void Transform::set_scale(const Eigen::Vector3f& scale) {
    _scale = scale;
}

Eigen::Vector3f Transform::operator*(const Eigen::Vector3f& other) const {
    return (_rotation * _scale.cwiseProduct(other));
}

Eigen::Vector3f Transform::homogeneous_mult(const Eigen::Vector3f& other) const {
    return (*this * other + _translation);
}

Eigen::Vector3f Transform::rotate(const Eigen::Vector3f& other) const {
    return (_rotation * other);
}

Transform& Transform::operator*=(const Transform& other) {
    // Assume that we have
    // T_1 R_1 S_1 for one transform and T_2 R_2 S_2 for the other transform.
    // The new transform would thus have a translation of T_1 + R_1 S_1 T_2
    // Its rotation/scale would be a combined R_1 S_1 R_2 S_2. We can extract
    // the new scale by taking the magnitude of the colums and the rotation
    // matrix are just the normalized columns.
    _translation += (*this * other._translation);

    Eigen::Matrix3f newRotScaleMat = _rotation * _scale.asDiagonal() * other._rotation * other._scale.asDiagonal();
    decompose_scale_rotation(newRotScaleMat, _rotation, _scale);
    return *this;
}

Transform Transform::operator*(const Transform& other) const {
    Transform xform = *this;
    xform *= other;
    return xform;
}

Transform Transform::from_transform_matrix(const Eigen::Matrix4f& matrix) {
    Transform xform;
    xform.set_translation(matrix.block(0, 3, 3, 1));

    Eigen::Matrix3f rot;
    Eigen::Vector3f scale;
    decompose_scale_rotation(matrix.block(0,0,3,3), rot, scale);

    xform.set_scale(scale);
    xform.set_rotation(rot);
    return xform;
}

Transform Transform::inverse() const {
    Eigen::Matrix4f mat = to_matrix();
    Eigen::Matrix4f new_mat = mat;
    new_mat.block(0,0,3,3) = mat.block(0,0,3,3).inverse();
    new_mat.block(0,3,3,1) = -new_mat.block(0,0,3,3) * mat.block(0,3,3, 1);
    return Transform::from_transform_matrix(new_mat);
}

Eigen::Matrix4f Transform::to_matrix() const {
    Eigen::Matrix4f matrix;
    matrix.setIdentity();
    matrix.block(0,0,3,3) = _rotation * _scale.asDiagonal();
    matrix.block(0,3,3,1) = _translation;
    return matrix;
}

}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
/**
 * @file
 * @ingroup openOR_core
 */

#ifndef openOR_core_math_utilities_hpp
#define openOR_core_math_utilities_hpp

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

namespace openOR {

   namespace Math {

      /**
       * \brief square
       * @ingroup openOR_core
       */
      template<typename T>
      T square(const T& arg) { return arg * arg; }

      /**
       * \brief clamp
       * @ingroup openOR_core
       */
      template<typename T>
      T clamp(const T& arg, const T& min, const T& max) { return std::min<T>(max, std::max<T>(min, arg)); }

      /**
       * \brief rounds a double to the next integer value
       */
      inline double round(const double& d)
      {
         return floor(d + 0.5);
      }

      /**
       * \brief rounds a float to the next integer value
       */
      inline float round(const float& f)
      {
         return floorf(f + 0.5f);
      }

      /**
       * \brief rounds a long double to the next integer value
       */

      inline long double round(const long double& d)
      {
         return floorl(d + 0.5);
      }

      /**
       * \brief nextPowerOfTwo
       * @ingroup openOR_core
       */
      inline static unsigned int nextPowerOfTwo(unsigned int v) {
         --v;
         v |= v >> 1;
         v |= v >> 2;
         v |= v >> 4;
         v |= v >> 8;
         v |= v >> 16;
         ++v;
         return v;
      }

      /**
       * \brief nextPowerOfTwo
       * @ingroup openOR_core
       */
      inline static unsigned short nextPowerOfTwo(unsigned short v) {
         --v;
         v |= v >> 1;
         v |= v >> 2;
         v |= v >> 4;
         v |= v >> 8;
         ++v;
         return v;
      }

      /**
       * \brief isPowerOfTwo
       * @ingroup openOR_core
       */
      inline bool isPowerOfTwo(unsigned int n) { return ((n & (n - 1)) == 0); }

      /**
       * \brief isPowerOfTwo
       * @ingroup openOR_core
       */
      inline bool isPowerOfTwo(unsigned short n) { return ((n & (n - 1)) == 0); }

      /**
       * \brief Solver for linear function A*x=y
       */
      template<typename Type>
      boost::numeric::ublas::vector<Type> solve(const boost::numeric::ublas::matrix<Type>& A, const boost::numeric::ublas::vector<Type>& y)
      {
         //create a permutation matrix for the LU-factorization
         boost::numeric::ublas::matrix<Type> matA(A);
         boost::numeric::ublas::permutation_matrix<std::size_t> pm(matA.size1());
         int res = boost::numeric::ublas::lu_factorize(matA, pm);
         if (res == 0)
            return boost::numeric::ublas::vector<Type>();

         boost::numeric::ublas::vector<Type> vecX(y);
         boost::numeric::ublas::lu_substitute(matA, pm, vecX);
         return vecX;
      }

   }
}
#endif


//============================================================================//
//---------------- pnt_integrity/GeodeticConverter.cpp ---------*- C++ -*-----//
//============================================================================//
// BSD 3-Clause License
//
// Copyright (c) 2017, ETHZ ASL
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//----------------------------------------------------------------------------//
/// Third-party.  Downloaded from: https://github.com/ethz-asl/geodetic_utils //
//============================================================================//
#ifndef GEODETIC_CONVERTER_H_
#define GEODETIC_CONVERTER_H_

#include <Eigen/Dense>

namespace geodetic_converter
{
// Geodetic system parameters
/// \brief Equatorial radius (a), in meters
const double kSemimajorAxis = 6378137;
/// \brief Semi-minor radius (b), in meters
const double kSemiminorAxis = 6356752.3142;
/// \brief First eccentricity squared (e2), dimensionless
/// e2 = (a^2 - b^2) / a^2 = f * (2 - f)
const double kFirstEccentricitySquared = 6.69437999014 * 0.001;
/// \brief Second eccentricity squared (e'2), dimensionless
/// e'2 = (a^2 - b^2) / b^2 = e^2 / (1 - e^2) = e2 / (1 - e2)
const double kSecondEccentricitySquared = 6.73949674228 * 0.001;
/// \brief  flattening, dimensionless
const double kFlattening = 1 / 298.257223563;
/// \brief Pi (pi), dimensionless
const double PI = 3.14159265358979323846;

/// \brief Class to implement gedetic conversions for the pnt_integrity library
class GeodeticConverter
{
public:
  /// \brief Constructor for converter object
  ///
  /// Constructor initializes the reference flag to false.
  GeodeticConverter() { haveReference_ = false; }

  /// \brief Destructor for the converter object
  ~GeodeticConverter() {}

  // Default copy constructor and assignment operator are OK.

  /// \brief Returns the reference flag
  ///
  /// Returns a flag to indicate if the converter's reference position has
  /// been set.
  bool isInitialised() { return haveReference_; }

  /// \brief Returns the reference position
  ///
  /// Returns the reference position with the  latitude / longitude in radians
  /// and altitude in meters
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  void getReference(double* latitude, double* longitude, double* altitude)
  {
    *latitude  = initial_latitude_;
    *longitude = initial_longitude_;
    *altitude  = initial_altitude_;
  }

  /// \brief Sets the reference position
  ///
  /// Sets the reference to the provided position (LLA)
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  void initialiseReference(const double latitude,
                           const double longitude,
                           const double altitude)
  {
    // Save NED origin
    initial_latitude_  = latitude;
    initial_longitude_ = longitude;
    initial_altitude_  = altitude;

    // Compute ECEF of NED origin
    geodetic2Ecef(latitude,
                  longitude,
                  altitude,
                  &initial_ecef_x_,
                  &initial_ecef_y_,
                  &initial_ecef_z_);

    // Compute ECEF to NED and NED to ECEF matrices
    double phiP = atan2(
      initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2)));

    ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_);
    ned_to_ecef_matrix_ =
      nRe(initial_latitude_, initial_longitude_).transpose();

    haveReference_ = true;
  }

  /// \brief Converts the provided LLA to ECEF
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void geodetic2Ecef(const double latitude,
                     const double longitude,
                     const double altitude,
                     double*      x,
                     double*      y,
                     double*      z)
  {
    // Convert geodetic coordinates to ECEF.
    // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
    double lat_rad = latitude;
    double lon_rad = longitude;
    double xi =
      sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
    *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
    *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
    *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) *
         sin(lat_rad);
  }

  /// \brief Converts the provided ECEF to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ecef2Geodetic(const double x,
                     const double y,
                     const double z,
                     double*      latitude,
                     double*      longitude,
                     double*      altitude)
  {
    // Convert ECEF coordinates to geodetic coordinates.
    // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
    // to geodetic coordinates," IEEE Transactions on Aerospace and
    // Electronic Systems, vol. 30, pp. 957-961, 1994.

    double r = sqrt(x * x + y * y);
    double Esq =
      kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
    double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
    double G = r * r + (1 - kFirstEccentricitySquared) * z * z -
               kFirstEccentricitySquared * Esq;
    double C =
      (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) /
      pow(G, 3);
    double S = cbrt(1 + C + sqrt(C * C + 2 * C));
    double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
    double Q =
      sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
    double r_0 =
      -(P * kFirstEccentricitySquared * r) / (1 + Q) +
      sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) -
           P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) -
           0.5 * P * r * r);
    double U   = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
    double V   = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) +
                    (1 - kFirstEccentricitySquared) * z * z);
    double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
    *altitude =
      U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
    *latitude  = atan((z + kSecondEccentricitySquared * Z_0) / r);
    *longitude = atan2(y, x);
  }

  /// \brief Converts the provided ECEF to NED
  ///
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ecef2Ned(const double x,
                const double y,
                const double z,
                double*      north,
                double*      east,
                double*      down)
  {
    // Converts ECEF coordinate position into local-tangent-plane NED.
    // Coordinates relative to given ECEF coordinate frame.

    Eigen::Vector3d vect, ret;
    vect(0) = x - initial_ecef_x_;
    vect(1) = y - initial_ecef_y_;
    vect(2) = z - initial_ecef_z_;
    ret     = ecef_to_ned_matrix_ * vect;
    *north  = ret(0);
    *east   = ret(1);
    *down   = -ret(2);
  }

  /// \brief Converts the provided NED to ECEF
  ///
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ned2Ecef(const double north,
                const double east,
                const double down,
                double*      x,
                double*      y,
                double*      z)
  {
    // NED (north/east/down) to ECEF coordinates
    Eigen::Vector3d ned, ret;
    ned(0) = north;
    ned(1) = east;
    ned(2) = -down;
    ret    = ned_to_ecef_matrix_ * ned;
    *x     = ret(0) + initial_ecef_x_;
    *y     = ret(1) + initial_ecef_y_;
    *z     = ret(2) + initial_ecef_z_;
  }

  /// \brief Converts the provided LLA to NED
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  void geodetic2Ned(const double latitude,
                    const double longitude,
                    const double altitude,
                    double*      north,
                    double*      east,
                    double*      down)
  {
    // Geodetic position to local NED frame
    double x, y, z;
    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
    ecef2Ned(x, y, z, north, east, down);
  }

  /// \brief Converts the provided NED to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  void ned2Geodetic(const double north,
                    const double east,
                    const double down,
                    double*      latitude,
                    double*      longitude,
                    double*      altitude)
  {
    // Local NED position to geodetic coordinates
    double x, y, z;
    ned2Ecef(north, east, down, &x, &y, &z);
    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
  }

  /// \brief Converts the provided LLA to ENU
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east ENU east in meters
  /// \param north ENU north in meters
  /// \param up ENU up in meters
  void geodetic2Enu(const double latitude,
                    const double longitude,
                    const double altitude,
                    double*      east,
                    double*      north,
                    double*      up)
  {
    // Geodetic position to local ENU frame
    double x, y, z;
    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);

    double aux_north, aux_east, aux_down;
    ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);

    *east  = aux_east;
    *north = aux_north;
    *up    = -aux_down;
  }

  /// \brief Converts the provided ENU to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east ENU east in meters
  /// \param north ENU north in meters
  /// \param up ENU up in meters
  void enu2Geodetic(const double east,
                    const double north,
                    const double up,
                    double*      latitude,
                    double*      longitude,
                    double*      altitude)
  {
    // Local ENU position to geodetic coordinates

    const double aux_north = north;
    const double aux_east  = east;
    const double aux_down  = -up;
    double       x, y, z;
    ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
  }

private:
  inline Eigen::Matrix3d nRe(const double lat_radians, const double lon_radians)
  {
    const double sLat = sin(lat_radians);
    const double sLon = sin(lon_radians);
    const double cLat = cos(lat_radians);
    const double cLon = cos(lon_radians);

    Eigen::Matrix3d ret;
    ret(0, 0) = -sLat * cLon;
    ret(0, 1) = -sLat * sLon;
    ret(0, 2) = cLat;
    ret(1, 0) = -sLon;
    ret(1, 1) = cLon;
    ret(1, 2) = 0.0;
    ret(2, 0) = cLat * cLon;
    ret(2, 1) = cLat * sLon;
    ret(2, 2) = sLat;

    return ret;
  }

  inline double rad2Deg(const double radians) { return (radians / PI) * 180.0; }

  inline double deg2Rad(const double degrees) { return (degrees / 180.0) * PI; }

  double initial_latitude_;
  double initial_longitude_;
  double initial_altitude_;

  double initial_ecef_x_;
  double initial_ecef_y_;
  double initial_ecef_z_;

  Eigen::Matrix3d ecef_to_ned_matrix_;
  Eigen::Matrix3d ned_to_ecef_matrix_;

  bool haveReference_;

};  // class GeodeticConverter
}  // namespace geodetic_converter

#endif  // GEODETIC_CONVERTER_H_


#include "matrix.hpp"
#include "generator_options.hpp"
#include <ctime>
#include <cstdlib>
#include <cassert>
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

template<typename MatrixType>
void populate(MatrixType& matrix, const std::size_t bandwidth, const double density, const bool symmetric)
{
  const double weight = 1e15;
  assert(matrix.size1() == matrix.size2());

  // Place random values with appropriate bandwidth
  for(std::size_t row=0; row<matrix.size1(); ++row)
  {
    for(std::size_t col=std::max(static_cast<std::size_t>(0), row-bandwidth); col<std::min(matrix.size2(), row+bandwidth+1); ++col)
    {
      if (drand48 () < density)
      {
        const double value = drand48() * weight;
        matrix(row, col) += value;

        if (symmetric)
          matrix(col, row) += value;
      }
    }
  }

  // Make diagonally dominant
  for (typename MatrixType::iterator1 rowIter = matrix.begin1(); rowIter != matrix.end1(); ++rowIter)
  {
    const typename MatrixType::value_type weight = 1e1;  // arbitrary value to increase the sum by to make strictly dominant
    typename MatrixType::value_type absSum;
    for(typename MatrixType::iterator2 colIter(rowIter.begin()); colIter != rowIter.end(); ++colIter)
    {
      if (colIter.index1() != colIter.index2())
        absSum += std::fabs(*colIter);
    }
    matrix(rowIter.index1(), rowIter.index1()) = absSum + weight;
  }
}

int main(int argc, char* argv[])
{
  GeneratorOptions options;
  options.processOptions(argc, argv);
  srand48(time(NULL));

  const unsigned size = options.getSize();
  const bool symmetric = options.getSymmetric();

  boost::numeric::ublas::mapped_matrix<double> matrix(size, size);

  populate(matrix, size/4, 4.0/size, symmetric);

  CCSMatrix<double> csrMatrix(matrix, symmetric);
  std::cout << "Generating " << (symmetric ? "a symmetric" : "an unsymmetric") << " matrix of size " << size << "." << std::endl;
  std::cout << "Number of non-zeros in generated matrix: " << csrMatrix.nnz() << "." << std::endl;

  csrMatrix.writeToFile(options.getOutputFile());
}


#ifndef BURKARDT_NON_HPP
#define BURKARDT_NON_HPP

/*
 * nonlinear equation example set from
 *  http://people.sc.fsu.edu/~jburkardt/f_src/test_nonlin/test_nonlin.html
 *  http://people.sc.fsu.edu/~jburkardt/f_src/test_nonlin/test_nonlin.f90
 */

#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

struct GenRosen {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i;

		y(0) = 1. - x(0);
		for (i=1; i<s; i++) {
			y(i) = 10. * (x(i) - x(i-1) * x(i-1));
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Powell {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);

		y(0) = x(0) + 10. * x(1);
		y(1) = sqrt(5.) * (x(2) - x(3));
		y(2) = (x(1) - 2. * x(2)) * (x(1) - 2. * x(2));
		y(3) = sqrt(10.) * (x(0) - x(3)) * (x(0) - x(3));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Wood {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);
		T tmp1, tmp2;

		tmp1 = x(1) - x(0) * x(0);
		tmp2 = x(3) - x(2) * x(2);

		y(0) = -200. * x(0) * tmp1 - (1. - x(0));
		y(1) = 200. * tmp1 + 20.2 * (x(1) - 1.) + 19.8 * (x(3) - 1.);
		y(2) = -180. * x(2) * tmp2 - (1. - x(2));
		y(3) = 180. * tmp2 + 20.2 * (x(3) - 1.) + 19.8 * (x(1) - 1.);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Watson {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j, k;
		T sum1, sum2, tmp, ti;

		for (i=0; i<s; i++) {
			y(i) = 0;
		}

		for (i=1; i<=29; i++) {
			ti = i / 29.;
			sum1 = 0.;
			tmp = 1.;
			for (j=1; j<s; j++) {
				sum1 += (T)j * tmp * x(j);
				tmp *= ti;
			}
			sum2 = 0;
			tmp = 1.;
			for (j=0; j<s; j++) {
				sum2 += tmp * x(j);
				tmp *= ti;
			}
			tmp = (sum1 - sum2 * sum2 - 1.) / ti;
			for (k=0; k<s; k++) {
				y(k) += tmp * ((T)k - 2. * ti * sum2);
				tmp *= ti;
			}
		}

		y(0) += 3. * x(0) - 2. * x(0) + x(1) + 2. * x(0)*x(0)*x(0);
		y(1) += x(1) - x(0) * x(0) - 1.;

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Chebyquad {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j;
		T t1, t2, t3;

		for (i=0; i<s; i++) {
			y(i) = 0.;
		}

		for (j=0; j<s; j++) {
			t1 = 1.;
			t2 = x(j);
			for (i=0; i<s; i++) {
				y(i) += t2;
				t3 = 2. * x(j) * t2 - t1;
				t1 = t2;
				t2 = t3;
			}
		}

		for (i=0; i<s; i++) {
			y(i) /= (T)s;
		}

		for (i=0; i<s; i++) {
			if ( (i+1) % 2 == 0) {
				y(i) += 1. / (T)((i+1)*(i+1)-1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Brown {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j;
		T sum, prod;

		sum = 0.;
		for (i=0; i<s; i++) {
			sum += x(i);
		}

		for (i=0; i<s-1; i++) {
			y(i) = x(i) + sum - (T)(s+1);
		}

		prod = 1.;
		for (i=0; i<s; i++) {
			prod *= x(i);
		}

		y(s-1) = prod - 1.;

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct DBVP {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k;
		T h, tmp;

		h = 1. / (s + 1);

		for (k=0; k<s; k++) {
			tmp = x(k) + (T)(k+1) * h + 1.;
			y(k) = 2. * x(k) + 0.5 * h * h * tmp*tmp*tmp;
			if (k > 0) {
				y(k) -= x(k-1);
			}
			if (k < s-1) {
				y(k) -= x(k+1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct DIntEq {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int j, k;
		T h, tk, tj, sum1, sum2, tmp;

		h = 1. / (s + 1);

		for (k=0; k<s; k++) {
			tk = (k+1.) / (s+1.);
			sum1 = 0.;
			for (j=0; j<k+1; j++) {
				tj = (j+1.) * h;
				tmp = x(j) + tj + 1.;
				sum1 += tj * tmp*tmp*tmp;
			}
			sum2 = 0.;
			for (j=k+1; j<s; j++) {
				tj = (j+1.) * h;
				tmp = x(j) + tj + 1.;
				sum2 += (1. - tj) * tmp*tmp*tmp;
			}
			y(k) = x(k) + h * ( (1. - tk) * sum1 + tk * sum2) / 2.;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct VDim {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int j;
		T sum1, tmp;

		sum1 = 0.;

		for (j=0; j<s; j++) {
			sum1 += (j+1.) * (x(j) - 1.);
		}

		tmp = sum1 * (1. + 2. * sum1 * sum1);

		for (j=0; j<s; j++) {
			y(j) = x(j) - 1. + (j+1.) * tmp;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Broyden {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k;

		for (k=0; k<s; k++) {
			y(k) = (3. - 2. * x(k)) * x(k) + 1.;
			if (k > 0) {
				y(k) -= x(k-1);
			}
			if (k < s-1) {
				y(k) -= 2. * x(k+1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct BroydenBand {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k, k1, k2, j;
		T tmp;

		for (k=0; k<s; k++) {
			k1 = k - 5;
			if (k1 < 0) k1 = 0;
			k2 = k + 1;
			if (k2 > s-1) k2 = s-1;

			tmp = 0.;
			for (j=k1; j<=k2; j++) {
				if (j != k) {
					tmp += x(j) * (1. + x(j));
				}
			}

			y(k) = x(k) + (2. + 5. * x(k)*x(k)) + 1. - tmp;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Hammarling2x2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);

		y(0) = (x(0) * x(0) + x(1) * x(2)) - 0.0001;
		y(1) = (x(0) * x(1) + x(1) * x(3)) - 1.;
		y(2) = (x(2) * x(0) + x(3) * x(2)) - 0.;
		y(3) = (x(2) * x(1) + x(3) * x(3)) - 0.0001;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-100., 100.);
		}
	}
};

struct Hammarling3x3 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(9);

		y(0) = (x(0) * x(0) + x(1) * x(3) + x(2) * x(6)) - 0.0001;
		y(1) = (x(0) * x(1) + x(1) * x(4) + x(2) * x(7)) - 1.;
		y(2) = (x(0) * x(2) + x(1) * x(5) + x(2) * x(8));

		y(3) = (x(3) * x(0) + x(4) * x(3) + x(5) * x(6));
		y(4) = (x(3) * x(1) + x(4) * x(4) + x(5) * x(7)) - 0.0001;
		y(5) = (x(3) * x(2) + x(4) * x(5) + x(5) * x(8));

		y(6) = (x(6) * x(0) + x(7) * x(3) + x(8) * x(6));
		y(7) = (x(6) * x(1) + x(7) * x(4) + x(8) * x(7));
		y(8) = (x(6) * x(2) + x(7) * x(5) + x(8) * x(8)) - 0.0001;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(9);
		for (i=0; i<9; i++) {
			x(i).assign(-100., 100.);
		}
	}
};

struct P17 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) + x(1) - 3.;
		y(1) = x(0) * x(0) + x(1) * x(1) - 9.;;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct P19 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * (x(0) * x(0) + x(1) * x(1));
		y(1) = x(1) * (x(0) * x(0) + x(1) * x(1));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct P20 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(1);

		y(0) = x(0) * (x(0) - 5.) * (x(0) - 5.);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(1);
		for (i=0; i<1; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Chandrasekhar {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);

		ub::vector<T> mu(s);
		T sum, term;
		int i, j;

		for (i=0; i<s; i++) {
			y(i) = x(i);
		}
		for (i=0; i<s; i++) {
			mu(i) = (2.*(i+1.)-1.) / (2. * s);
		}

		for (i=0; i<s; i++) {
			sum = 0.;
			for (j=0; j<s; j++) {
				sum += mu(i) * x(j) / (mu(i) + mu(j));
			}
			term = 1. - 0.9 * sum / (2. * s);
			y(i) -= 1. / term;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-3., 3.);
		}
	}
};

#endif // BURKARDT_NON_HPP


/**
 * @file symplectictimesteppingwaves_test.cc
 * @brief NPDE homework "SymplecticTimesteppingWaves" code
 * @author Amélie Loher
 * @date 09.04.2020
 * @copyright Developed at ETH Zurich
 */

#include "../symplectictimesteppingwaves.h"
#include "../symplectictimesteppingwaves_assemble.h"
#include "../symplectictimesteppingwaves_ode.h"

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <Eigen/SparseLU>

#include <utility>

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/uscalfe/uscalfe.h>

namespace SymplecticTimesteppingWaves::test {

TEST(SymplecticTimesteppingWaves, sympTimesteppingHarmonicOscillatorODE) {
  unsigned int m = 10;

  Eigen::Vector2d sol = sympTimesteppingHarmonicOscillatorODE(m);

  Eigen::Vector2d ref_sol;
  ref_sol << 0.000675997, 1;

  double tol = 1.0e-4;
  ASSERT_NEAR((sol - ref_sol).lpNorm<Eigen::Infinity>(), 0.0, tol);
}

TEST(SymplecticTimesteppingWaves, assembleGalerkinMatrix) {
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../../meshes/simple.msh");
  auto mesh_p = reader.mesh();

  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto one = [](Eigen::Vector2d x) -> double { return 1.0; };

  Eigen::SparseMatrix<double> galMat =
      assembleGalerkinMatrix(fe_space, one, one, one);

  Eigen::MatrixXd sol(galMat);

  Eigen::MatrixXd ref_sol(5, 5);
  ref_sol << 1.75, 0.1875, 0, 0.1875, -0.958333, 0.1875, 1.75, 0.1875, 0,
      -0.958333, 0, 0.1875, 1.75, 0.1875, -0.958333, 0.1875, 0, 0.1875, 1.75,
      -0.958333, -0.958333, -0.958333, -0.958333, -0.958333, 4.16667;

  double tol = 1.0e-4;
  ASSERT_NEAR((sol - ref_sol).lpNorm<Eigen::Infinity>(), 0.0, tol);
}

TEST(SymplecticTimesteppingWaves, computeEnergies) {
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../../meshes/simple.msh");
  auto mesh_p = reader.mesh();

  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto c = [](Eigen::Vector2d x) -> double { return 1.0 + x.dot(x); };

  SympTimestepWaveEq stepper(fe_space, c);

  Eigen::VectorXd p = Eigen::VectorXd::Ones(5);
  Eigen::VectorXd q = 2 * Eigen::VectorXd::Ones(5);

  double sol = stepper.computeEnergies(p, q);

  double ref_sol = 3.83333;

  double tol = 1.0e-4;
  ASSERT_NEAR((sol - ref_sol), 0.0, tol);
}

TEST(SymplecticTimesteppingWaves, solvewave) {
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  const lf::io::GmshReader reader(
      std::move(mesh_factory), CURRENT_SOURCE_DIR "/../../meshes/simple.msh");
  auto mesh_p = reader.mesh();

  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  auto c = [](Eigen::Vector2d x) -> double { return 1.0 + x.dot(x); };

  Eigen::VectorXd u0 = Eigen::VectorXd::Ones(5);
  Eigen::VectorXd v0 = 2 * Eigen::VectorXd::Ones(5);

  double T = 1.0;
  unsigned int m = 10;

  std::pair<Eigen::VectorXd, Eigen::VectorXd> sol =
      solvewave(fe_space, c, u0, v0, T, m);

  Eigen::VectorXd ref_sol_first(5);
  ref_sol_first << 2.01346, 1.76133, 1.53227, 1.76133, 1.77866;

  Eigen::VectorXd ref_sol_second(11);
  ref_sol_second << 2.83333, 2.83333, 2.83334, 2.83336, 2.83338, 2.8334,
      2.83342, 2.83346, 2.8335, 2.83355, 2.83359;

  double tol = 1.0e-4;
  ASSERT_NEAR((sol.first - ref_sol_first).lpNorm<Eigen::Infinity>(), 0.0, tol);
  ASSERT_NEAR((sol.second - ref_sol_second).lpNorm<Eigen::Infinity>(), 0.0,
              tol);
}

} /* namespace SymplecticTimesteppingWaves::test */


#include <Eigen/Core>
#include <iostream>
#include <ridgelet/construction/ridgelet_functions.hpp>

using namespace std;

int main(int argc, char *argv[])
{
  unsigned int N = 100;
  Eigen::VectorXd vec(N);
  Eigen::VectorXd ty(N);

  vec.setLinSpaced(N, -2, 2);

  TransitionFunction t;

  std::transform(vec.data(), vec.data() + vec.size(), ty.data(), t);
  cout << "Transfer function:"
       << "\n";
  cout << "x: " << vec.array().transpose() << endl;
  cout << "y: " << ty.array().transpose() << endl;

  PsiSpherical1<> psi_spherical;

  Eigen::VectorXd ws(N);
  std::transform(vec.data(), vec.data() + vec.size(), ws.data(), psi_spherical);

  cout << "Spherical: "
       << "\n";
  cout << "y" << ws.array().transpose() << "\n";

  return 0;
}


#include "soar.hpp"

#include <Eigen/Dense>
#include <fmt/format.h>
#include <vector>

using namespace Eigen;

Soar::Soar(const Ref<const MatrixXd> &matA, const Ref<const MatrixXd> &matB)
    : ndim_(matA.rows()), matA_(matA), matB_(matB),
      u_(VectorXd::Random(ndim_)) {}

MatrixXd Soar::compute(int n) {
  VectorXd q = u_ / u_.norm();
  VectorXd f = VectorXd::Zero(ndim_);

  // initialize
  MatrixXcd matQ = MatrixXcd::Zero(ndim_, n);
  MatrixXcd matP = MatrixXcd::Zero(ndim_, n);
  MatrixXcd matT = MatrixXcd::Zero(n, n);
  std::vector<int> deflation;

  matQ.col(0) = q;

  for (int i = 0; i < n - 1; ++i) {
    // Recurrence role
    VectorXcd r = matA_ * matQ.col(i) + matB_ * matP.col(i);
    std::complex<double> norm_init = r.norm();
    MatrixXcd basis = matQ.leftCols(i + 1);

    // Modified Gram Schmidt procedure
    // First orthogonalization
    VectorXcd coef = VectorXd::Zero(i + 1);
    for (int j = 0; j < i + 1; ++j) {
      // Projection coeficients and projection subtraction
      VectorXcd v = basis.col(j);
      coef(j) = v.dot(r);
      r -= coef(j) * v;
    }
    // Saving coeficients
    matT.col(i).head(i + 1) = coef;

    // Reorthogonalization, if needed.
    if (r.norm() < 0.7 * norm_init) {
      // Second Gram Schmidt orthogonalization
      for (int j = 0; j < i + 1; ++j) {
        VectorXd v = basis.col(j);
        coef(j) = v.dot(r);
        r -= coef(j) * v;
      }
      matT.col(i).head(i + 1) += coef;
    }

    double r_norm = r.norm();
    matT(i + 1, i) = r_norm;

    // check for breakdown
    if (r_norm > tol_) {
      matQ.col(i + 1) = r / r_norm;
      VectorXd e_i = VectorXd::Zero(i + 1);
      e_i(i) = 1.0;
      // VectorXd v_aux = matT.block(1, 0, i + 1, i + 1).ldlt().solve(e_i);
      VectorXd v_aux =
          matT.block(1, 0, i + 1, i + 1).colPivHouseholderQr().solve(e_i);
      f = matQ.leftCols(i + 1) * v_aux;
    } else {
      // Deflation reset
      matT(i + 1, i) = 1.0;
      matQ.col(i + 1) = VectorXd::Zero(ndim_);
      VectorXd e_i = VectorXd::Zero(i + 1);
      e_i(i) = 1.0;
      // VectorXd v_aux = matT.block(1, 0, i + 1, i + 1).ldlt().solve(e_i);
      VectorXd v_aux =
          matT.block(1, 0, i + 1, i + 1).colPivHouseholderQr().solve(e_i);
      f = matQ.leftCols(i + 1) * v_aux;

      // Deflation verification
      VectorXd f_proj;
      for (int k : deflation) {
        VectorXd p = matP.col(k);
        double coef_f = p.dot(f) / p.dot(p);
        f_proj = f - coef_f * p;
      }

      if (f_proj.norm() > tol_) {
        deflation.push_back(i);
      } else {
        fmt::print("SOAR lucky breakdown.\n");
        break;
      }
    }
    matP.col(i + 1) = f;
  }

  fmt::print("zero1: {:9.3f}\n",
             (matQ.transpose() * matQ - MatrixXd::Identity(n, n)).norm());

  VectorXd e_n = VectorXd::Zero(n - 1);
  e_n(n - 2) = 1.0;
  VectorXd r = matA_ * matQ.col(n - 2) + matB_ * matP.col(n - 2);
  for (int i = 0; i < n - 1; ++i) {
    double coef = matQ.col(i).dot(r);
    r -= coef * matQ.col(i);
  }
  double nm = (matA_ * matQ.leftCols(n - 1) + matB_ * matP.leftCols(n - 1) -
               matQ.leftCols(n - 1) * matT.topLeftCorner(n - 1, n - 1) -
               r * e_n.transpose())
                  .norm();
  fmt::print("zero2: {:9.3f}\n", nm);
  return matQ;
}


#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class Network {
  public:
    Network(vector<int> &sizes, vector<VectorXd> &training_inputs, vector<VectorXd> &training_outputs,
       vector<VectorXd> &test_inputs, vector<int> &test_labels,
       int mini_batch_size, double learning_rate);
    void feedforward(VectorXd &a);
    void backpropagate(VectorXd &y);
    void stochastic_gradient_descent(int epochs);
    double evaluate();
    int save_weights_and_biases(string filepath);
  private:
    void update_mini_batch(vector<int> &indices, int batch_num); /* helper for ``stochastic_gradient_descent``. */
    VectorXd sigmoid_prime(int &layer_num); /* helper for ``backpropogate``. */
    VectorXd relu_prime(int &layer_num); /* helper for ``backpropogate``. */
    VectorXd cost_derivative(VectorXd &y); /* helper for ``backpropogate``. */
    int num_layers_; /* Number of layers in the network (including input and output layers). */
    vector<int> sizes_; /* Vector containing the sizes of each layer. ``sizes_.size() == num_layers``. */
    vector<VectorXd> biases_; /* Bias vectors for each layer.
                               * 0th index contains a dummy vector since there is no bias for the input layer. */
    vector<MatrixXd> weights_; /* Weight vectors for each layer except the input.
                                * 0th index contains a dummy matrix since there is no weight for the input layer. */
    vector<VectorXd> as_; /* Stores the activations of each layer to avoid repeated computation. */
    vector<VectorXd> zs_; /* Stores the z-vectors of each layer to avoid repeated computation. */
    vector<VectorXd> nabla_b_; /* Stores the derivatives of the cost function with respect to the baises. */
    vector<MatrixXd> nabla_w_; /* Stores the derivatives of the cost function with respect to the weights. */
    vector<VectorXd> training_inputs_;
    vector<VectorXd> training_outputs_;
    vector<VectorXd> test_inputs_;
    vector<int> test_labels_;
    int mini_batch_size_;
    double learning_rate_;
};


#include <math_lib/plane.h>
#include <math_lib/projection.h>

#include <boost/qvm/vec_operations.hpp>

#include <gtest/gtest.h>

using namespace pagoda;

TEST(Plane, when_using_default_constructor_should_construct_the_xy_plane)
{
	Plane<float> p;
	EXPECT_EQ(p.GetDistanceToOrigin(), 0.0f);
	EXPECT_TRUE(p.GetNormal() == (Vec3F{0, 0, 1}));
}

TEST(Plane, when_constructing_with_normal_and_distance_should_set_the_normal_and_distance_values)
{
	Plane<float> p(Vec3F{1, 2, 3}, 4);
	EXPECT_EQ(p.GetDistanceToOrigin(), 4.0f);
	EXPECT_TRUE(p.GetNormal() == (Vec3F{1, 2, 3}));
}

TEST(Plane, when_constructing_from_point_and_normal_should_construct_the_plane_correctly)
{
	auto p = Plane<float>::FromPointAndNormal(Vec3F{1, 0, 0}, Vec3F{0, 1, 0});
	EXPECT_EQ(p.GetDistanceToOrigin(), 0.0f);
	EXPECT_TRUE(p.GetNormal() == (Vec3F{0, 1, 0}));
}

TEST(Plane, when_constructing_from_three_points_should_construct_the_plane_correctly)
{
	auto p = Plane<float>::FromPoints(Vec3F{1, 0.5, 0}, Vec3F{0, 0.5, 0}, Vec3F{0, 0.5, 1});
	EXPECT_EQ(p.GetDistanceToOrigin(), 0.5f);
	EXPECT_TRUE(p.GetNormal() == (Vec3F{0, 1, 0}));
}

TEST(Plane, when_constructing_from_a_point_and_two_vectors_should_construct_the_plane_correctly)
{
	auto p = Plane<float>::FromPointAndVectors(Vec3F{1, 1, 1}, Vec3F{1, 0, 0}, Vec3F{0, 1, 0});
	EXPECT_EQ(p.GetDistanceToOrigin(), 1.0f);
	EXPECT_TRUE(p.GetNormal() == (Vec3F{0, 0, 1}));
}

TEST(Plane, when_getting_vectors_contained_in_the_plane_should_get_vectors_orthogonal_to_the_normal)
{
	Plane<float> p(Vec3F{1, 2, 3}, 4);
	EXPECT_EQ(boost::qvm::dot(p.GetVector(), (Vec3F{1, 2, 3})), 0.0f);
	EXPECT_EQ(boost::qvm::dot(p.GetVector2(), (Vec3F{1, 2, 3})), 0.0f);
}

TEST(Plane, when_getting_points_contained_in_the_plane_should_have_zero_distance_to_the_plane)
{
	Plane<float> plane(Vec3F{0, 0, 1}, 4);
	auto p1 = plane.GetPoint();
	auto p2 = plane.GetPoint2();

	EXPECT_TRUE(projection(p1, plane) == p1);
	EXPECT_TRUE(projection(p2, plane) == p2);
}

TEST(Plane, when_a_getting_the_side_of_the_plane_a_point_is_in_should_return_the_right_value)
{
	Plane<float> plane = Plane<float>::FromPointAndNormal(Vec3F{0, 0, 0}, Vec3F{1, 0, 0});

	EXPECT_EQ(plane.GetPlaneSide((Vec3F{1, 0, 0})), Plane<float>::PlaneSide::Front);
	EXPECT_EQ(plane.GetPlaneSide((Vec3F{0, 0, 0})), Plane<float>::PlaneSide::Contained);
	EXPECT_EQ(plane.GetPlaneSide((Vec3F{-1, 0, 0})), Plane<float>::PlaneSide::Back);
}


#include <boost/test/unit_test.hpp>
#include <cmath>
#include <gmp.h>
#include <limits>
#include <mpfr.h>

#include "runtime/header.h"

extern "C" {
floating *hook_FLOAT_ceil(floating *);
floating *hook_FLOAT_floor(floating *);
floating *hook_FLOAT_trunc(floating *);
floating *hook_FLOAT_round(floating *, mpz_t, mpz_t);
mpz_ptr hook_FLOAT_float2int(floating *);
floating *hook_FLOAT_int2float(mpz_t, mpz_t, mpz_t);
floating *hook_FLOAT_sin(floating *);
floating *hook_FLOAT_cos(floating *);
floating *hook_FLOAT_tan(floating *);
floating *hook_FLOAT_sec(floating *);
floating *hook_FLOAT_csc(floating *);
floating *hook_FLOAT_cot(floating *);
floating *hook_FLOAT_asin(floating *);
floating *hook_FLOAT_acos(floating *);
floating *hook_FLOAT_atan(floating *);
floating *hook_FLOAT_atan2(floating *, floating *);
mpz_ptr hook_FLOAT_precision(floating *);
mpz_ptr hook_FLOAT_exponentBits(floating *);
mpz_ptr hook_FLOAT_exponent(floating *);
bool hook_FLOAT_isNaN(floating *);
floating *hook_FLOAT_maxValue(mpz_t, mpz_t);
floating *hook_FLOAT_minValue(mpz_t, mpz_t);
bool hook_FLOAT_gt(floating *, floating *);
bool hook_FLOAT_ge(floating *, floating *);
bool hook_FLOAT_lt(floating *, floating *);
bool hook_FLOAT_le(floating *, floating *);
bool hook_FLOAT_eq(floating *, floating *);
bool hook_FLOAT_ne(floating *, floating *);
floating *hook_FLOAT_abs(floating *);
floating *hook_FLOAT_neg(floating *);
floating *hook_FLOAT_min(floating *, floating *);
floating *hook_FLOAT_max(floating *, floating *);
floating *hook_FLOAT_add(floating *, floating *);
floating *hook_FLOAT_sub(floating *, floating *);
floating *hook_FLOAT_mul(floating *, floating *);
floating *hook_FLOAT_div(floating *, floating *);
floating *hook_FLOAT_rem(floating *, floating *);
floating *hook_FLOAT_pow(floating *, floating *);
floating *hook_FLOAT_root(floating *, mpz_t);
floating *hook_FLOAT_log(floating *);
floating *hook_FLOAT_exp(floating *);
floating *hook_FLOAT_rat2float(mpz_t, mpz_t, mpz_t, mpz_t);
bool hook_FLOAT_sign(floating *);

floating *move_float(floating *i) {
  floating *result = (floating *)malloc(sizeof(floating));
  *result = *i;
  return result;
}
}

static void set_float(floating *a, unsigned prec, unsigned exp, double val) {
  mpfr_init2(a->f, prec);
  a->exp = exp;
  mpfr_set_d(a->f, val, MPFR_RNDN);
}

BOOST_AUTO_TEST_SUITE(FloatTest)

BOOST_AUTO_TEST_CASE(ceil) {
  floating a[1];
  floating *result;
  set_float(a, 24, 8, 10.5);
  result = hook_FLOAT_ceil(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 11.0), 0);
}

BOOST_AUTO_TEST_CASE(floor) {
  floating a[1];
  floating *result;
  set_float(a, 24, 8, 10.5);
  result = hook_FLOAT_floor(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 10.0), 0);
}

BOOST_AUTO_TEST_CASE(trunc) {
  floating a[1];
  floating *result;

  set_float(a, 24, 8, 145.23);
  result = hook_FLOAT_trunc(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 145.0), 0);

  set_float(a, 53, 11, -0.5345);
  result = hook_FLOAT_trunc(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 0.0), 0);

  set_float(a, 24, 8, -2342.99);
  result = hook_FLOAT_trunc(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, -2342.0), 0);

  set_float(a, 53, 11, 54.34);
  result = hook_FLOAT_trunc(a);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 54.0), 0);
}

BOOST_AUTO_TEST_CASE(round) {
  floating a[1];
  floating *result;
  mpz_t b, c;
  mpz_init_set_ui(b, 2);
  mpz_init_set_ui(c, 8);
  set_float(a, 53, 11, 10.5);
  result = hook_FLOAT_round(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 12.0), 0);
  set_float(a, 53, 11, 9.5);
  result = hook_FLOAT_round(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 8.0), 0);
  set_float(a, 53, 11, 10.5);
  mpz_set_ui(b, 24);
  result = hook_FLOAT_round(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 10.5), 0);
  set_float(a, 53, 11, 9.5);
  result = hook_FLOAT_round(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 9.5), 0);
}

BOOST_AUTO_TEST_CASE(float2int) {
  floating a[1];
  mpz_ptr result;
  set_float(a, 53, 11, 10.5);
  result = hook_FLOAT_float2int(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 10), 0);
  set_float(a, 53, 11, 9.5);
  result = hook_FLOAT_float2int(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 10), 0);
}

BOOST_AUTO_TEST_CASE(int2float) {
  mpz_t a, b, c;
  floating *result;
  mpz_init_set_ui(a, 9);
  mpz_init_set_ui(b, 2);
  mpz_init_set_ui(c, 8);
  result = hook_FLOAT_int2float(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 8.0), 0);
  mpz_set_ui(a, 11);
  result = hook_FLOAT_int2float(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 12.0), 0);
  mpz_set_ui(a, 10);
  result = hook_FLOAT_int2float(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 8.0), 0);
  mpz_set_ui(b, 24);
  result = hook_FLOAT_int2float(a, b, c);
  BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, 10.0), 0);
}

BOOST_AUTO_TEST_CASE(trig) {
  floating a[1];
  floating *result;
  set_float(a, 53, 11, M_PI_4);
  result = hook_FLOAT_sin(a);
  double e = 0.000000000000001;
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), sin(M_PI_4), e);
  result = hook_FLOAT_cos(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), cos(M_PI_4), e);
  result = hook_FLOAT_tan(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), 1.0, e);
  result = hook_FLOAT_sec(a);
  BOOST_CHECK_CLOSE_FRACTION(
      mpfr_get_d(result->f, MPFR_RNDN), 1.0 / cos(M_PI_4), e);
  result = hook_FLOAT_csc(a);
  BOOST_CHECK_CLOSE_FRACTION(
      mpfr_get_d(result->f, MPFR_RNDN), 1.0 / sin(M_PI_4), e);
  result = hook_FLOAT_cot(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), 1.0, e);
  set_float(a, 53, 11, 0.0);
  result = hook_FLOAT_asin(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), 0.0, e);
  result = hook_FLOAT_acos(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), M_PI_2, e);
  result = hook_FLOAT_atan(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), 0, e);
  set_float(a, 53, 11, 1.0);
  result = hook_FLOAT_asin(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), M_PI_2, e);
  result = hook_FLOAT_acos(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), 0, e);
  result = hook_FLOAT_atan(a);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), M_PI_4, e);
  floating b[1];
  set_float(b, 53, 11, 0.0);
  result = hook_FLOAT_atan2(a, b);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), M_PI_2, e);
  set_float(a, 53, 11, -1.0);
  result = hook_FLOAT_atan2(a, b);
  BOOST_CHECK_CLOSE_FRACTION(mpfr_get_d(result->f, MPFR_RNDN), -M_PI_2, e);
}

BOOST_AUTO_TEST_CASE(precision) {
  floating a[1];
  mpz_ptr result;
  set_float(a, 2, 8, 0.0);
  result = hook_FLOAT_precision(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 2), 0);
  set_float(a, 24, 8, 0.0);
  result = hook_FLOAT_precision(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 24), 0);
}

BOOST_AUTO_TEST_CASE(exponentBits) {
  floating a[1];
  mpz_ptr result;
  set_float(a, 24, 8, 0.0);
  result = hook_FLOAT_exponentBits(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 8), 0);
  set_float(a, 53, 11, 0.0);
  result = hook_FLOAT_exponentBits(a);
  BOOST_CHECK_EQUAL(mpz_cmp_ui(result, 11), 0);
}

BOOST_AUTO_TEST_CASE(exponent) {
  floating a[1];
  mpz_ptr result;
  set_float(a, 24, 8, 0.0);
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, -127), 0);
  set_float(a, 24, 8, -0.0);
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, -127), 0);
  set_float(a, 24, 8, 1.0 / 0.0);
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, 128), 0);
  set_float(a, 24, 8, 0.0 / 0.0);
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, 128), 0);
  set_float(a, 24, 8, 4.0);
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, 2), 0);
  set_float(a, 24, 8, std::numeric_limits<float>::min());
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, -126), 0);
  set_float(a, 24, 8, std::numeric_limits<float>::denorm_min());
  result = hook_FLOAT_exponent(a);
  BOOST_CHECK_EQUAL(mpz_cmp_si(result, -127), 0);
}

BOOST_AUTO_TEST_CASE(isNaN) {
  floating a[1];
  set_float(a, 24, 8, 0.0);
  BOOST_CHECK(!hook_FLOAT_isNaN(a));
  set_float(a, 24, 8, -0.0);
  BOOST_CHECK(!hook_FLOAT_isNaN(a));
  set_float(a, 24, 8, 1.0 / 0.0);
  BOOST_CHECK(!hook_FLOAT_isNaN(a));
  set_float(a, 24, 8, 0.0 / 0.0);
  BOOST_CHECK(hook_FLOAT_isNaN(a));
}

BOOST_AUTO_TEST_CASE(maxValue) {
  mpz_t a, b;
  mpz_init_set_ui(a, 24);
  mpz_init_set_ui(b, 8);
  floating *result;
  result = hook_FLOAT_maxValue(a, b);
  BOOST_CHECK_EQUAL(
      mpfr_cmp_d(result->f, std::numeric_limits<float>::max()), 0);
  mpz_init_set_ui(a, 53);
  mpz_init_set_ui(b, 11);
  result = hook_FLOAT_maxValue(a, b);
  BOOST_CHECK_EQUAL(
      mpfr_cmp_d(result->f, std::numeric_limits<double>::max()), 0);
}

BOOST_AUTO_TEST_CASE(minValue) {
  mpz_t a, b;
  mpz_init_set_ui(a, 24);
  mpz_init_set_ui(b, 8);
  floating *result;
  result = hook_FLOAT_minValue(a, b);
  BOOST_CHECK_EQUAL(
      mpfr_cmp_d(result->f, std::numeric_limits<float>::denorm_min()), 0);
  mpz_init_set_ui(a, 53);
  mpz_init_set_ui(b, 11);
  result = hook_FLOAT_minValue(a, b);
  BOOST_CHECK_EQUAL(
      mpfr_cmp_d(result->f, std::numeric_limits<double>::denorm_min()), 0);
}

BOOST_AUTO_TEST_CASE(lt) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_lt(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(!result);
      } else if (i < j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(!hook_FLOAT_lt(arr + i, nan));
    BOOST_CHECK(!hook_FLOAT_lt(nan, arr + i));
  }
  BOOST_CHECK(!hook_FLOAT_lt(nan, nan));
}

BOOST_AUTO_TEST_CASE(le) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_le(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(result);
      } else if (i <= j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(!hook_FLOAT_le(arr + i, nan));
    BOOST_CHECK(!hook_FLOAT_le(nan, arr + i));
  }
  BOOST_CHECK(!hook_FLOAT_le(nan, nan));
}

BOOST_AUTO_TEST_CASE(gt) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_gt(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(!result);
      } else if (i > j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(!hook_FLOAT_gt(arr + i, nan));
    BOOST_CHECK(!hook_FLOAT_gt(nan, arr + i));
  }
  BOOST_CHECK(!hook_FLOAT_gt(nan, nan));
}

BOOST_AUTO_TEST_CASE(ge) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_ge(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(result);
      } else if (i >= j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(!hook_FLOAT_ge(arr + i, nan));
    BOOST_CHECK(!hook_FLOAT_ge(nan, arr + i));
  }
  BOOST_CHECK(!hook_FLOAT_ge(nan, nan));
}

BOOST_AUTO_TEST_CASE(eq) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_eq(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(result);
      } else if (i == j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(!hook_FLOAT_eq(arr + i, nan));
    BOOST_CHECK(!hook_FLOAT_eq(nan, arr + i));
  }
  BOOST_CHECK(!hook_FLOAT_eq(nan, nan));
}

BOOST_AUTO_TEST_CASE(ne) {
  floating arr[6], nan[1];
  set_float(arr, 24, 8, -1.0 / 0.0);
  set_float(arr + 1, 24, 8, -1.0);
  set_float(arr + 2, 24, 8, -0.0);
  set_float(arr + 3, 24, 8, 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, 1.0 / 0.0);
  set_float(nan, 24, 8, 0.0 / 0.0);
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    for (int j = 0; j < sizeof(arr) / sizeof(arr[0]); j++) {
      bool result = hook_FLOAT_ne(arr + i, arr + j);
      if ((i == 2 && j == 3) || (i == 3 && j == 2)) {
        BOOST_CHECK(!result);
      } else if (i != j) {
        BOOST_CHECK(result);
      } else {
        BOOST_CHECK(!result);
      }
    }
  }
  for (int i = 0; i < sizeof(arr) / sizeof(arr[0]); i++) {
    BOOST_CHECK(hook_FLOAT_ne(arr + i, nan));
    BOOST_CHECK(hook_FLOAT_ne(nan, arr + i));
  }
  BOOST_CHECK(hook_FLOAT_ne(nan, nan));
}

BOOST_AUTO_TEST_CASE(abs) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    floating *result = hook_FLOAT_abs(arr + i);
    float f = fabsf(ref[i]);
    if (f != f) {
      BOOST_CHECK(mpfr_nan_p(result->f));
    } else {
      BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(log) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    floating *result = hook_FLOAT_log(arr + i);
    float f = logf(ref[i]);
    if (f != f) {
      BOOST_CHECK(mpfr_nan_p(result->f));
    } else {
      BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(exp) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    floating *result = hook_FLOAT_exp(arr + i);
    float f = expf(ref[i]);
    if (f != f) {
      BOOST_CHECK(mpfr_nan_p(result->f));
    } else {
      BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(neg) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    floating *result = hook_FLOAT_neg(arr + i);
    float f = -ref[i];
    if (f != f) {
      BOOST_CHECK(mpfr_nan_p(result->f));
    } else {
      BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(min) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_min(arr + i, arr + j);
      float f = fminf(ref[i], ref[j]);
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(max) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_max(arr + i, arr + j);
      float f = fmaxf(ref[i], ref[j]);
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(add) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_add(arr + i, arr + j);
      float f = ref[i] + ref[j];
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(sub) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_sub(arr + i, arr + j);
      float f = ref[i] - ref[j];
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(mul) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_mul(arr + i, arr + j);
      float f = ref[i] * ref[j];
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(div) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_div(arr + i, arr + j);
      float f = ref[i] / ref[j];
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(rem) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_rem(arr + i, arr + j);
      float f = fmodf(ref[i], ref[j]);
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(pow) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    for (int j = 0; j < sizeof(ref) / sizeof(ref[0]); j++) {
      floating *result = hook_FLOAT_pow(arr + i, arr + j);
      float f = powf(ref[i], ref[j]);
      if (f != f) {
        BOOST_CHECK(mpfr_nan_p(result->f));
      } else {
        BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(root) {
  floating arr[9];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  float ref[9];
  ref[0] = 0.0f;
  ref[1] = -0.0f;
  ref[2] = 1.0f / 0.0f;
  ref[3] = -1.0f / 0.0f;
  ref[4] = 1.0f;
  ref[5] = -1.0f;
  ref[6] = 3.0f;
  ref[7] = 0.5f;
  ref[8] = 0.0f / 0.0f;
  mpz_t k;
  mpz_init_set_ui(k, 2);
  for (int i = 0; i < sizeof(ref) / sizeof(ref[0]); i++) {
    floating *result = hook_FLOAT_root(arr + i, k);
    float f = sqrt(ref[i]);
    if (f != f) {
      BOOST_CHECK(mpfr_nan_p(result->f));
    } else {
      BOOST_CHECK_EQUAL(mpfr_cmp_d(result->f, f), 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(sign) {
  floating arr[10];
  set_float(arr + 0, 24, 8, 0.0);
  set_float(arr + 1, 24, 8, -0.0);
  set_float(arr + 2, 24, 8, 1.0 / 0.0);
  set_float(arr + 3, 24, 8, -1.0 / 0.0);
  set_float(arr + 4, 24, 8, 1.0);
  set_float(arr + 5, 24, 8, -1.0);
  set_float(arr + 6, 24, 8, 3.0);
  set_float(arr + 7, 24, 8, 0.5);
  set_float(arr + 8, 24, 8, 0.0 / 0.0);
  BOOST_CHECK(!hook_FLOAT_sign(arr + 0));
  BOOST_CHECK(hook_FLOAT_sign(arr + 1));
  BOOST_CHECK(!hook_FLOAT_sign(arr + 2));
  BOOST_CHECK(hook_FLOAT_sign(arr + 3));
  BOOST_CHECK(!hook_FLOAT_sign(arr + 4));
  BOOST_CHECK(hook_FLOAT_sign(arr + 5));
  BOOST_CHECK(!hook_FLOAT_sign(arr + 6));
  BOOST_CHECK(!hook_FLOAT_sign(arr + 7));
  BOOST_CHECK(!hook_FLOAT_sign(arr + 8));
}

BOOST_AUTO_TEST_CASE(rat2float) {
  mpz_t num, den, prec, exp;
  mpz_init_set_ui(num, 1);
  mpz_init_set_ui(den, 3);
  mpz_init_set_ui(prec, 53);
  mpz_init_set_ui(exp, 11);
  floating *result = hook_FLOAT_rat2float(num, den, prec, exp);
  mpfr_t ref;
  mpfr_init2(ref, 53);
  mpfr_set_d(ref, 0.33333333333333333333333333333333333, MPFR_RNDN);
  BOOST_CHECK_EQUAL(mpfr_cmp(ref, result->f), 0);
}

BOOST_AUTO_TEST_SUITE_END()


//test_ptf_var.cpp

#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include<memory>

#include <Eigen/Dense>

#include "compute_returns_eigen.h"
#include "portfolio.h"
#include "instrument.h"
#include "ptf_var.h"
#include "var_model.h"
#include "compute_var.h"

using namespace Eigen;
using namespace std;

void readCSV(std::istream &input, std::vector< std::vector<std::string> > &output)
//https://www.gamedev.net/topic/444193-c-how-to-load-in-a-csv-file/
{
	std::string csvLine;
	// read every line from the stream
	while( std::getline(input, csvLine) )
	{
		std::istringstream csvStream(csvLine);
		std::vector<std::string> csvColumn;
		std::string csvElement;
		// read every element from the line that is seperated by commas
		// and put it into the vector or strings
		while( std::getline(csvStream, csvElement, ',') )
		{
			csvColumn.push_back(csvElement);
		}
		output.push_back(csvColumn);
	}
}

int main()
{

    try{

    // Read mid FX fix for currency pairs majors and exotics
    // daily series obtained for Bank of England through
	//https://www.quandl.com

	std::fstream file("/home/mrnoname/Documents/VaR/data/StockIndexData.csv", ios::in);
	if(!file.is_open())
	{
		std::cout << "File not found!\n";
		return 1;
	}
	// typedef to save typing for the following object
	typedef std::vector< std::vector<std::string> > csvVector;
	csvVector csvData;

	readCSV(file, csvData);

    //test
    for(size_t i = 0;i < 5; ++i){
        for(size_t j = 0;j < csvData[i].size();++j){
            cout << csvData[i][j] << '\t';

        }

        cout << endl;
    }
    cout << endl;

    // Remove lines with missing values

    size_t n(csvData.size() - 1);
    size_t m(csvData[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n-1062));

    for(size_t i = 1062;i < n;++i){
        for(size_t j = 1;j < csvData[i].size();++j){
            std::string tmp = csvData[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1062] = 99999.;
            }
            else{
                _prices[j-1][i-1062] = std::stod(tmp);
            }
        }
    }

    std::vector<std::string> indexNames(csvData[0].size() - 1);

    for(size_t i = 1;i < csvData[0].size();++i){
        indexNames[i-1] = csvData[0][i];
    }

	//Remove missing values to compute trailling returns
    //Asynchornous time series. Shift to the next value
    Mat prices;
    prices.resize(m,Vec(0));

	for(size_t i = 0;i < _prices.size();++i){
        for(size_t j = 0;j < _prices[i].size();++j){
            if(!((_prices[i][j] == 99999) || (_prices[i][j] == 0)))
                prices[i].push_back(_prices[i][j]);
        }
	}

    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,252,true));
	// 252 / 4 = 63 - 3 months
    // 4 * 252 = 1008 use 4 years of data to compute mean, and std dev

    Mat _rtns = cr->getReturns();

    // ------------------------------------------------

    // Case of full replication of index - DJIA,GSPC,NDX,GDAXI,FCHI,SSEC,SENSEX : 7 indices

	double a = double(1./7.); //cout << a << endl;

	std::vector<double> weights{a,a,a,a,a,a,a}; //initialization. Equi-weighted asset for mere convenience

	Ptf _ptf;

	for(unsigned int i = 0;i < 7;++i){
        shared_ptr<Instrument> instrument(new DeltaOne());
        auto p = std::make_pair(i,instrument);
		_ptf.push_back(p);
	}

	shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, false, 1.e+07));

	cout << "ptf's avg rtn: " << ptf->getMeanPtfRn() << endl;
	cout << "ptf's vol: " << ptf->getPtfSdev() << endl<< endl;

	double alpha = .05;

	VaRPtfCompute model(ptf, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - equi index " << alpha << " : " << model.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	RiskMetricsVaR var1; //(.05,.94,false);

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics(ptf, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics.computeVaR() << endl;

	// 2. GARCH

	GarchVaR var2; //(.05, 0., .25, .75, false);

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch(ptf, var2);

	cout << "GARCH VaR: " << VaRGarch.computeVaR() << endl;

	// ------------------------------------------------------------

	// 3. Historical method

	HistoricalVaR var3;

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical(ptf, var3);

	cout << "Historical VaR: " << VaRHistorical.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	HistoricalVaR var4(.05, .98, hybrid);

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical1(ptf, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical1.computeVaR() << endl;

	// 5. Historical method - HW method

	HistoricalVaR var5(.05, .94, hw);

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical2(ptf, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical2.computeVaR() << endl;

	//-----------------------------------------------------------------

	// Compute conponent VaR

	Vec compVaR = model.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << compVaR[i] << endl;

    double sumCompVaR(0.);
    for(auto& i : compVaR) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	Vec marVaR = model.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << marVaR[i] << endl;

	// Compute incremental VaR

	double amount = 1.e+06;

	cout << endl << "incremental VaR - add " << amount << " : " << model.computeIncrementalVaR(amount) << endl;

	//------------------------------------------------------------------------------------------------

    weights = {.1,.15,.2,.1,.1,.2,.15};

	shared_ptr<Portfolio> ptf1(new Portfolio(_ptf, weights, cr, false, 1.e+07));

	cout << "ptf's avg rtn: " << ptf1->getMeanPtfRn() << endl;
	cout << "ptf's vol: " << ptf1->getPtfSdev() << endl<< endl;

	VaRPtfCompute model1(ptf1, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - active index " << alpha << " : " << model1.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics1(ptf1, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics1.computeVaR() << endl;

	// 2. GARCH

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch1(ptf1, var2);

	cout << "GARCH VaR: " << VaRGarch1.computeVaR() << endl;

	// ------------------------------------------------------------

	// 3. Historical method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical11(ptf1, var3);

	cout << "Historical VaR: " << VaRHistorical11.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical12(ptf1, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical12.computeVaR() << endl;

	// 5. Historical method - HW method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical13(ptf1, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical13.computeVaR() << endl;

	//-----------------------------------------------------------------

	// Compute conponent VaR

	compVaR = model1.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << compVaR[i] << endl;

    sumCompVaR = 0.;
    for(auto& i : compVaR) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	marVaR = model1.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << marVaR[i] << endl;

	// Compute incremental VaR

	cout << endl << "incremental VaR - add " << amount << " : " << model1.computeIncrementalVaR(amount) << endl;

	//-----------------------------------------------------------------

	// Buy 1 call and 1 put on S&P 500. Sell .5 unit of index

	// Case of derivatives, full replication of index

	Vec weights1 =  {1.,1.,-.5};

	Ptf _ptf1;

	shared_ptr<Instrument> instrument(new Derivatives(0.46118, 0.01013));
    _ptf1.push_back(std::make_pair(1,instrument));
    shared_ptr<Instrument> instrument1(new Derivatives(-0.52983, 0.00658));
    _ptf1.push_back(std::make_pair(1,instrument1));
    shared_ptr<Instrument> instrument2(new DeltaOne());
	_ptf1.push_back(std::make_pair(1,instrument2));

	/*
    Call @SPX 161216C02200000 Delta0.46118 Gamma0.01013 Rho0.38430 Theta-0.79242 Vega1.69184 Impvol0.11648

    Put @SPX 161216P02200000 Delta-0.52983 Gamma0.00658 Rho-0.34403 Theta-0.93300 Vega1.67799 Impvol0.11835

	*/

	shared_ptr<Portfolio> ptf2(new Portfolio(_ptf1, weights1, cr, false, 1.e+07));

    VaRPtfCompute model2(ptf2, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - equity derivatives " << alpha << " : " << model2.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics21(ptf2, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics21.computeVaR() << endl;

	// 2. GARCH

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch22(ptf1, var2);

	cout << "GARCH VaR: " << VaRGarch22.computeVaR() << endl;

	// ------------------------------------------------------------

	// 3. Historical method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical21(ptf1, var3);

	cout << "Historical VaR: " << VaRHistorical21.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical22(ptf1, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical22.computeVaR() << endl;

	// 5. Historical method - HW method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical23(ptf1, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical23.computeVaR() << endl;

	// Compute conponent VaR

	Vec compVaR2 = model2.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 3;++i)
		cout << i << ": " << compVaR2[i] << endl;

    sumCompVaR = 0.;
    for(auto& i : compVaR2) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	Vec marVaR2 = model2.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 3;++i)
		cout << i << ": " << marVaR2[i] << endl;

	// Compute incremental VaR

	cout << endl << "incremental VaR - add " << amount << " : " << model2.computeIncrementalVaR(amount) << endl;

    // ------------------------------------------------------------

    return 0;

    } catch (const std::exception& e) { // caught by reference to base
        std::cout << " a standard exception was caught, with message '"
                  << e.what() << "'\n";
    }

}


//
// Created by Amir Masoud Abdol on 2020-07-28
//

#include "HackingProbabilityStrategy.h"

#include <boost/math/special_functions/relative_difference.hpp>

using namespace sam;
using boost::math::relative_difference;

HackingProbabilityStrategy::~HackingProbabilityStrategy(){
    // Pure destructors
};

std::unique_ptr<HackingProbabilityStrategy>
HackingProbabilityStrategy::build(json &config) {

  auto params = config.get<FrankenbachStrategy::Parameters>();
  return std::make_unique<FrankenbachStrategy>(params);
}

float FrankenbachStrategy::estimate(Experiment *experiment) {

  /// @todo replace this with boost comparison
  if ((params.base_hp - 0.) < 0.00001) {
    return 0.0;
  }

  // We have something in the middle now, so, we are calculating based on the
  // p-value we check for significance, if sig, then we return 0. else, then we
  // assign a value

  bool is_any_non_sig{false};
  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {
    is_any_non_sig |= !experiment->dvs_[i].sig_;
  }

  if (!is_any_non_sig) {
    return 0.0;
  }

  // I have a feeling this is a very inefficient implementation
  int d = experiment->setup.nd();
  int g = experiment->setup.ng();

  arma::Row<float> dangers(g - d);

  dangers.imbue([&, i = d]() mutable {
    return this->border(experiment->dvs_[i++].effect_sei);
  });

  probabilities.resize(g - d);
  probabilities.imbue([&, i = d]() mutable {
    if (experiment->dvs_[i].effect_ > dangers[i - 1]) {

      float d_sig = experiment->dvs_[i].effect_sei * 1.959964;
      arma::Row<float> danger_breaks =
          arma::linspace<arma::Row<float>>(dangers[i - 1], d_sig, 11);

      // If the hacking probability is 1, then everything in this range is
      // going to be hacked, a.k.a, hp = 1; Update: I think I had this wrong
      // previously, where I assign the probability to everything, while it
      // should only be assigned to those studies that are passing the effect
      // test in the first place
      if ((params.base_hp - 1.) < 0.00001) {
        return static_cast<float>(1);
      }

      return arma::as_scalar(hp_range.at(
          arma::max(find(danger_breaks < experiment->dvs_[i++].effect_))));
    }
    // else
    return static_cast<float>(0);
  });

  spdlog::trace("Chance of hacking: {}", arma::max(probabilities));

  // @todo Remember that you should consider some option here. At the moment,
  // I'm returning the maximum of all probabilities, but that's not necessarily
  // the best things to do, also, it works just fine in Frankenbach simulation
  // because they have only one one outcome anyway
  return arma::max(probabilities);
}


#define CATCH_CONFIG_MAIN
#include <catch/catch.hpp>

#include <ctime>

#include <complex>
#include <iostream>

#include <Eigen/Core>

#include <fftw3/shared_plan.hpp>

using Index         = Eigen::Index;
using Real          = double;
using Complex       = std::complex<Real>;
using RealVector    = Eigen::VectorXd;
using ComplexVector = Eigen::VectorXcd;
using RealMatrix    = Eigen::MatrixXd;
using ComplexMatrix = Eigen::MatrixXcd;

using FFT  = fftw3::FFT<Real>;
using IFFT = fftw3::IFFT<Real>;

static const double eps = 1.0e-14;

void test_complex_fft_many_1d(Index n, Index howmany)
{
    INFO("1D complex-to-complex FFT (n = " << n << ", howmany = " << howmany
                                           << ')');

    ComplexMatrix f1(n, howmany);
    ComplexMatrix re_f1(n, howmany);
    ComplexMatrix g1(n, howmany);

    RealMatrix f2(n, howmany);
    RealMatrix re_f2(n, howmany);
    ComplexMatrix g2(n / 2 + 1, howmany);

    int n_       = static_cast<int>(n);
    int howmany_ = static_cast<int>(howmany);

    // Complex-to-complex
    auto plan_fft  = FFT::make_plan(n_, howmany_, f1.data(), g1.data());
    auto plan_ifft = IFFT::make_plan(n_, howmany_, f1.data(), g1.data());

    f1.setRandom();
    FFT::run(plan_fft, f1.data(), g1.data());
    IFFT::run(plan_ifft, g1.data(), re_f1.data());
    re_f1 /= Real(n);

    for (Index i = 0; i < howmany; ++i)
    {
        const Real residual = (f1.col(i) - re_f1.col(i)).norm();
        CHECK(residual == Approx(0.0).margin(n * eps));
    }
}

void test_real_fft_many_1d(Index n, Index howmany)
{
    INFO("1D real-to-complex FFT (n = " << n << ", howmany = " << howmany
                                        << ')');

    RealMatrix f2(n, howmany);
    RealMatrix re_f2(n, howmany);
    ComplexMatrix g2(n / 2 + 1, howmany);

    RealVector residuals(howmany);

    int n_       = static_cast<int>(n);
    int howmany_ = static_cast<int>(howmany);

    // Real-to-complex transform
    auto plan_fft_r2c  = FFT::make_plan(n_, howmany_, f2.data(), g2.data());
    auto plan_ifft_r2c = IFFT::make_plan(n_, howmany_, g2.data(), re_f2.data());

    f2.setRandom();
    FFT::run(plan_fft_r2c, f2.data(), g2.data());
    IFFT::run(plan_ifft_r2c, g2.data(), re_f2.data());
    re_f2 /= Real(n);

    for (Index i = 0; i < howmany; ++i)
    {
        const Real residual = (f2.col(i) - re_f2.col(i)).norm();
        CHECK(residual == Approx(0.0).margin(n * eps));
    }
}

TEST_CASE("Test thread-safe FFTW3 wrapper for multiple 1D FFT")
{
    std::srand(static_cast<unsigned>(std::time(0)));
    SECTION("Complex-to-complex FFT")
    {
        test_complex_fft_many_1d(100, 20);
        test_complex_fft_many_1d(1000, 10);
        test_complex_fft_many_1d(10000, 5);
        test_complex_fft_many_1d(100000, 2);
        test_complex_fft_many_1d(1000000, 1);
    }

    SECTION("Real-to-complex FFT")
    {
        test_real_fft_many_1d(100, 20);
        test_real_fft_many_1d(1000, 10);
        test_real_fft_many_1d(10000, 5);
        test_real_fft_many_1d(100000, 2);
        test_real_fft_many_1d(1000000, 1);
    }
}


////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::exponential::log_unnormalized_pdf.hpp //
//                                                                            //
//  (C) Copyright 2009 Erwann Rogard                                          //
//  Use, modification and distribution are subject to the                     //
//  Boost Software License, Version 1.0. (See accompanying file               //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)          //
////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_EXPONENTIAL_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_EXPONENTIAL_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <string>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{
namespace math{

    template<typename T,typename P>
    T
    log_unnormalized_pdf(
        const boost::math::exponential_distribution<T,P>& d,
        const T& x
    ){

        typedef std::string str_;
        BOOST_MATH_STD_USING // for ADL of std functions
        static const char* function =
            (str_("log_unnormalized_pdf(")+
            "const exponential_distribution<%1%>&,%1%)").c_str();

        T lambda = d.lambda();
        T result;

        if(0 == boost::math::detail::verify_lambda(
            function, lambda, &result, P()))
            return result;
        if(0 == boost::math::detail::verify_exp_x(
            function, x, &result, P()))
            return result;
        result = (-lambda * x);
        return result;
    }

}// math
}// boost

#endif