//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2022 Ilia Shirobokov <i.shirobokov@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE pedersen_test

#include <vector>
#include <iostream>
#include <random>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/random_element.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>

#include <nil/crypto3/math/polynomial/polynomial.hpp>

#include <nil/crypto3/zk/commitments/polynomial/pedersen.hpp>

using namespace nil::crypto3;

BOOST_AUTO_TEST_SUITE(pedersen_test_suite)

BOOST_AUTO_TEST_CASE(pedersen_basic_test) {

    // setup
    using curve_type = algebra::curves::bls12<381>;
    using curve_group_type = curve_type::template g1_type<>;
    using field_type = typename curve_type::scalar_field_type;

    constexpr static const int n = 50;
    constexpr static const int k = 26;
    static curve_group_type::value_type g = algebra::random_element<curve_group_type>();
    static curve_group_type::value_type h = algebra::random_element<curve_group_type>();
    while (g == h) {
        h = algebra::random_element<curve_group_type>();
    }

    typedef typename zk::commitments::pedersen<curve_type> pedersen_type;

    typedef typename pedersen_type::proof_type proof_type;
    typedef typename pedersen_type::params_type params_type;

    params_type params;

    params.n = n;
    params.k = k;
    params.g = g;
    params.h = h;

    BOOST_CHECK(g != h);
    BOOST_CHECK(n >= k);
    BOOST_CHECK(k > 0);

    // commit
    constexpr static const field_type::value_type w = field_type::value_type(37684);

    // eval
    proof_type proof = pedersen_type::proof_eval(params, w);

    // verify
    BOOST_CHECK(pedersen_type::verify_eval(params, proof));

    std::vector<int> idx;
    std::vector<int> idx_base;
    for (int i = 1; i <= n; ++i) {
        idx_base.push_back(i);
    }
    std::random_device rd;
    std::mt19937 gen(rd());
    std::shuffle(idx_base.begin(), idx_base.end(), gen);
    for (int i = 0; i < k; ++i) {
        idx.push_back(idx_base[i]);
    }

    BOOST_CHECK(idx.size() >= k);
    field_type::value_type secret = pedersen_type::message_eval(params, proof, idx);
    BOOST_CHECK(w == secret);
}

BOOST_AUTO_TEST_CASE(pedersen_short_test) {

    // setup
    using curve_type = algebra::curves::bls12<381>;
    using curve_group_type = curve_type::template g1_type<>;
    using field_type = typename curve_type::scalar_field_type;

    constexpr static const int n = 2;
    constexpr static const int k = 1;
    static curve_group_type::value_type g = algebra::random_element<curve_group_type>();
    static curve_group_type::value_type h = algebra::random_element<curve_group_type>();
    while (g == h) {
        h = algebra::random_element<curve_group_type>();
    }

    typedef typename zk::commitments::pedersen<curve_type> pedersen_type;

    typedef typename pedersen_type::proof_type proof_type;
    typedef typename pedersen_type::params_type params_type;

    params_type params;

    params.n = n;
    params.k = k;
    params.g = g;
    params.h = h;

    BOOST_CHECK(g != h);
    BOOST_CHECK(n >= k);
    BOOST_CHECK(k > 0);

    // commit
    constexpr static const field_type::value_type w = field_type::value_type(3);

    // eval
    proof_type proof = pedersen_type::proof_eval(params, w);

    // verify
    BOOST_CHECK(pedersen_type::verify_eval(params, proof));

    std::vector<int> idx;
    std::vector<int> idx_base;
    for (int i = 1; i <= n; ++i) {
        idx_base.push_back(i);
    }
    std::random_device rd;
    std::mt19937 gen(rd());
    std::shuffle(idx_base.begin(), idx_base.end(), gen);
    for (int i = 0; i < k; ++i) {
        idx.push_back(idx_base[i]);
    }

    BOOST_CHECK(idx.size() >= k);
    field_type::value_type secret = pedersen_type::message_eval(params, proof, idx);
    BOOST_CHECK(w == secret);
}

BOOST_AUTO_TEST_CASE(pedersen_long_test) {

    // setup
    using curve_type = algebra::curves::bls12<381>;
    using curve_group_type = curve_type::template g1_type<>;
    using field_type = typename curve_type::scalar_field_type;

    constexpr static const int n = 2000000000;
    constexpr static const int k = 1999999999;
    static curve_group_type::value_type g = algebra::random_element<curve_group_type>();
    static curve_group_type::value_type h = algebra::random_element<curve_group_type>();
    while (g == h) {
        h = algebra::random_element<curve_group_type>();
    }

    typedef typename zk::commitments::pedersen<curve_type> pedersen_type;

    typedef typename pedersen_type::proof_type proof_type;
    typedef typename pedersen_type::params_type params_type;

    params_type params;

    params.n = n;
    params.k = k;
    params.g = g;
    params.h = h;

    BOOST_CHECK(g != h);
    BOOST_CHECK(n >= k);
    BOOST_CHECK(k > 0);

    // commit
    constexpr static const field_type::value_type w = field_type::value_type(300000000);

    // eval
    proof_type proof = pedersen_type::proof_eval(params, w);

    // verify
    BOOST_CHECK(pedersen_type::verify_eval(params, proof));

    std::vector<int> idx;
    std::vector<int> idx_base;
    for (int i = 1; i <= n; ++i) {
        idx_base.push_back(i);
    }
    std::random_device rd;
    std::mt19937 gen(rd());
    std::shuffle(idx_base.begin(), idx_base.end(), gen);
    for (int i = 0; i < k; ++i) {
        idx.push_back(idx_base[i]);
    }

    BOOST_CHECK(idx.size() >= k);
    field_type::value_type secret = pedersen_type::message_eval(params, proof, idx);
    BOOST_CHECK(w == secret);
}

BOOST_AUTO_TEST_SUITE_END()


//std::function< std::vector<bool> (long N)> remember this is what is being passed in!
#include <NTL/ZZ.h>

#include "../elements/Element.hpp"
#include "../algorithms/rem_forest.hpp"

#include <vector>
#include <random>

using std::vector;

vector<bool> constant_slow(long B) {
	long n = 1;
	for (long i = 0; i < 10*B; ++i) { n = (((n*2+17)/3-19)*5-101); } //just gibberish

	vector<bool> incidence;
	bool ans = (B+n)%5;

	incidence.push_back(ans);
	return incidence;
}

vector<bool> random_zz_remtree(long B) {

	vector <Elt<NTL::ZZ> > A_rand (B);
	vector <Elt<NTL::ZZ> > m_rand (B);

	std::random_device rd;
	std::mt19937 mt(rd());
	std::uniform_int_distribution<long> dist(1, B);

	for(long i = 0; i < B; ++i){
		//long bitsize = log2(i+1)+2;
		A_rand[i] = dist(mt);
		m_rand[i] = dist(mt);
	}

	remainder_forest(A_rand, m_rand, 0, 0);

	vector<bool> ret(1);
	return ret;
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include "gdls_star/gdls_star.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <cmath>
#include <complex>
#include <utility>
#include <vector>

#include "math/alignment.h"
#include "math/utils.h"
#include "upnp/build_upnp_action_matrix_using_symmetry.h"

namespace msft {
namespace {
// Useful aliases.
using Matrix3x10d = Eigen::Matrix<double, 3, 10>;
using Matrix10d = Eigen::Matrix<double, 10, 10>;
using Matrix8cd = Eigen::Matrix<std::complex<double>, 8, 8>;
using Matrix8d = Eigen::Matrix<double, 8, 8>;
using RowVector10d = Eigen::Matrix<double, 1, 10>;
using Vector10d = Eigen::Matrix<double, 10, 1>;

using CostParameters = GdlsStar::CostParameters;
using Input = GdlsStar::Input;
using Priors = GdlsStar::Priors;
using Solution = GdlsStar::Solution;

constexpr int kNumMaxRotationsExploitingSymmetry = 8;

// Computes the skew-symmetric matrix to compute the cross product.
inline Eigen::Matrix3d
ComputeSkewSymmetricMatrix(const Eigen::Vector3d& vector) {
  Eigen::Matrix3d skew_symmetric_matrix;
  skew_symmetric_matrix.setZero();
  skew_symmetric_matrix(0, 1) = -vector.z();
  skew_symmetric_matrix(1, 0) = vector.z();
  skew_symmetric_matrix(0, 2) = vector.y();
  skew_symmetric_matrix(2, 0) = -vector.y();
  skew_symmetric_matrix(1, 2) = -vector.x();
  skew_symmetric_matrix(2, 1) = vector.x();
  return skew_symmetric_matrix;
}

// This function arranges a 3D point into a 3x10 matrix so that we can
// rotate a point using the rotation expressed as a function of the monomials.
// See Eq. xxiv of the gDLS* supplemental material.
Matrix3x10d LeftMultiply(const Eigen::Vector3d& point) {
  Matrix3x10d phi_mat;
  // Row 0.
  phi_mat(0, 0) = point.x();
  phi_mat(0, 1) = point.x();
  phi_mat(0, 2) = -point.x();
  phi_mat(0, 3) = -point.x();
  phi_mat(0, 4) = 0.0;
  phi_mat(0, 5) = 2 * point.z();
  phi_mat(0, 6) = -2 * point.y();
  phi_mat(0, 7) = 2 * point.y();
  phi_mat(0, 8) = 2 * point.z();
  phi_mat(0, 9) = 0.0;

  // Row 1.
  phi_mat(1, 0) = point.y();
  phi_mat(1, 1) = -point.y();
  phi_mat(1, 2) = point.y();
  phi_mat(1, 3) = -point.y();
  phi_mat(1, 4) = -2.0 * point.z();
  phi_mat(1, 5) = 0.0;
  phi_mat(1, 6) = 2 * point.x();
  phi_mat(1, 7) = 2 * point.x();
  phi_mat(1, 8) = 0.0;
  phi_mat(1, 9) = 2 * point.z();

  // Row 3.
  phi_mat(2, 0) = point.z();
  phi_mat(2, 1) = -point.z();
  phi_mat(2, 2) = -point.z();
  phi_mat(2, 3) = point.z();
  phi_mat(2, 4) = 2.0 * point.y();
  phi_mat(2, 5) = -2.0 * point.x();
  phi_mat(2, 6) = 0.0;
  phi_mat(2, 7) = 0.0;
  phi_mat(2, 8) = 2.0 * point.x();
  phi_mat(2, 9) = 2.0 * point.y();
  return phi_mat;
}

// Check that the input to gDLS* is valid. It checks the inputs have the same
// sizes and that the prior scales are valid.
inline void IsInputDatumValid(const Input& input) {
  CHECK_EQ(input.ray_origins.size(), input.ray_directions.size());
  CHECK_EQ(input.ray_origins.size(), input.world_points.size());
  CHECK_GE(input.priors.scale_penalty, 0.0);
  CHECK_GE(input.priors.gravity_penalty, 0.0);
}

Eigen::Matrix4d ComputeHMatrix(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const double scale_penalty_factor) {
  Eigen::Matrix4d h_inverse = Eigen::Matrix4d::Zero();
  for (size_t i = 0; i < ray_directions.size(); ++i) {
    // Computing the scalar in the top-left corner of matrix H^-1.
    h_inverse(0, 0) +=
        ray_origins[i].squaredNorm() -
        ray_origins[i].dot(ray_directions[i]) *
        ray_origins[i].dot(ray_directions[i]);

    // Computing the 3x1 row-vector of the top-right part of matrix H^-1.
    const Eigen::Vector3d temp_term =
        ray_origins[i].dot(ray_directions[i]) * ray_directions[i] -
        ray_origins[i];
    h_inverse.block<3, 1>(1, 0) += temp_term;

    // Computing the 1x3 vector of the bottom-left part of matrix H^-1.
    h_inverse.block<1, 3>(0, 1) += temp_term.transpose();

    // Bottom right 3x3 block of matrix H^-1.
    h_inverse.block<3, 3>(1, 1) +=
        Eigen::Matrix3d::Identity() -
        (ray_directions[i] * ray_directions[i].transpose());
  }

  // Add the scale penalty term to the first entry of h_inverse.
  h_inverse(0, 0) += scale_penalty_factor;

  const Eigen::Matrix4d h_matrix = h_inverse.inverse();

  return h_matrix;
}

// Compute the the matrix F in Eq. xiv i the supplemental material of gDLS*.
// This matrix is F = -B * H.
Eigen::MatrixXd ComputeFMat(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const Eigen::Matrix4d& h_matrix) {
  const int num_points = ray_origins.size();
  Eigen::MatrixXd f_mat(num_points, 4);
  Eigen::RowVector4d helper_vec;
  for (size_t i = 0; i < num_points; ++i) {
    // Scalar term: -r_i' c_i.
    helper_vec[0] = -ray_directions[i].dot(ray_origins[i]);
    // Vector term: r'.
    helper_vec.tail(3) = ray_directions[i].transpose();
    // Computing i-th row of matrix F.
    f_mat.block(i, 0, 1, 4) = helper_vec * h_matrix;
  }
  return f_mat;
}

void ComputeScaleAndTranslationFactors(const Input& input,
                                       const Eigen::Matrix4d& h_matrix,
                                       RowVector10d* scale_factor,
                                       Matrix3x10d* translation_factor) {
  // Useful aliases.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;
  const size_t num_correspondences = ray_directions.size();
  Eigen::Matrix<double, 4, 10> sv_helper = Eigen::Matrix<double, 4, 10>::Zero();
  for (size_t i = 0; i < num_correspondences; ++i) {
    const Matrix3x10d left_multiply_matrix = LeftMultiply(world_points[i]);
    // Scale factor.
    sv_helper.row(0) +=
        (ray_origins[i].transpose() - ray_origins[i].dot(ray_directions[i]) *
         ray_directions[i].transpose()) * left_multiply_matrix;

    // Translation factor.
    sv_helper.block<3, 10>(1, 0) +=
        (ray_directions[i] * ray_directions[i].transpose() -
         Eigen::Matrix3d::Identity()) * left_multiply_matrix;
  }

  sv_helper = h_matrix * sv_helper;
  *scale_factor = sv_helper.row(0);
  *translation_factor = sv_helper.block<3, 10>(1, 0);
}

// Computes vector k_i from the scale-constrained gDLS.
inline Eigen::Vector3d ComputeLinearTermPerPoint(
    const int point_idx,
    const Eigen::Matrix4d& h_matrix,
    const Eigen::MatrixXd& f_matrix,
    const Eigen::Vector3d& ray_origin,
    const Eigen::Vector3d& ray_direction,
    const Priors& priors) {
  // Scale linear term.
  const Eigen::Vector3d scalar_linear_term =
      f_matrix(point_idx, 0) * ray_direction - h_matrix.col(0).tail(3) +
      h_matrix(0, 0) * ray_origin;

  // Compute total linear term.
  const Eigen::Vector3d linear_term =
      priors.scale_prior * priors.scale_penalty * scalar_linear_term;
  return linear_term;
}

Matrix10d
ComputeQuadraticPenaltyMatrixFromGravityRegularizer(const Input& input,
                                                    double penalty) {
  // Compute the left-multiply matrix for gravity index.
  const Matrix3x10d world_gravity_dir_matrix =
      LeftMultiply(input.priors.world_down_direction.normalized());
  const Eigen::Vector3d query_gravity_dir =
      input.priors.query_down_direction.normalized();
  const Eigen::Matrix3d query_down_dir_mat =
      ComputeSkewSymmetricMatrix(query_gravity_dir);
  // M = penalty * L(g_I)' * Cross(g_Q)^T * Cross(g_Q) L(g_I),
  // L() is the left-multiply function, and Cross() is the skew-symmetric
  // matrix for cross product. Here g indicates gravity direction according
  // to Q (query) and I (reference).
  const Matrix10d penalty_matrix =
      penalty * world_gravity_dir_matrix.transpose() *
      query_down_dir_mat.transpose() *
      query_down_dir_mat * world_gravity_dir_matrix;
  return penalty_matrix;
}

inline Matrix10d ComputeQuadraticPenaltiesFromScaleConstraint(
    const double scale_penalty,
    const RowVector10d& scale_factor) {
  return scale_penalty * scale_factor.transpose() * scale_factor;
}

inline Vector10d ComputeLinearPenaltiesFromScaleConstraint(
    const Priors& priors,
    const Eigen::Matrix4d& h_matrix,
    const RowVector10d& scale_factor) {
  const double& scale_prior = priors.scale_prior;
  const double& scale_penalty = priors.scale_penalty;
  const double scalar =
      scale_penalty * scale_prior * h_matrix(0, 0) - scale_prior;
  return scale_penalty * scalar * scale_factor;
}

inline double ComputeConstantTermFromScaleConstraint(
    const Priors& priors,
    const Eigen::Matrix4d& h_matrix) {
  const double& scale_penalty = priors.scale_penalty;
  const double& scale_prior = priors.scale_prior;
  const double scale_term =
      scale_penalty * scale_prior * h_matrix(0, 0) - scale_prior;
  const double term = scale_term * scale_term;
  return scale_penalty * term;
}

// The observed pattern is that duplicate rotations appear consequtively in the
// vector, i.e., rotation[i] == rotation[i + 1] is common.
std::vector<Eigen::Quaterniond> RemoveDuplicateRotations(
    const std::vector<Eigen::Quaterniond>& candidate_rotations) {
  const double kAngleThreshold = DegToRad(0.1);
  std::vector<Eigen::Quaterniond> rotations;
  rotations.reserve(candidate_rotations.size());

  // If no rotations then return empty vector.
  if (candidate_rotations.empty()) {
    return rotations;
  }

  for (int i = 0; i < candidate_rotations.size(); ++i) {
    bool duplicate_rotation = false;
    const Eigen::Quaterniond& candidate_rotation = candidate_rotations[i];
    for (int j = rotations.size() - 1; j >= 0; --j) {
      if (candidate_rotation.angularDistance(rotations[j]) < kAngleThreshold) {
        duplicate_rotation = true;
        break;
      }
    }
    if (!duplicate_rotation) {
      rotations.push_back(candidate_rotation);
    }
  }
  return rotations;
}

// Constructs the vector s as indicated in Eq. 13 of gDLS* main paper.
inline Vector10d ComputeRotationVector(const Eigen::Quaterniond& rotation) {
  Vector10d rotation_vector;
  // Set the values of the rotation vector.
  rotation_vector[0] = rotation.w() * rotation.w();
  rotation_vector[1] = rotation.x() * rotation.x();
  rotation_vector[2] = rotation.y() * rotation.y();
  rotation_vector[3] = rotation.z() * rotation.z();
  rotation_vector[4] = rotation.w() * rotation.x();
  rotation_vector[5] = rotation.w() * rotation.y();
  rotation_vector[6] = rotation.w() * rotation.z();
  rotation_vector[7] = rotation.x() * rotation.y();
  rotation_vector[8] = rotation.x() * rotation.z();
  rotation_vector[9] = rotation.y() * rotation.z();
  return rotation_vector;
}

std::vector<Eigen::Vector3d> ComputeScalesAndTranslations(
    const std::vector<Eigen::Quaterniond>& rotations,
    const Matrix3x10d& translation_factor,
    const Input& input,
    const Eigen::Matrix4d& h_matrix,
    const RowVector10d& scale_factor,
    std::vector<double>* scales) {
  // Solve for translation as a function of rotation.
  std::vector<Eigen::Vector3d> translations;
  translations.reserve(rotations.size());
  CHECK_NOTNULL(scales)->reserve(rotations.size());
  const double& scale_penalty = input.priors.scale_penalty;
  for (const Eigen::Quaterniond& rotation : rotations) {
    const Vector10d rotationVector = ComputeRotationVector(rotation);
    translations.emplace_back(
        translation_factor * rotationVector +
        scale_penalty * input.priors.scale_prior * h_matrix.col(0).tail(3));
    scales->emplace_back(
        scale_factor * rotationVector +
        scale_penalty * input.priors.scale_prior * h_matrix(0, 0));
  }
  return translations;
}

void DiscardBadSolutions(const Input& input_datum, Solution* solution) {
  // Useful aliases.
  std::vector<Eigen::Quaterniond>& solution_rotations = solution->rotations;
  std::vector<Eigen::Vector3d>& solution_translations = solution->translations;
  std::vector<double>& solution_scales = solution->scales;
  CHECK_EQ(solution_rotations.size(), solution_translations.size());
  CHECK_EQ(solution_rotations.size(), solution_scales.size());
  std::vector<Eigen::Quaterniond> final_rotations;
  std::vector<Eigen::Vector3d> final_translations;
  std::vector<double> final_scales;
  final_rotations.reserve(solution_rotations.size());
  final_translations.reserve(solution_translations.size());
  final_scales.reserve(solution_scales.size());

  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;

  // For every computed solution, check that points are in front of camera.
  for (int i = 0; i < solution_rotations.size(); ++i) {
    const Eigen::Quaterniond& soln_rotation = solution_rotations[i];
    const Eigen::Vector3d& soln_translation = solution_translations[i];
    const double scale = solution_scales[i];

    // Check that all points are in front of the camera. Discard the solution
    // if this is not the case.
    bool all_points_in_front_of_camera = true;

    for (int j = 0; j < world_points.size(); ++j) {
      const Eigen::Vector3d transformed_point =
          soln_rotation * world_points[j] + soln_translation -
          scale * ray_origins[j];

      // Find the rotation that puts the image ray at unit Z direction.
      const Eigen::Quaterniond unrot =
          Eigen::Quaterniond::FromTwoVectors(ray_directions[j],
                                             Eigen::Vector3d::UnitZ());

      // Rotate the transformed point and check if the z coordinate is
      // negative. This will indicate if the point is projected behind the
      // camera.
      const Eigen::Vector3d rotated_projection = unrot * transformed_point;
      if (rotated_projection.z() < 0) {
        all_points_in_front_of_camera = false;
        break;
      }
    }

    if (all_points_in_front_of_camera) {
      final_rotations.emplace_back(soln_rotation);
      final_translations.emplace_back(soln_translation);
      final_scales.push_back(scale);
    }
  }

  // Set the final solutions.
  std::swap(solution_rotations, final_rotations);
  std::swap(solution_translations, final_translations);
  std::swap(solution_scales, final_scales);
}

}  // namespace

void GdlsStar::ComputeHelperMatrices(const Input& input) {
  // Compute helper matrices.
  // Compute H matrix from.
  helper_matrices_.h_matrix =
      ComputeHMatrix(input.ray_origins,
                     input.ray_directions,
                     input.priors.scale_penalty);

  // Compute the F matrix, they are needed for the translation prior part.
  helper_matrices_.f_matrix = ComputeFMat(input.ray_origins,
                                          input.ray_directions,
                                          helper_matrices_.h_matrix);

  // Compute scale and translation factors.
  ComputeScaleAndTranslationFactors(input,
                                    helper_matrices_.h_matrix,
                                    &helper_matrices_.scale_factor,
                                    &helper_matrices_.translation_factor);
}

void GdlsStar::ComputeLeastSquaresCostParameters(const Input& input) {
  // Useful aliases.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;
  const size_t num_correspondences = ray_directions.size();

  cost_params_.quadratic_penalty_mat.setZero();
  cost_params_.linear_penalty_vector.setZero();
  cost_params_.gamma = 0.0;
  for (int i = 0; i < num_correspondences; ++i) {
    // Compute the quadratic term per point.
    const Matrix3x10d left_multiply_matrix = LeftMultiply(world_points[i]);
    const Matrix3x10d cost_coeff_term =
        (ray_directions[i] * ray_directions[i].transpose() -
         Eigen::Matrix3d::Identity()) *
        (LeftMultiply(world_points[i]) -
         ray_origins[i] * helper_matrices_.scale_factor +
         helper_matrices_.translation_factor);
    cost_params_.quadratic_penalty_mat +=
        cost_coeff_term.transpose() * cost_coeff_term;

    // Compute the linear term per point as calculated in Eq. 22 of scale and
    // gravity constrained gdls.
    const Eigen::Vector3d scalar_penalty_vector =
        ComputeLinearTermPerPoint(i,
                                  helper_matrices_.h_matrix,
                                  helper_matrices_.f_matrix,
                                  ray_origins[i],
                                  ray_directions[i],
                                  input.priors);
    cost_params_.linear_penalty_vector +=
        scalar_penalty_vector.transpose() * cost_coeff_term;

    // Compute the constant term per point.
    cost_params_.gamma += scalar_penalty_vector.dot(scalar_penalty_vector);
  }

  // Add penalties for gravity constraints.
  if (input.priors.gravity_penalty > 0.0) {
    cost_params_.quadratic_penalty_mat +=
        ComputeQuadraticPenaltyMatrixFromGravityRegularizer(
            input, input.priors.gravity_penalty);
  }

  // Add penalties for scale constraints.
  if (input.priors.scale_penalty > 0.0) {
    cost_params_.quadratic_penalty_mat +=
        ComputeQuadraticPenaltiesFromScaleConstraint(
            input.priors.scale_penalty,
            helper_matrices_.scale_factor);
    cost_params_.linear_penalty_vector +=
        ComputeLinearPenaltiesFromScaleConstraint(
            input.priors,
            helper_matrices_.h_matrix,
            helper_matrices_.scale_factor);
    cost_params_.gamma +=
        ComputeConstantTermFromScaleConstraint(
            input.priors,
            helper_matrices_.h_matrix);
  }
}

CostParameters GdlsStar::ComputeCostParameters(const Input& input) {
  // Validate input.
  IsInputDatumValid(input);

  // Compute all the helper matrices.
  ComputeHelperMatrices(input);

  // Compute the least squares cost parameters.
  ComputeLeastSquaresCostParameters(input);

  return cost_params_;
}

std::vector<Eigen::Quaterniond> GdlsStar::EstimateRotations() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotationsExploitingSymmetry);
  // Build action matrix.
  const Matrix8d action_matrix = theia::BuildActionMatrixUsingSymmetry(
      cost_params_.quadratic_penalty_mat,
      cost_params_.linear_penalty_vector,
      &template_matrix_);

  const Eigen::EigenSolver<Matrix8d> eigen_solver(action_matrix);
  const Matrix8cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // Complex solutions can be good, in particular when the number of
    // correspondences is really low. To use these complex solutions, we simply
    // ignore their imaginary part.
    rotations[i] = Eigen::Quaterniond(eigen_vectors(4, i).real(),
                                      eigen_vectors(5, i).real(),
                                      eigen_vectors(6, i).real(),
                                      eigen_vectors(7, i).real()).normalized();
  }

  return RemoveDuplicateRotations(rotations);
}

bool GdlsStar::EstimateSimilarityTransformation(const Input& input,
                                                Solution* solution) {
  CHECK_NOTNULL(solution)->rotations.clear();
  solution->translations.clear();
  solution->scales.clear();

  // Construct cost parameters.
  ComputeCostParameters(input);

  // Estimate rotations.
  solution->rotations = EstimateRotations();

  // Compute translations and scales.
  solution->translations =
      ComputeScalesAndTranslations(solution->rotations,
                                   helper_matrices_.translation_factor,
                                   input,
                                   helper_matrices_.h_matrix,
                                   helper_matrices_.scale_factor,
                                   &solution->scales);

  // Discard solutions that do not have the points in front of the camera.
  DiscardBadSolutions(input, solution);
  return !solution->rotations.empty();
}

}  // namespace msft


#include <mmtbx/error.h>
#include <iotbx/pdb/hierarchy.h>
#include <scitbx/vec3.h>
#include <boost/optional.hpp>
#include <limits>
#include <cstring>

#define HB_Q1 0.42
#define HB_Q2 0.2
#define HB_F 332
#define HB_F_Q1_Q2 27.888

namespace mmtbx { namespace secondary_structure { namespace dssp {
  using namespace iotbx::pdb::hierarchy;

  inline double hbond_energy (
      scitbx::vec3<double> c_xyz,
      scitbx::vec3<double> h_xyz,
      scitbx::vec3<double> o_xyz,
      scitbx::vec3<double> n_xyz) {
    double r_ON = (o_xyz - n_xyz).length();
    double r_CH = (c_xyz - h_xyz).length();
    double r_OH = (o_xyz - h_xyz).length();
    double r_CN = (c_xyz - n_xyz).length();
    double E = HB_F_Q1_Q2 * (1./r_ON + 1./r_CH - 1./r_OH - 1./r_CN);
    return E;
  }

  // given a backbone nitrogen atom, calculate the position of the attached
  // hydrogen atom.
  boost::optional< scitbx::vec3<double> >
  get_n_h_position (
    atom const& N,
    double nh_bond_length=1.01)
  {
    bool have_caN = false;
    bool have_cN = false;
    scitbx::vec3<double> N_xyz = N.data->xyz;
    scitbx::vec3<double> caN_xyz(0,0,0);
    scitbx::vec3<double> cN_xyz(0,0,0);
    // find CA attached to N
    boost::optional<atom_group> ag_N = N.parent();
    MMTBX_ASSERT(ag_N);
    unsigned n_ats = ag_N->atoms_size();
    std::vector<atom> agN_atoms = ag_N->atoms();
    for(unsigned i_at=0;i_at<n_ats;i_at++) {
      atom const& a = agN_atoms[i_at];
      if (std::strcmp(a.data->name.elems, " CA ") == 0) {
        caN_xyz = scitbx::vec3<double>(a.data->xyz);
        have_caN = true;
        break;
      }
    }
    if (! have_caN) {
      return boost::optional< scitbx::vec3<double> >();
    }
    // find previous C attached to N
    boost::optional<residue_group> rg_N = ag_N->parent();
    boost::optional<chain> chn_N = rg_N->parent();
    unsigned n_rgs = chn_N->residue_groups_size();
    std::vector<residue_group> rgs = chn_N->residue_groups();
    residue_group prev_rg;
    bool have_prev_rg = false;
    for (unsigned i_rg = 0; i_rg < n_rgs; i_rg++) {
      if (rgs[i_rg].memory_id() == rg_N->memory_id()) {
        break;
      } else {
        prev_rg = rgs[i_rg];
        have_prev_rg = true;
      }
    }
    if (! have_prev_rg) {
      return boost::optional< scitbx::vec3<double> >();
    }
    unsigned n_ags = prev_rg.atom_groups_size();
    std::vector<atom_group> const& ags = prev_rg.atom_groups();
    for (unsigned i_ag = 0; i_ag < n_ags; i_ag++) {
      if (ags[i_ag].data->altloc.elems[0] == ag_N->data->altloc.elems[0]) {
        n_ats = ags[i_ag].atoms_size();
        std::vector<atom> prev_atoms = ags[i_ag].atoms();
        for (unsigned i_at = 0; i_at < n_ats; i_at++) {
          atom const& a = prev_atoms[i_at];
          if (std::strcmp(a.data->name.elems, " C  ") == 0) {
            cN_xyz = scitbx::vec3<double>(a.data->xyz);
            have_cN = true;
            break;
          }
        }
        break;
      }
    }
    if (! have_cN) {
      return boost::optional< scitbx::vec3<double> >();
    }
    scitbx::vec3<double> midpoint = (caN_xyz + cN_xyz) / 2;
    scitbx::vec3<double> vec_nm = N_xyz - midpoint;
    scitbx::vec3<double>hN_xyz = N_xyz + (vec_nm.normalize() * nh_bond_length);
    return boost::optional< scitbx::vec3<double> >(hN_xyz);
  }

  // given carbonyl oxygen and amino nitrogen atoms, calculate the hydrogen
  // bond energy between them.
  boost::optional<double>
  get_o_n_hbond_energy (
    atom const& O,
    atom const& N,
    double nh_bond_length=1.01)
  {
    scitbx::vec3<double> N_xyz = N.data->xyz;
    scitbx::vec3<double> O_xyz = O.data->xyz;
    bool have_cO = false;
    scitbx::vec3<double> cO_xyz(0,0,0);
    boost::optional< scitbx::vec3<double> > hN_xyz;
    // find C attached to O
    boost::optional<atom_group> ag_O = O.parent();
    MMTBX_ASSERT(ag_O);
    unsigned n_ats = ag_O->atoms_size();
    std::vector<atom> const& atoms = ag_O->atoms();
    for(unsigned i_at=0;i_at<n_ats;i_at++) {
      atom const& a = atoms[i_at];
      if (std::strcmp(a.data->name.elems, " C  ") == 0) {
        cO_xyz = scitbx::vec3<double>(a.data->xyz);
        have_cO = true;
        break;
      }
    }
    if (! have_cO) {
      return boost::optional<double>();
    }
    double rCN = (cO_xyz - N_xyz).length_sq();
    if (rCN > 49.0) {
      return boost::optional<double>();
    }
    // now calculate the hydrogen position
    hN_xyz = get_n_h_position(N, nh_bond_length);
    if (! hN_xyz) {
      return boost::optional<double>();
    }
    // finally we can calculate the energy
    return hbond_energy(cO_xyz, *hN_xyz, O_xyz, N_xyz);
  }

}}} // namespace mmtbx::secondary_structure::dssp


#include "PitBased.h"
#include <boost/math/distributions/normal.hpp>
#include "../Parameters.h"
#include "../Obs.h"

UpdaterPitBased::UpdaterPitBased(const Options& iOptions, const Data& iData) : Updater(iOptions, iData) {
   iOptions.check();
}
float UpdaterPitBased::update(float iCdf,
      const Obs& iRecentObs,
      const Distribution::ptr iRecent,
      const Distribution::ptr iDist,
      const Parameters& iParameters) const {
   float obs = iRecentObs.getValue();
   if(!Global::isValid(obs))
      return iCdf;

   float recentPit = iRecent->getCdf(obs);
   if(!Global::isValid(recentPit))
      return iCdf;

   float sigma0 = iParameters[0];
   float sigma = getSigma(sigma0);
   float returnValue = Global::MV;
   int   n = Global::getTimeDiff(iDist->getDate(), iDist->getInit(), iDist->getOffset(),
                                 iRecentObs.getDate(), iRecentObs.getInit(), iRecentObs.getOffset());

   // TODO
   if(n == 0) {
      if(iCdf < recentPit)
         return 0;
      else if(iCdf > recentPit)
         return 1;
      else
         return recentPit;
   }
   n = 1;

   if(sigma > 0.5) {
      return iCdf;
   }
   else {
      float accum = 0;
      for(int i = -mNumIterations; i <= mNumIterations; i++) {
         float mean = recentPit;
         float std  = sqrt((float) n)*sigma;
         assert(std > 0);
         boost::math::normal dist(mean, std);
         float part1 = boost::math::cdf(dist, iCdf + 2*i) - boost::math::cdf(dist, 2*i);
         float part2 = (1 - boost::math::cdf(dist, -iCdf + 2*i)) - (1 - boost::math::cdf(dist, 2*i));
         float diff = part1 + part2;
         if(diff < 0) diff = 0;
         if(diff > 1) diff = 1;
         accum += diff;
      }
      if(accum < 0) {
         accum = 0;
         assert(0);
      }
      else if(accum > 1) {
         accum = 1;
         //assert(0);
      }
      returnValue = accum;
   }

   return returnValue;
}
float UpdaterPitBased::getSigma(float iSigma0) const {
   float sigma;
   if(iSigma0 > 0.3) {
      sigma = 10;
   }
   else {
      sigma = tan(iSigma0*3.5)/3.5;
   }
   return sigma;
}

void UpdaterPitBased::getDefaultParameters(Parameters& iParameters) const {
   float sigma0 = 0.30;
   iParameters[0] = sigma0;
}

void UpdaterPitBased::updateParameters(const std::vector<Distribution::ptr>& iDists,
      const std::vector<Obs>& iObs,
      const std::vector<Distribution::ptr>& iRecentDists,
      const std::vector<Obs>& iRecentObs,
      Parameters& iParameters) const {
   // Own
   assert(iDists.size() == iObs.size());
   assert(iDists.size() == iRecentDists.size());
   assert(iDists.size() == iRecentObs.size());
   float prevSigma = iParameters[0];
   int counter = 0;
   float total = 0;
   for(int i = 0; i < iDists.size(); i++) {
      int   n = Global::getTimeDiff(iDists[i]->getDate(), iDists[i]->getInit(), iDists[i]->getOffset(),
            iRecentObs[i].getDate(), 0, iRecentObs[i].getOffset());
      //std::cout << iDists[i]->getDate() << " " << iRecentObs[i].getDate() << " " << n << std::endl;
      if(n > 0) {
         //std::cout << "Update parameters: " << iObs[i].getValue() << " " << iRecentObs[i].getValue() << " " << n << std::endl;
         float recentObs = iRecentObs[i].getValue();
         float obs       = iObs[i].getValue();
         if(Global::isValid(recentObs) && Global::isValid(obs)) {
            float recentPit = iRecentDists[i]->getCdf(recentObs);
            float pit       = iDists[i]->getCdf(obs);
            if(Global::isValid(recentPit) && Global::isValid(pit)) {
               // TODO: Use the value of 'n'
               total += fabs(recentPit - pit);
               counter++;
            }
         }
      }
   }
   if(counter > 0) {
      float newSigma = total / counter;
      iParameters[0] = Processor::combine(prevSigma, newSigma, counter);
   }
}


//  (C) Copyright Nick Thompson 2021.
//  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)

#include <random>
#include <array>
#include <vector>
#include <benchmark/benchmark.h>
#include <boost/math/interpolators/bezier_polynomial.hpp>

using boost::math::interpolators::bezier_polynomial;

template<class Real>
void BezierPolynomial(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<Real> unif(0, 10);

    std::vector<std::array<Real, 3>> v(state.range(0));

    for (size_t i = 0; i < v.size(); ++i) {
        v[i][0] = unif(mt);
        v[i][1] = unif(mt);
        v[i][2] = unif(mt);
    }

    auto bp = bezier_polynomial(std::move(v));
    Real t = 0;
    for (auto _ : state)
    {
        auto p = bp(t);
        benchmark::DoNotOptimize(p[0]);
        t += std::numeric_limits<Real>::epsilon();
    }
     state.SetComplexityN(state.range(0));
}

BENCHMARK_TEMPLATE(BezierPolynomial, double)->DenseRange(2, 30)->Complexity();

BENCHMARK_MAIN();


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <math/rev/scal/util.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <limits>

TEST(AgradRev, if_else) {
  using stan::math::if_else;
  using stan::math::var;

  EXPECT_FLOAT_EQ(1.0, if_else(true, var(1.0), var(2.0)).val());
  EXPECT_FLOAT_EQ(2.0, if_else(false, var(1.0), var(2.0)).val());

  EXPECT_FLOAT_EQ(1.0, if_else(true, 1.0, var(2.0)).val());
  EXPECT_FLOAT_EQ(2.0, if_else(false, 1.0, var(2.0)).val());

  EXPECT_FLOAT_EQ(1.0, if_else(true, var(1.0), 2.0).val());
  EXPECT_FLOAT_EQ(2.0, if_else(false, var(1.0), 2.0).val());
}

TEST(AgradRev, if_else_nan) {
  using stan::math::if_else;

  double nan = std::numeric_limits<double>::quiet_NaN();
  stan::math::var nan_v = std::numeric_limits<double>::quiet_NaN();
  stan::math::var a_v = 1.2;

  EXPECT_FLOAT_EQ(1.2, if_else(true, 1.2, nan_v).val());
  EXPECT_FLOAT_EQ(1.2, if_else(true, a_v, nan).val());
  EXPECT_FLOAT_EQ(1.2, if_else(true, a_v, nan_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, 1.2, nan_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, a_v, nan).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, a_v, nan_v).val());

  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan_v, 2.4).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan, a_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan_v, a_v).val());

  a_v = 2.4;
  EXPECT_FLOAT_EQ(2.4, if_else(false, nan_v, 2.4).val());
  EXPECT_FLOAT_EQ(2.4, if_else(false, nan, a_v).val());
  EXPECT_FLOAT_EQ(2.4, if_else(false, nan_v, a_v).val());

  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan_v, nan).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan, nan_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(true, nan_v, nan_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, nan, nan_v).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, nan_v, nan).val());
  EXPECT_PRED1(boost::math::isnan<double>, if_else(false, nan_v, nan_v).val());
}

TEST(AgradRev, check_varis_on_stack_27) {
  stan::math::var x = 1.0;
  stan::math::var y = 2.0;
  test::check_varis_on_stack(stan::math::if_else(true, x, y));
  test::check_varis_on_stack(stan::math::if_else(false, x, y));
  test::check_varis_on_stack(stan::math::if_else(true, x, 2.0));
  test::check_varis_on_stack(stan::math::if_else(false, x, 2.0));
  test::check_varis_on_stack(stan::math::if_else(true, 1.0, y));
  test::check_varis_on_stack(stan::math::if_else(false, 1.0, y));
}


/*=============================================================================
    Spirit v1.6.0
    Copyright (c) 2002-2003 Joel de Guzman
    Copyright (c) 2002 Juan Carlos Arevalo-Baeza
    http://spirit.sourceforge.net/

    Permission to copy, use, modify, sell and distribute this software is
    granted provided this copyright notice appears in all copies. This
    software is provided "as is" without express or implied warranty, and
    with no claim as to its suitability for any purpose.
=============================================================================*/
#include <boost/spirit/core.hpp>
#include <boost/spirit/utility/functor_parser.hpp>
#include <iostream>
#include <vector>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;

///////////////////////////////////////////////////////////////////////////////
//
//  Our parser functor
//
///////////////////////////////////////////////////////////////////////////////
struct number_parser
{
    typedef int result_t;
    template <typename ScannerT>
    int
    operator()(ScannerT const& scan, result_t& result) const
    {
        if (scan.at_end())
            return -1;

        char ch = *scan;
        if (ch < '0' || ch > '9')
            return -1;

        result = 0;
        int len = 0;

        do
        {
            result = result*10 + int(ch - '0');
            ++len;
            ++scan;
        } while (!scan.at_end() && (ch = *scan, ch >= '0' && ch <= '9'));

        return len;
    }
};

functor_parser<number_parser> number_parser_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Our number parser functions
//
///////////////////////////////////////////////////////////////////////////////
bool
parse_number(char const* str, int& n)
{
    return parse(str, lexeme_d[number_parser_p[assign(n)]], space_p).full;
}

bool
parse_numbers(char const* str, std::vector<int>& n)
{
    return
        parse(
            str,
            lexeme_d[number_parser_p[append(n)]]
                >> *(',' >> lexeme_d[number_parser_p[append(n)]]),
            space_p
        ).full;
}

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA number parser implemented as a functor for Spirit...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me an integer number command\n";
    cout << "Commands:\n";
    cout << "  A <num> --> parses a single number\n";
    cout << "  B <num>, <num>, ... --> parses a series of numbers ";
    cout << "separated by commas\n";
    cout << "  Q --> quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str[0] == 'q' || str[0] == 'Q')
            break;

        else if (str[0] == 'a' || str[0] == 'A')
        {
            int n;
            if (parse_number(str.c_str()+1, n))
            {
                cout << "-------------------------\n";
                cout << "Parsing succeeded\n";
                cout << str << " Parses OK: " << n << endl;
                cout << "-------------------------\n";
            }
            else
            {
                cout << "-------------------------\n";
                cout << "Parsing failed\n";
                cout << "-------------------------\n";
            }
        }

        else if (str[0] == 'b' || str[0] == 'B')
        {
            std::vector<int> n;
            if (parse_numbers(str.c_str()+1, n))
            {
                cout << "-------------------------\n";
                cout << "Parsing succeeded\n";
                int size = n.size();
                cout << str << " Parses OK: " << size << " number(s): " << n[0];
                for (int i = 1; i < size; ++i) {
                    cout << ", " << n[i];
                }
                cout << endl;
                cout << "-------------------------\n";
            }
            else
            {
                cout << "-------------------------\n";
                cout << "Parsing failed\n";
                cout << "-------------------------\n";
            }
        }

        else
        {
            cout << "-------------------------\n";
            cout << "Unrecognized command!!";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/time/schedule.hpp>
#include <qle/instruments/impliedbondspread.hpp>
#include <qle/pricingengines/discountingriskybondengine.hpp>

#include <boost/make_shared.hpp>

using namespace boost::unit_test_framework;
using namespace QuantLib;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(BondsTest)

BOOST_AUTO_TEST_CASE(testBondSpreads) {

    BOOST_TEST_MESSAGE("Testing QuantExt bond spread helper");

    SavedSettings backup;
    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();

    // market data
    Handle<Quote> rateQuote(boost::make_shared<SimpleQuote>(0.02));
    Handle<Quote> issuerSpreadQuote(boost::make_shared<SimpleQuote>(0.01));
    DayCounter dc = Actual365Fixed();
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(today, rateQuote, dc, Compounded, Semiannual));
    Handle<DefaultProbabilityTermStructure> dpts(boost::make_shared<FlatHazardRate>(today, issuerSpreadQuote, dc));
    Handle<Quote> bondSpecificSpread(boost::make_shared<SimpleQuote>(0.005));

    // build the bond
    Date startDate = today;
    Date endDate = startDate + Period(10, Years);
    Period tenor = 6 * Months;
    Calendar calendar = WeekendsOnly();
    BusinessDayConvention bdc = Following;
    BusinessDayConvention bdcEnd = bdc;
    DateGeneration::Rule rule = DateGeneration::Forward;
    bool endOfMonth = false;
    Date firstDate, lastDate;
    Schedule schedule(startDate, endDate, tenor, calendar, bdc, bdcEnd, rule, endOfMonth, firstDate, lastDate);

    Real redemption = 100.0;
    Real couponRate = 0.04;
    Leg leg =
        FixedRateLeg(schedule).withNotionals(redemption).withCouponRates(couponRate, dc).withPaymentAdjustment(bdc);

    boost::shared_ptr<QuantLib::Bond> bond(boost::make_shared<QuantLib::Bond>(0, WeekendsOnly(), today, leg));
    Handle<Quote> recovery;
    boost::shared_ptr<PricingEngine> pricingEngine(
        boost::make_shared<QuantExt::DiscountingRiskyBondEngine>(yts, dpts, recovery, bondSpecificSpread, 1 * Months));
    bond->setPricingEngine(pricingEngine);

    Real price = bond->dirtyPrice();
    BOOST_TEST_MESSAGE("Bond price = " << price);

    // now calculated the implied bond spread, given the price
    boost::shared_ptr<SimpleQuote> tmpSpread = boost::make_shared<SimpleQuote>(0.0);
    Handle<Quote> tmpSpreadH(tmpSpread);
    boost::shared_ptr<PricingEngine> tmpEngine;
    tmpEngine.reset(new QuantExt::DiscountingRiskyBondEngine(yts, dpts, recovery, tmpSpreadH, 1 * Months));

    Real impliedSpread = QuantExt::detail::ImpliedBondSpreadHelper::calculate(bond, tmpEngine, tmpSpread, price, false,
                                                                              1.e-12, 10000, -0.02, 1.00);
    BOOST_TEST_MESSAGE("Implied spread = " << impliedSpread);
    BOOST_CHECK_CLOSE(impliedSpread, bondSpecificSpread->value(), 0.0001);

    Real price2 = bond->dirtyPrice();
    BOOST_CHECK_EQUAL(price, price2);

    // which spread would mean the bond price is par?
    Real parRedemption = 100.0;
    Real impliedSpreadPar = QuantExt::detail::ImpliedBondSpreadHelper::calculate(
        bond, tmpEngine, tmpSpread, parRedemption, false, 1.e-12, 10000, -0.02, 1.00);
    BOOST_TEST_MESSAGE("Par bond price would require spread of " << impliedSpreadPar);
    BOOST_CHECK_EQUAL(
        price, bond->dirtyPrice()); // ensure hypothetical impliedSpread calc has not affected the original position

    boost::dynamic_pointer_cast<SimpleQuote>(*bondSpecificSpread)->setValue(impliedSpreadPar);
    Real pricePar = bond->dirtyPrice();
    BOOST_TEST_MESSAGE("Bond spread of " << bondSpecificSpread->value() << " means price of " << pricePar);
    BOOST_CHECK_CLOSE(pricePar, parRedemption, 0.0001);

    // now check that bond pricing works even if no credit curve exists

    dpts = Handle<DefaultProbabilityTermStructure>();
    pricingEngine.reset(new QuantExt::DiscountingRiskyBondEngine(yts, dpts, recovery, bondSpecificSpread, 1 * Months));
    tmpEngine.reset(new QuantExt::DiscountingRiskyBondEngine(yts, dpts, recovery, tmpSpreadH, 1 * Months));
    bond->setPricingEngine(pricingEngine);
    Real priceNoIssuerCurve = bond->dirtyPrice();
    BOOST_TEST_MESSAGE("Bond price (ignoring issuer spread) = " << priceNoIssuerCurve);
    impliedSpread = QuantExt::detail::ImpliedBondSpreadHelper::calculate(bond, tmpEngine, tmpSpread, priceNoIssuerCurve,
                                                                         false, 1.e-12, 10000, -0.02, 1.00);
    BOOST_TEST_MESSAGE("Bond spread (ignoring issuer spread) = " << impliedSpread);
    BOOST_CHECK_CLOSE(impliedSpread, bondSpecificSpread->value(), 0.0001);

    // which spread would mean the bond price is par?
    impliedSpreadPar = QuantExt::detail::ImpliedBondSpreadHelper::calculate(bond, tmpEngine, tmpSpread, parRedemption,
                                                                            false, 1.e-12, 10000, -0.02, 1.00);
    BOOST_TEST_MESSAGE("Par bond price would require spread of " << impliedSpreadPar);
    BOOST_CHECK_CLOSE(impliedSpreadPar, bondSpecificSpread->value() + issuerSpreadQuote->value(), 0.0001);
    boost::dynamic_pointer_cast<SimpleQuote>(*bondSpecificSpread)->setValue(impliedSpreadPar);
    pricePar = bond->dirtyPrice();
    BOOST_TEST_MESSAGE("Bond spread of " << bondSpecificSpread->value() << " means price of " << pricePar);
    BOOST_CHECK_CLOSE(pricePar, parRedemption, 0.0001);
}
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file qfeinterpolator.cc
 * @brief NPDE homework DebuggingFEM code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include "qfeinterpolator.h"

#include <lf/mesh/mesh.h>

#include <Eigen/Core>

namespace DebuggingFEM {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d globalCoordinate(int idx, const lf::mesh::Entity &cell) {
  // Consistency check for arguments
  LF_ASSERT_MSG(cell.RefEl() == lf::base::RefEl::kTria(),
                "Implemented for triangles only");
  // Fetch pointer to asscoiated geometry object
  const lf::geometry::Geometry *geom = cell.Geometry();
  // For returning the global coordinates of the interpolation node
  Eigen::Vector2d result;
  // Reference coordinates of the vertices of the triangle
  Eigen::Matrix<double, 2, 3> corners(2, 3);
  corners << 0., 1., 0., 0., 0., 1.;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_1 */

}  // namespace DebuggingFEM


#include <benchmark/benchmark.h>

#include <Eigen/Core>

#include <mitrax/dim.hpp>

#include <random>
#include <string>

#include "../../../include/get_binaryop.hpp"

using namespace mitrax;
using namespace mitrax::literals;

template < typename T, typename Op >
[[gnu::noinline]]
void bm(benchmark::State& state, Op op, rt_dim_pair_t d1, rt_dim_pair_t d2){
	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_int_distribution< T > dis(
		std::numeric_limits< T >::min(),
		std::numeric_limits< T >::max()
	);

	Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > m1(
		size_t(d1.rows()), size_t(d1.cols())
	);
	for(int y = 0; y < m1.rows(); ++y){
		for(int x = 0; x < m1.cols(); ++x){
			m1(y, x) = dis(gen);
		}
	}

	Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > m2(
		size_t(d2.rows()), size_t(d2.cols())
	);
	for(int y = 0; y < m2.rows(); ++y){
		for(int x = 0; x < m2.cols(); ++x){
			m2(y, x) = dis(gen);
		}
	}

	while(state.KeepRunning()){
		auto res = op(m1, m2).eval();
		benchmark::DoNotOptimize(res);
	}
}

#include <boost/hana/tuple.hpp>
#include <boost/hana/for_each.hpp>

namespace init{

	constexpr auto dimensions = boost::hana::make_tuple(
			dim_pair(2_CS, 2_RS),
			dim_pair(4_CS, 2_RS),
			dim_pair(8_CS, 2_RS),
			dim_pair(8_CS, 4_RS),
			dim_pair(8_CS, 8_RS),
			dim_pair(8_CS, 16_RS),
			dim_pair(8_CS, 32_RS),
			dim_pair(8_CS, 64_RS),
			dim_pair(16_CS, 64_RS),
			dim_pair(32_CS, 64_RS),
			dim_pair(64_CS, 64_RS),
			dim_pair(128_CS, 64_RS),
			dim_pair(256_CS, 64_RS),
			dim_pair(256_CS, 128_RS),
			dim_pair(256_CS, 256_RS)
		);

	using plus = std::plus<>;
	using multiplies = std::multiplies<>;

}

#include "main.hpp"


#include <iostream>
#define ADEPT_NO_AUTOMATIC_DIFFERENTIATION
#define ADEPT_REAL_TYPE_SIZE 4
#include <adept_arrays.h>
#include "Timer.h"

#define ASSIGN   =
#define WARMUP_OPERATOR + exp
#define OPERATOR + fastexp
//#define SUFFIX_OP + 0.5
#define SUFFIX_OP

using namespace adept;

int main()
{
  Timer timer;
  timer.print_on_exit();
  int n = 128;

  static const int rep = 10000;
  //  static const int rep = 10;

  std::cout << "Packet<Real>::size = " << internal::Packet<Real>::size << "\n";

  Stack stack;

  aMatrix M(n,n), P(n,n), Q(n,n);
  //  Array<2,aReal,false> M(n,n), P(n,n), Q(n,n);
  aReal Mc[n][n], Pc[n][n], Qc[n][n];

  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < n; ++j) {
      P(i,j) = Pc[i][j] = 0.01 * (i-j);
      Q(i,j) = Qc[i][j] = 0.1 * (j+1);
      M(i,j) = Mc[i][j] = 0.0;
    }
  }

  int t_c_style_w = timer.new_activity("C-style for loops (warm-up)");
  int t_c_style = timer.new_activity("C-style for loops");
  int t_adept_w = timer.new_activity("Adept (warm-up)");
  int t_adept = timer.new_activity("Adept");
  int t_adept_container_w = timer.new_activity("Adept container only (warm-up)");
  int t_adept_container = timer.new_activity("Adept container only");
#ifndef ADEPT_NO_AUTOMATIC_DIFFERENTIATION
  int t_jacobian_w = timer.new_activity("Jacobian (warm-up)");
  int t_jacobian = timer.new_activity("Jacobian");
  int t_jacobian_array_w = timer.new_activity("Jacobian array-op (warm-up)");
  int t_jacobian_array = timer.new_activity("Jacobian array-op");
#endif

  stack.new_recording();
  timer.start(t_c_style_w);
  for (int irep = 0; irep < rep; ++irep) {
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	Mc[i][j] ASSIGN Pc[i][j] WARMUP_OPERATOR (Qc[i][j] SUFFIX_OP);
      }
    }
  }
  timer.stop();

  if (n <= 10) {
    std::cout << "C-style M = \n";
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	std::cout << " " << Mc[i][j];
      }
      std::cout << "\n";
    }
  }

  //  std::cout << stack;

  stack.new_recording();
  timer.start(t_c_style);
  for (int irep = 0; irep < rep; ++irep) {
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	Mc[i][j] ASSIGN Pc[i][j] OPERATOR (Qc[i][j] SUFFIX_OP);
      }
    }
  }
  timer.stop();
  //  std::cout << stack;

#ifndef ADEPT_NO_AUTOMATIC_DIFFERENTIATION
  stack.independent(&Pc[0][0], n*n);
  stack.dependent(&Mc[0][0], n*n);

  timer.start(t_jacobian_w);
  Real* jac;
  jac = new Real[n*n*n*n];

  stack.jacobian_forward(jac);
  timer.stop();
  timer.start(t_jacobian);
  stack.jacobian_forward(jac);
  timer.stop();
#endif

  //  std::cout << Mc[0][0] << " " << Mc[10][10] << "\n";

  stack.new_recording();
  timer.start(t_adept_w);
  for (int irep = 0; irep < rep; ++irep) {
    //    M ASSIGN noalias(P WARMUP_OPERATOR (Q SUFFIX_OP));
    M ASSIGN P WARMUP_OPERATOR (Q SUFFIX_OP);
  }
  timer.stop();
  //  std::cout << stack;

  if (n <= 10) {
    std::cout << "Array-style M = \n";
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	std::cout << " " << M(i,j);
      }
      std::cout << "\n";
    }
  }

  std::cout << "Alignment offset = " << (P OPERATOR (Q SUFFIX_OP)).alignment_offset() << "\n";

  stack.new_recording();
  timer.start(t_adept);
  for (int irep = 0; irep < rep; ++irep) {
    //    M += noalias(P OPERATOR (Q SUFFIX_OP));
    M ASSIGN P OPERATOR (Q SUFFIX_OP);
  }
  timer.stop();
  //  std::cout << stack;

#ifndef ADEPT_NO_AUTOMATIC_DIFFERENTIATION

  stack.clear_independents();
  stack.clear_dependents();
  stack.independent(P);
  stack.dependent(Q);
  //  stack.independent(P.data(), n*n);
  //  stack.dependent(M.data(), n*n);

  std::cout << stack;

  timer.start(t_jacobian_array_w);
  stack.jacobian_forward(jac);
  timer.stop();
  timer.start(t_jacobian_array);
  stack.jacobian_forward(jac);
  timer.stop();
#endif

  stack.new_recording();
  timer.start(t_adept_container_w);
  for (int irep = 0; irep < rep; ++irep) {
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	M(i,j) ASSIGN P(i,j) WARMUP_OPERATOR (Q(i,j) SUFFIX_OP);
      }
    }
  }
  timer.stop();
  //  std::cout << stack;
  //  std::cout << M;

  stack.new_recording();
  timer.start(t_adept_container);
  for (int irep = 0; irep < rep; ++irep) {
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < n; ++j) {
	M(i,j) ASSIGN P(i,j) OPERATOR (Q(i,j) SUFFIX_OP);
      }
    }
  }
  timer.stop();
  //  std::cout << stack;
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file list_test.cpp
/// \brief Basic regression test for list expressions.

#include <boost/test/minimal.hpp>

#include "mcrl2/data/list.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::data::sort_list;

template <typename Predicate>
void test_data_expression(const std::string& s, const mcrl2::data::variable_vector& v, Predicate p)
{
  data_expression e = parse_data_expression(s, v);
  BOOST_CHECK(p(e));
}

/* Test case for various list expressions, based
   on the following specification:

proc P(l: List(Nat)) = (1 in l) -> tau . P(10 |> l ++ [#l] <| 100)
                     + (l.1 == 30) -> tau . P(tail(l))
                     + (head(l) == 20) -> tau . P([rhead(l)] ++ rtail(l));

init P([20, 30, 40]);
*/
void list_expression_test()
{
  data::data_specification specification;

  specification.add_context_sort(sort_list::list(sort_pos::pos()));

  data::rewriter normaliser(specification);

  variable_vector v;
  v.push_back(parse_variable("l:List(Nat)"));

  test_data_expression("1 in l", v, is_in_application);
  test_data_expression("10 |> l", v, is_cons_application);
  test_data_expression("l <| 10", v, is_snoc_application);
  test_data_expression("#l", v, is_count_application);
  test_data_expression("l ++ [10]", v, is_concat_application);
  test_data_expression("l.1", v, is_element_at_application);
  test_data_expression("head(l)", v, is_head_application);
  test_data_expression("rhead(l)", v, is_rhead_application);
  test_data_expression("tail(l)", v, is_tail_application);
  test_data_expression("rtail(l)", v, is_rtail_application);

  data_expression e = parse_data_expression("[10]", v);
  BOOST_CHECK(is_cons_application(normaliser(e)));
}

int test_main(int argc, char** argv)
{
  list_expression_test();

  return EXIT_SUCCESS;
}


#include <boost/assign/std/vector.hpp>
#include <cradle/common.hpp>
#include <cradle/geometry/common.hpp>
#include <cradle/geometry/transformations.hpp>
#include <cradle/math/common.hpp>
#include <vector>

#include <cradle/test.hpp>

using namespace cradle;

TEST_CASE("sizeof_vector")
{
    // Check that vectors don't have any size overhead.
    REQUIRE(sizeof(vector<3, int>) == 3 * sizeof(int));
    REQUIRE(sizeof(vector<1, float>) == sizeof(float));
    REQUIRE(sizeof(vector<2, double>) == 2 * sizeof(double));
}

void
f(vector3i const& v)
{
}

// confirm that the vector operators work as expected
TEST_CASE("vector_operators")
{
    vector<3, int> p = make_vector<int>(1, 1, 0),
                   q = make_vector<int>(4, 2, 6);
    vector<3, int> v = make_vector<int>(3, 1, 2);

    REQUIRE(p - q == make_vector<int>(-3, -1, -6));
    REQUIRE(p + v == make_vector<int>(4, 2, 2));

    REQUIRE(v * 3 == make_vector<int>(9, 3, 6));
    REQUIRE(v / 2 == make_vector<int>(1, 0, 1));

    REQUIRE(!(p == q));
    REQUIRE(p != q);

    REQUIRE(!(p - q == v));
    REQUIRE(p - q != v);
}

TEST_CASE("vector_slice")
{
    {
        vector3i p = make_vector<int>(6, 7, 8);
        REQUIRE(slice(p, 0) == make_vector<int>(7, 8));
        REQUIRE(slice(p, 1) == make_vector<int>(6, 8));
        REQUIRE(slice(p, 2) == make_vector<int>(6, 7));
    }

    {
        vector2f p = make_vector<float>(9, 17);
        REQUIRE(slice(p, 0) == make_vector<float>(17));
        REQUIRE(slice(p, 1) == make_vector<float>(9));
    }

    {
        vector4i p = make_vector<int>(4, 3, 2, 1);
        REQUIRE(slice(p, 0) == make_vector<int>(3, 2, 1));
        REQUIRE(slice(p, 1) == make_vector<int>(4, 2, 1));
        REQUIRE(slice(p, 2) == make_vector<int>(4, 3, 1));
        REQUIRE(slice(p, 3) == make_vector<int>(4, 3, 2));
    }
}

TEST_CASE("test_unslice_vector")
{
    {
        vector3i p = make_vector<int>(6, 7, 8);
        REQUIRE(unslice(p, 0, 0) == make_vector<int>(0, 6, 7, 8));
        REQUIRE(unslice(p, 1, 0) == make_vector<int>(6, 0, 7, 8));
        REQUIRE(unslice(p, 2, 0) == make_vector<int>(6, 7, 0, 8));
        REQUIRE(unslice(p, 3, 0) == make_vector<int>(6, 7, 8, 0));
    }

    {
        vector2d p = make_vector<double>(9, 17);
        REQUIRE(unslice(p, 0, 2.1) == make_vector<double>(2.1, 9, 17));
        REQUIRE(unslice(p, 1, 2.1) == make_vector<double>(9, 2.1, 17));
        REQUIRE(unslice(p, 2, 2.1) == make_vector<double>(9, 17, 2.1));
    }
}

TEST_CASE("test_uniform_vector")
{
    REQUIRE((uniform_vector<3, int>(0)) == make_vector<int>(0, 0, 0));
    REQUIRE(
        (uniform_vector<4, unsigned>(1)) == make_vector<unsigned>(1, 1, 1, 1));
    REQUIRE((uniform_vector<2, float>(6)) == make_vector<float>(6, 6));
}

TEST_CASE("vector_almost_equal")
{
    REQUIRE(almost_equal(
        make_vector<double>(0, 0, 0),
        make_vector<double>(0, 0, default_equality_tolerance<double>() / 2)));
    REQUIRE(!almost_equal(
        make_vector<float>(0, 0, 0), make_vector<float>(0, 0, 1)));
    REQUIRE(almost_equal(
        make_vector<float>(0, 0, 0), make_vector<float>(0, 0, 1), 2));
}

TEST_CASE("vector_cross")
{
    // with objects
    REQUIRE(almost_equal(
        cross(make_vector<double>(1, 0, 0), make_vector<double>(0, 1, 0)),
        make_vector<double>(0, 0, 1)));
    REQUIRE(almost_equal(
        cross(make_vector<double>(0, 1, 0), make_vector<double>(1, 0, 0)),
        make_vector<double>(0, 0, -1)));

    // with expressions
    REQUIRE(almost_equal(
        cross(
            make_vector<double>(1, 0, 0) - make_vector<double>(0, 0, 0),
            make_vector<double>(0, 1, 0) - make_vector<double>(0, 0, 0)),
        make_vector<double>(0, 0, 1)));

    // mixing expressions and objects
    REQUIRE(almost_equal(
        cross(
            make_vector<double>(1, 0, 0) - make_vector<double>(0, 0, 0),
            make_vector<double>(0, 1, 0)),
        make_vector<double>(0, 0, 1)));
    REQUIRE(almost_equal(
        cross(
            make_vector<double>(1, 0, 0),
            make_vector<double>(0, 1, 0) - make_vector<double>(0, 0, 0)),
        make_vector<double>(0, 0, 1)));
}

TEST_CASE("vector_dot")
{
    // with objects
    REQUIRE(almost_equal(
        dot(make_vector<double>(1, 1), make_vector<double>(0.7, 0.3)), 1.));
    REQUIRE(almost_equal(
        dot(make_vector<double>(1, 0, 0), make_vector<double>(0, 1, 0)), 0.));
    REQUIRE(almost_equal(
        dot(make_vector<double>(1), make_vector<double>(0.6)), 0.6));
    REQUIRE(dot(make_vector<int>(1, 2, 0), make_vector<int>(2, 3, 0)) == 8);

    // with expressions
    REQUIRE(almost_equal(
        dot(make_vector<double>(1, 0, 1) - make_vector<double>(0, 0, 0),
            make_vector<double>(0.7, 0, 0.3) - make_vector<double>(0, 0, 0)),
        1.));

    // mixing expressions and objects
    REQUIRE(almost_equal(
        dot(make_vector<float>(1, 1),
            make_vector<float>(0.7f, 0.3f) - make_vector<float>(0, 0)),
        1.f));
    REQUIRE(almost_equal(
        dot(make_vector<double>(1, 0, 1) - make_vector<double>(0, 0, 0),
            make_vector<double>(0.7, 0, 0.3)),
        1.));
}

TEST_CASE("vector_length")
{
    // length2()
    REQUIRE(length2(make_vector<int>(2, 0, 1)) == 5);

    // length()
    REQUIRE(almost_equal(length(make_vector<double>(2, 1)), sqrt(5.)));

    // length() with an expression
    REQUIRE(almost_equal(
        length(make_vector<double>(2, 0, 1) - make_vector<double>(1, 0, 0)),
        sqrt(2.)));
}

TEST_CASE("unit_vector")
{
    REQUIRE(almost_equal(
        unit(make_vector<double>(4, 0, 3)), make_vector<double>(0.8, 0, 0.6)));

    // with an expression
    REQUIRE(almost_equal(
        unit(make_vector<double>(3, 0) - make_vector<double>(0, 4)),
        make_vector<double>(0.6, -0.8)));
}

TEST_CASE("perpendicular_vector")
{
    for (int x = -1; x < 2; ++x)
    {
        for (int y = -1; y < 2; ++y)
        {
            for (int z = -1; z < 2; ++z)
            {
                if (x != 0 || y != 0 || z != 0)
                {
                    vector<3, double> v = make_vector<double>(x, y, z);
                    CRADLE_CHECK_ALMOST_EQUAL(
                        dot(v, get_perpendicular(v)), 0.);
                    CRADLE_CHECK_ALMOST_EQUAL(
                        length(get_perpendicular(v)), 1.);
                }
            }
        }
    }
}

TEST_CASE("product_test")
{
    REQUIRE(product(make_vector<int>(2, 3, 1)) == 6);
    REQUIRE(product(make_vector<int>(2, -1, 3, 1)) == -6);

    REQUIRE(almost_equal(product(make_vector<float>(2.5, 4, 2)), 20.f));
    REQUIRE(almost_equal(product(make_vector<double>(2.5, 4)), 10.));

    // with an expression
    REQUIRE(
        product(make_vector<int>(2, -1, 3, 0) - make_vector<int>(6, 0, 0, 1))
        == -12);
}

TEST_CASE("vector_io")
{
    vector3i p = make_vector<int>(2, 0, 3);
    REQUIRE(to_string(p) == "(2, 0, 3)");
}

TEST_CASE("compute_mean_vector_test")
{
    using namespace boost::assign;

    std::vector<vector3d> vectors;
    vectors += make_vector<double>(2, 0, 3), make_vector<double>(6, 1, 7),
        make_vector<double>(0, 0, 0), make_vector<double>(1, 2, 0),
        make_vector<double>(3, 2, 1), make_vector<double>(6, 4, 1);

    REQUIRE(almost_equal(
        compute_mean(vectors, uniform_vector<3>(0.)),
        make_vector<double>(3, 1.5, 2)));
}

TEST_CASE("plane_test")
{
    plane<double> default_constructed;

    vector3d p = make_vector<double>(0, 0, 0);
    vector3d normal = make_vector<double>(1, 0, 0);

    plane<double> plane(p, normal);
    REQUIRE(plane.point == p);
    REQUIRE(plane.normal == normal);

    vector3d q = make_vector<double>(0, 0, 1);
    plane.point = q;
    REQUIRE(plane.point == q);
    REQUIRE(plane.normal == normal);

    normal = make_vector<double>(0, 1, 0);
    plane.normal = normal;
    REQUIRE(plane.point == q);
    REQUIRE(plane.normal == normal);
}

TEST_CASE("simple_box1i_test")
{
    box1i b(make_vector<int>(-1), make_vector<int>(4));

    REQUIRE(get_center(b)[0] == 1);
    REQUIRE(b.corner[0] == -1);
    REQUIRE(b.size[0] == 4);

    REQUIRE(!is_inside(b, make_vector<int>(-2)));
    REQUIRE(is_inside(b, make_vector<int>(-1)));
    REQUIRE(is_inside(b, make_vector<int>(2)));
    REQUIRE(!is_inside(b, make_vector<int>(3)));
    REQUIRE(!is_inside(b, make_vector<int>(4)));
}

TEST_CASE("simple_box1d_test")
{
    box1d b(make_vector<double>(-1), make_vector<double>(3));

    CRADLE_CHECK_ALMOST_EQUAL(get_center(b), make_vector<double>(0.5));
    CRADLE_CHECK_ALMOST_EQUAL(b.corner, make_vector<double>(-1));
    CRADLE_CHECK_ALMOST_EQUAL(b.size, make_vector<double>(3));

    REQUIRE(!is_inside(b, make_vector<double>(-2)));
    REQUIRE(is_inside(b, make_vector<double>(-1)));
    REQUIRE(is_inside(b, make_vector<double>(0)));
    REQUIRE(is_inside(b, make_vector<double>(1)));
    REQUIRE(is_inside(b, make_vector<double>(1.5)));
    REQUIRE(is_inside(b, make_vector<double>(1.9)));
    REQUIRE(!is_inside(b, make_vector<double>(2)));
    REQUIRE(!is_inside(b, make_vector<double>(4)));
}

TEST_CASE("simple_box2d_test")
{
    box2d b(make_vector<double>(-1, -1), make_vector<double>(3, 3));

    CRADLE_CHECK_ALMOST_EQUAL(area(b), 9.);

    CRADLE_CHECK_ALMOST_EQUAL(get_center(b), make_vector<double>(0.5, 0.5));
    CRADLE_CHECK_ALMOST_EQUAL(b.corner, make_vector<double>(-1, -1));
    CRADLE_CHECK_ALMOST_EQUAL(b.size, make_vector<double>(3, 3));

    REQUIRE(!is_inside(b, make_vector<double>(-2, -2)));
    REQUIRE(!is_inside(b, make_vector<double>(-2, 0)));
    REQUIRE(!is_inside(b, make_vector<double>(0, 4)));
    REQUIRE(!is_inside(b, make_vector<double>(0, 2)));
    REQUIRE(is_inside(b, make_vector<double>(-1, -1)));
    REQUIRE(is_inside(b, make_vector<double>(0, 1.9)));
    REQUIRE(is_inside(b, make_vector<double>(0, 0)));
    REQUIRE(is_inside(b, make_vector<double>(1.5, 1.5)));
    REQUIRE(is_inside(b, make_vector<double>(0, 1)));
}

TEST_CASE("box_slicing_test")
{
    REQUIRE(
        slice(
            box3d(make_vector<double>(0, 2, 1), make_vector<double>(4, 3, 5)),
            0)
        == box2d(make_vector<double>(2, 1), make_vector<double>(3, 5)));
    REQUIRE(
        slice(
            box3d(make_vector<double>(0, 2, 1), make_vector<double>(4, 3, 5)),
            1)
        == box2d(make_vector<double>(0, 1), make_vector<double>(4, 5)));
    REQUIRE(
        slice(
            box3d(make_vector<double>(0, 2, 1), make_vector<double>(4, 3, 5)),
            2)
        == box2d(make_vector<double>(0, 2), make_vector<double>(4, 3)));

    REQUIRE(
        slice(box2i(make_vector<int>(0, 2), make_vector<int>(4, 3)), 0)
        == box1i(make_vector<int>(2), make_vector<int>(3)));
    REQUIRE(
        slice(box2i(make_vector<int>(0, 2), make_vector<int>(4, 3)), 1)
        == box1i(make_vector<int>(0), make_vector<int>(4)));
}

TEST_CASE("add_box_border_test")
{
    REQUIRE(
        add_border(
            box3i(make_vector<int>(0, 2, 1), make_vector<int>(4, 3, 5)), 2)
        == box3i(make_vector<int>(-2, 0, -1), make_vector<int>(8, 7, 9)));

    REQUIRE(
        add_border(
            box3i(make_vector<int>(0, 2, 1), make_vector<int>(4, 3, 5)),
            make_vector<int>(2, 1, 0))
        == box3i(make_vector<int>(-2, 1, 1), make_vector<int>(8, 5, 5)));
}

TEST_CASE("simple_vector_test")
{
    circle<double> c(make_vector<double>(0, 0), 1);
    CRADLE_CHECK_ALMOST_EQUAL(area(c), pi);
    REQUIRE(!is_inside(c, make_vector<double>(0, 2)));
    REQUIRE(!is_inside(c, make_vector<double>(2, 0)));
    REQUIRE(!is_inside(c, make_vector<double>(1.1, 0)));
    REQUIRE(!is_inside(c, make_vector<double>(0.9, 0.9)));
    REQUIRE(!is_inside(c, make_vector<double>(0, -1.1)));
    REQUIRE(is_inside(c, make_vector<double>(0.9, 0)));
    REQUIRE(is_inside(c, make_vector<double>(0, 0)));
    REQUIRE(is_inside(c, make_vector<double>(0, -0.9)));
    REQUIRE(is_inside(c, make_vector<double>(-0.7, 0.7)));
    REQUIRE(is_inside(c, make_vector<double>(0.3, 0.5)));
}

TEST_CASE("off_center_test")
{
    circle<double> c(make_vector<double>(4, 1), 2);
    CRADLE_CHECK_ALMOST_EQUAL(area(c), 4 * pi);
    REQUIRE(!is_inside(c, make_vector<double>(0, 0)));
    REQUIRE(!is_inside(c, make_vector<double>(1.9, 1)));
    REQUIRE(!is_inside(c, make_vector<double>(6.1, 1)));
    REQUIRE(!is_inside(c, make_vector<double>(4, 3.1)));
    REQUIRE(!is_inside(c, make_vector<double>(4, -1.1)));
    REQUIRE(is_inside(c, make_vector<double>(4, 1)));
    REQUIRE(is_inside(c, make_vector<double>(2.6, 2.4)));
}

TEST_CASE("by_value_test")
{
    vector2d p0 = make_vector<double>(0, 1), p1 = make_vector<double>(4, 4);
    line_segment<2, double> segment(p0, p1);

    REQUIRE(segment[0] == p0);
    REQUIRE(segment[1] == p1);
    CRADLE_CHECK_ALMOST_EQUAL(length(segment), 5.);
}

TEST_CASE("identity_matrix_test")
{
    REQUIRE(
        (identity_matrix<4, double>())
        == make_matrix<double>(
            1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1));

    REQUIRE(
        (identity_matrix<3, double>())
        == make_matrix<double>(1, 0, 0, 0, 1, 0, 0, 0, 1));
}

TEST_CASE("matrix_operations_test")
{
    matrix<3, 3, double> m, i = identity_matrix<3, double>();

    m = i - 2 * i;
    REQUIRE(m == make_matrix<double>(-1, 0, 0, 0, -1, 0, 0, 0, -1));

    m *= 2;
    REQUIRE(m == make_matrix<double>(-2, 0, 0, 0, -2, 0, 0, 0, -2));

    m = i;
    m /= 2;
    REQUIRE(m == make_matrix<double>(0.5, 0, 0, 0, 0.5, 0, 0, 0, 0.5));

    m += i * 3;
    REQUIRE(m == make_matrix<double>(3.5, 0, 0, 0, 3.5, 0, 0, 0, 3.5));

    m -= 2 * i;
    REQUIRE(m == make_matrix<double>(1.5, 0, 0, 0, 1.5, 0, 0, 0, 1.5));

    REQUIRE(m == m);
    REQUIRE(m != i);
}

TEST_CASE("matrix_conversion_test")
{
    matrix<3, 3, double> m(identity_matrix<3, float>());
}

TEST_CASE("matrix_inverse3_test")
{
    matrix<4, 4, double> m
        = translation(make_vector<double>(4, 3, 7))
          * scaling_transformation(make_vector<double>(.1, 2, 1.2))
          * rotation_about_x(angle<double, degrees>(90));

    matrix<4, 4, double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(
            inv_m, transform_point(m, make_vector<double>(0, 0, 0))),
        make_vector<double>(0, 0, 0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(
            inv_m, transform_point(m, make_vector<double>(2, 1, 7))),
        make_vector<double>(2, 1, 7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(
            inv_m, transform_point(m, make_vector<double>(1, 0, 17))),
        make_vector<double>(1, 0, 17));
}

TEST_CASE("matrix_inverse2_test")
{
    matrix<3, 3, double> m
        = translation(make_vector<double>(3, 7))
          * scaling_transformation(make_vector<double>(.1, 1.2))
          * rotation(angle<double, degrees>(90));

    matrix<3, 3, double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0, 0))),
        make_vector<double>(0, 0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(1, 7))),
        make_vector<double>(1, 7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0, 17))),
        make_vector<double>(0, 17));
}

TEST_CASE("matrix_inverse1_test")
{
    matrix<2, 2, double> m = translation(make_vector<double>(1))
                             * scaling_transformation(make_vector<double>(.1));

    matrix<2, 2, double> inv_m = inverse(m);

    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(0))),
        make_vector<double>(0));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(7))),
        make_vector<double>(7));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(17))),
        make_vector<double>(17));
    CRADLE_CHECK_ALMOST_EQUAL(
        transform_point(inv_m, transform_point(m, make_vector<double>(1))),
        make_vector<double>(1));
}


#ifndef LAYER_NN
#define LAYER_NN

#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

class layer {
    public:
        /*---- Linked List ---*/
        layer   *next;
        layer   *prev;

        /*--- General Parameters ---*/
        string  activation;
        int     layerNumber;
        int     input_size, output_size;

        /*--- Specific Parameters ---*/
        mat     W, dW;
        mat     B, dB;

        /*--- Input & Output ---*/
        mat     Z, Y, dL;

        /* METHODS */
        layer();

        void    feed(mat);
        void    back(mat,mat);
        void    gradient_descent(float);

};

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file Sampler.cpp
 * @author Alex Cunningham
 */

#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <gtsam/linear/Sampler.h>
namespace gtsam {

/* ************************************************************************* */
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
	: model_(model), generator_(static_cast<unsigned>(seed))
{
}

/* ************************************************************************* */
Sampler::Sampler(const Vector& sigmas, int32_t seed)
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
{
}

/* ************************************************************************* */
Sampler::Sampler(int32_t seed)
: generator_(static_cast<unsigned>(seed))
{
}

/* ************************************************************************* */
Vector Sampler::sampleDiagonal(const Vector& sigmas) {
	size_t d = sigmas.size();
	Vector result(d);
	for (size_t i = 0; i < d; i++) {
		double sigma = sigmas(i);

		// handle constrained case separately
		if (sigma == 0.0) {
			result(i) = 0.0;
		} else {
			typedef boost::normal_distribution<double> Normal;
			Normal dist(0.0, sigma);
			boost::variate_generator<boost::minstd_rand&, Normal> norm(generator_, dist);
			result(i) = norm();
		}
	}
	return result;
}

/* ************************************************************************* */
Vector Sampler::sample() {
	assert(model_.get());
	const Vector& sigmas = model_->sigmas();
	return sampleDiagonal(sigmas);
}

/* ************************************************************************* */
Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
	assert(model.get());
	const Vector& sigmas = model->sigmas();
	return sampleDiagonal(sigmas);
}
/* ************************************************************************* */

} // \namespace gtsam


#ifndef DENSEFACTORIZATIONFACTORY_HPP
#define DENSEFACTORIZATIONFACTORY_HPP

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

template <class serialType>
class denseFactorizationFactory
{
public:

  denseFactorizationFactory();

  denseFactorizationFactory(serialType &mat);

  void reset(serialType &mat);

  void BDCSVD();

  void HouseholderQR();

  void PartialPivLU();

  serialType getU() { return U; }

  serialType getSingularValues() { return S; }

  serialType getV() { return V; }

  serialType getQ() { return Q; }

private:

  serialType *dense_matrix_ = NULL;

  // containers to store factorizations
  serialType U;
  serialType S;
  serialType V;
  serialType Q;

};

#include "denseFactorizationFactory_impl.hpp"

#endif /*DENSEFACTORIZATIONFACTORY_HPP*/


// std::function allows us to save a function object as a member of any class
// and to use a function between separate compilation units
#include <boost/phoenix.hpp>
#include <cmath>
#include <functional>
#include <iomanip>
#include <iostream>
#include <string>

int main()
{
    using namespace std::string_literals;
    using namespace boost::phoenix::arg_names;

    auto testFunction = std::function<float(float, float)>{};

    float a = 3;
    float b = -4.2;
    float x = 2;

    // TODO: explore why it fails
    // ordinary function
    // testFunction = std::fmax;
    // std::cout << "max of " << a << " and " << b
    //           << ": " << testFunction(a, b) << '\n';

    // class with a call operator
    testFunction = std::multiplies<float>();
    std::cout << "multiplication of " << a << " and " << b
              << ": " << testFunction(a, b) << '\n';

    // class with a generic call operator
    testFunction = std::multiplies<>();
    std::cout << "multiplication of " << a << " and " << b
              << ": " << testFunction(a, b) << '\n';

    // lambda
    testFunction = [x](float a, float b) { return a * x + b; };
    std::cout << "lambda result: " << testFunction(a, b) << '\n';

    // generic lambda
    testFunction = [x](auto a, auto b) { return a * x + b; };
    std::cout << "generic lambda result: " << testFunction(a, b) << '\n';

    // boost.phoenix expression
    testFunction = (arg1 + arg2) / 2;
    std::cout << "boost.phoenix expression result: "
              << testFunction(a, b) << '\n';

    // use member function as a function object
    auto s = "A small pond"s;
    std::function<bool(std::string)> f = &std::string::empty;

    std::cout << std::boolalpha;
    std::cout << "'" << s  << "'" << " is empty: " << f(s) << '\n';
}


#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <iostream>

using Length_unit = boost::units::si::length;
using Time_unit = boost::units::si::time;
using Velocity_unit = boost::units::si::velocity;
using Length = boost::units::quantity<Length_unit>;
using Time = boost::units::quantity<Time_unit>;
using Velocity = boost::units::quantity<Velocity_unit>;

Velocity compute_velocity(const Length dx, const Time dt) {
    return dt/dx;
}

int main() {
    // define units to make formulas easier to read
    Length_unit m {boost::units::si::meter};
    Time_unit s {boost::units::si::seconds};

    Length distance {3.5*m};
    Time time {2.0*s};
    Velocity velocity = compute_velocity(distance, time);
    std::cout << velocity << std::endl;
    return 0;
}


// Copyright (C) 2009-2012 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0
// (see accompanying file LICENSE_1_0.txt or a copy at
// http://www.boost.org/LICENSE_1_0.txt)
// Home at http://www.boost.org/libs/local_function

#include <boost/local_function.hpp>
#include <boost/function.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function.hpp>
#include <boost/detail/lightweight_test.hpp>

//[phoenix_factorial_local
int main(void) {
    using boost::phoenix::arg_names::arg1;

    int BOOST_LOCAL_FUNCTION(int n) { // Unfortunately, monomorphic.
        return (n <= 0) ? 1 : n * factorial_impl(n - 1);
    } BOOST_LOCAL_FUNCTION_NAME(recursive factorial_impl)

    boost::phoenix::function< boost::function<int (int)> >
            factorial(factorial_impl); // Phoenix function from local function.

    int i = 4;
    BOOST_TEST(factorial(i)() == 24);      // Call.
    BOOST_TEST(factorial(arg1)(i) == 24);  // Lazy call.
    return boost::report_errors();
}
//]


#pragma once

#ifdef PY_BINDINGS
    #include <pybind11/pybind11.h>
    #include <pybind11/stl.h>
    #include <pybind11/iostream.h>
    #include <pybind11/functional.h>
    namespace py = pybind11;
#endif

// #include <boost/multiprecision/gmp.hpp>
// using BigInt = boost::multiprecision::mpz_int;
// namespace std {
//     inline BigInt abs(BigInt num) {
//         return boost::multiprecision::abs(num);
//     }
// }
// inline std::string toString(BigInt& num, int base = 10) {
//     num.str(base);
// }

#include <gmpxx.h>
using BigInt = mpz_class;
namespace std {
    inline BigInt abs(BigInt& num) {
        return ::abs(num);
    }
}

inline std::string toString(BigInt& num, int base = 10) {
    return num.get_str(base);
}

#ifdef PY_BINDINGS
    namespace pybind11 { namespace detail {
        template <> struct type_caster<BigInt> {
        public:
            /**
             * This macro establishes the name 'BigInt' in
             * function signatures and declares a local variable
             * 'value' of type BigInt
             */
            PYBIND11_TYPE_CASTER(BigInt, _("BigInt"));

            /**
             * Conversion part 1 (Python->C++): convert a PyObject into a BigInt
             * instance or return false upon failure. The second argument
             * indicates whether implicit conversions should be applied.
             */
            bool load(handle src, bool) {
                /* Extract PyObject from handle */
                PyObject *source = src.ptr();
                /* Try converting into a Python integer value */
                PyObject *pyNumber = nullptr;
                PyObject *pyString = nullptr;

                bool error = false;

                pyNumber = PyNumber_Long(source);
                if (!pyNumber) {
                    error = true;
                } else {
                    // using base 10 because boost multiprecision does not
                    // seem to support sign for base 16
                    pyString = PyNumber_ToBase(pyNumber,10);
                    if (!pyString) {
                        error = true;
                    } else {
                        const char* string = PyUnicode_AsUTF8(pyString);
                        if (string) {
                            try {
                                value = BigInt(string);
                            } catch (const std::runtime_error& e) {
                                error = true;
                            }
                        }

                    }

                }

                error |= (PyErr_Occurred() != nullptr);
                if (pyNumber)
                    Py_DECREF(pyNumber);
                if (pyString)
                    Py_DECREF(pyString);
                return !error;
            }

            /**
             * Conversion part 2 (C++ -> Python): convert an BigInt instance into
             * a Python object. The second and third arguments are used to
             * indicate the return value policy and parent object (for
             * ``return_value_policy::reference_internal``) and are generally
             * ignored by implicit casters.
             */
            static handle cast(BigInt src, return_value_policy /* policy */, handle /* parent */) {
                std::string string = toString(src,10);
                PyObject *pyString = PyUnicode_FromString(string.c_str());
                PyObject *result = PyNumber_Long(pyString);
                Py_DECREF(pyString);
                return result;
            }
        };
    }} // namespace pybind11::detail
#endif // ifdef PY_BINDINGS


#include "../src/streamingcc_include/hyper_loglog.h"
#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <random>
#include <map>
#include <iostream>
#include <vector>
#include <set>

using namespace streamingcc;
using namespace std;

BOOST_AUTO_TEST_CASE(util_Test) {
    hyper_loglog count;
    set<int> s;
    for(int i = 0; i < 100000; ++i)
    {
      int cur = rand() % 10000000;
      s.insert(cur);
      count.update(cur);
    }
    int e = count.estimate(), ans = s.size();
    BOOST_CHECK(e >= 0.1 * ans && e <= 10 * ans);
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-swar

    Shuffle the elements of a boost::simd::pack using an index permutation described by compile-time
    integral constants.

    @par Semantic:

    For any boost::simd::pack @c x of base type @c T and cardinal @c N and @c N compile-time
    integral constants @c I1...In with value comprised between @c -1 and @c N-1, the following code:

    @code
    boost::simd::pack<T,N> r = shuffle<I1,...,In>(x);
    @endcode

    is equivalent to:

    @code
    boost::simd::pack<T,N> r{ I1 != -1 ? x[I1] : 0, ..., In != -1 ? x[In] : 0 };
    @endcode

    The actual integral constants is mapped at compile-time to the optimal sequence of
    intrinsics to apply the desired permutation.

    The special index value @c -1 is used to specify that, instead of fetching a data from inside
    the shuffled boost::simd::pack, the value @c 0 has to be inserted in the result.

    @par Example:

    @snippet shuffle.unary.cpp shuffle-unary

    Possible output:

    @code
    Original: (1, 2, 3, 4)
    Permuted: (4, 0, 3, 1)
    @endcode

    @param  a  boost::simd::pack to shuffle
  **/
  template<int P0, int ... Ps, typename T>  T shuffle(T const& a);

  /*!
    @ingroup group-swar

    Shuffle the elements of two boost::simd::pack using an index permutation described by compile-time
    integral constants.

    @par Semantic:

    For any boost::simd::pack @c x and @c y of base type @c T and cardinal @c N and @c N
    compile-time integral constants @c I1...In with value comprised between @c -1 and @c 2*N-1,
    the following code:

    @code
    boost::simd::pack<T,N> r = shuffle<I1,...,In>(x,y);
    @endcode

    is equivalent to:

    @code
    boost::simd::pack<T,N> r{ I1 != -1 ? (I1<N ? x[I1] : y[I1-N]) : 0, ..., In != -1 ? (In<N ? x[In] : y[In-N]) : 0 };
    @endcode

    The actual integral constants is mapped at compile-time to the optimal sequence of
    intrinsics to apply the desired permutation.

    The special index value @c -1 is used to specify that, instead of fetching a data from inside
    the shuffled boost::simd::pack, the value @c 0 has to be inserted in the result.

    @par Example:

    @snippet shuffle.binary.cpp shuffle-binary

    Possible output:

    @code
    Original: (1, 2, 3, 4) (10, 20, 30, 40)
    Permuted: (0, 4, 40, 0)
    @endcode

    @param  a  boost::simd::pack to shuffle
    @param  b  boost::simd::pack to shuffle
  **/
  template<int P0, int ... Ps, typename T>  T shuffle(T const& a,T const& b);

  /*!
    @ingroup group-swar

    Shuffle the elements of a boost::simd::pack using an index permutation described by compile-time
    meta-function.

    @par Semantic:

    For any boost::simd::pack @c x of base type @c T and cardinal @c N and a meta-function @c Perm,
    the following code:

    @code
    boost::simd::pack<T,N> r = shuffle<Perm>(x);
    @endcode

    is equivalent to:

    @code
    boost::simd::pack<T,N> r = shuffle<Perm::apply<0,N>::value,...,Perm::apply<N-1,N>::value>(x);
    @endcode

    The permutation computed by the meta-function @c Perm is mapped at compile-time to the
    optimal sequence of intrinsics to apply the desired permutation.

    @par Defining a permutation meta-function

    Permutation meta-function can be built in two different ways:

    - Define a Permutation meta-function as a struct with an internal @c apply structure that
      proceed to compute a given permutation index at compile-time. This @c apply internal
      structure takes two integral constant types as parameter: @c C the cardinal of the
      boost::simd::pack to be shuffled and @c I the index of the permutation index computed.

    - Define a @c constexpr function taking two integers: @c c the cardinal of the boost::simd::pack
      to be shuffled and @c i the index of the permutation index computed. This function returns the
      computed value of the permutation index computed. Said function is then wrapped inside the
      boost::simd::pattern template type before being used with boost::simd::shuffle.

    The special index value @c -1 can be returned to specify that, instead of fetching a data from
    inside the shuffled boost::simd::pack, the value @c 0 has to be inserted in the result.

    @notebox{Using permutation expressed as a metafunction has the advantage to be cardinal agnostic,
    thus making a given shuffle calls independant of the actual pack cardinal, leading to a more
    generic code}

    @par Example:

    @snippet shuffle.perm.cpp shuffle-perm

    Possible output:

    @code
    Original: (1, 2, 3, 4)
    Permuted: (4, 0, 3, 1)
    Permuted: (4, 4, 4, 4)
    @endcode

    @tparam Permutation Permutation meta-function generating the permutation index
    @param  a           boost::simd::pack to shuffle
  **/
  template<typename Permutation, typename T>  T shuffle(T const& a);

  /*!
    @ingroup group-swar

    Shuffle the elements of two boost::simd::pack using an index permutation described by compile-time
    meta-function.

    @par Semantic:

    For any boost::simd::pack @c x and @c y of base type @c T and cardinal @c N and a meta-function @c Perm,
    the following code:

    @code
    boost::simd::pack<T,N> r = shuffle<Perm>(x,y);
    @endcode

    is equivalent to:

    @code
    boost::simd::pack<T,N> r = shuffle<Perm::apply<0,N>::value,...,Perm::apply<N-1,N>::value>(x,y);
    @endcode

    The permutation computed by the meta-function @c Perm is mapped at compile-time to the
    optimal sequence of intrinsics to apply the desired permutation.

    @par Defining a permutation meta-function

    Permutation meta-function can be built in two different ways:

    - Define a Permutation meta-function as a struct with an internal @c apply structure that
      proceed to compute a given permutation index at compile-time. This @c apply internal
      structure takes two integral constant types as parameter: @c C the cardinal of the
      boost::simd::pack to be shuffled and @c I the index of the permutation index computed.

    - Define a @c constexpr function taking two integers: @c c the cardinal of the boost::simd::pack
      to be shuffled and @c i the index of the permutation index computed. This function returns the
      computed value of the permutation index computed. Said function is then wrapped inside the
      boost::simd::pattern template type before being used with boost::simd::shuffle.

    The special index value @c -1 can be returned to specify that, instead of fetching a data from
    inside the shuffled boost::simd::pack, the value @c 0 has to be inserted in the result.

    @notebox{Using permutation expressed as a metafunction has the advantage to be cardinal agnostic,
    thus making a given shuffle calls independant of the actual pack cardinal, leading to a more
    generic code}

    @par Example:

    @snippet shuffle.perm2.cpp shuffle-perm2

    Possible output:

    @code
    Original: (1, 2, 3, 4)
    Permuted: (10, 20, 3, 4)
    @endcode

    @tparam Permutation Permutation meta-function generating the permutation index
    @param  a           boost::simd::pack to shuffle
    @param  b           boost::simd::pack to shuffle
  **/
  template<typename Permutation, typename T>  T shuffle(T const& a,T const& b);
} }
#endif

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

#endif