// Copyright (c) 2018 Steven Watanabe
//
// Distributed under the Boost Software License Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS2_DIMENSIONS_HPP_INCLUDED
#define BOOST_UNITS2_DIMENSIONS_HPP_INCLUDED

#include <boost/units2/def.hpp>
#include <boost/units2/unit.hpp>

namespace boost {
namespace units2 {

BOOST_UNITS2_DEF(length);
BOOST_UNITS2_DEF(mass);
BOOST_UNITS2_DEF(time);
BOOST_UNITS2_DEF(temperature);
BOOST_UNITS2_DEF(amount);
BOOST_UNITS2_DEF(current);
BOOST_UNITS2_DEF(luminous_intensity);
BOOST_UNITS2_DEF(angle);
BOOST_UNITS2_DEF(solid_angle);

inline constexpr const auto velocity = length/time;
inline constexpr const auto acceleration = velocity/time;
inline constexpr const auto force = mass*acceleration;
inline constexpr const auto energy = force*length;

}
}

#endif


#include <boost/test/unit_test.hpp>

#include "../test_utils.hh"
#include "../fixture.hh"
#include "scylla_blas/queue/worker_proc.hh"
#include "../vector_utils.hh"

BOOST_FIXTURE_TEST_CASE(float_vector_scale_IT, vector_fixture)
{
    // Given vector of 4 floats
    std::vector<float> vals = {1.6f, 2.9999f, 3.0f, 0.0f};
    auto vector = getScyllaVectorOf(test_const::float_vector_1_id, vals);

    // When performing scaling by 2
    scheduler->sscal(2, *vector);

    // Then result vector is scaled by 2.
    std::vector<float> vals2 = {1.6f * 2, 2.9999f * 2, 3.0f * 2, 0.0f * 2};
    std::optional<scylla_blas::vector_value<float>> difference = cmp_vector(*vector, vals2);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                vals2[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(double_vector_scale_IT, vector_fixture)
{
    // Given vector of 4 doubles
    std::vector<double> vals = {1.6, 2.999999, 3.0, 3.141592653589793238462643383};
    auto vector = getScyllaVectorOf(test_const::double_vector_1_id, vals);

    // When performing scaling by 59.49
    const double alpha = 59.05;
    scheduler->dscal(alpha, *vector);

    // Then result vector is scaled by 59.49.
    std::vector<double> vals2 = {
            1.6 * alpha,
            2.999999 * alpha,
            3.0 * alpha,
            3.141592653589793238462643383 * alpha};
    std::optional<scylla_blas::vector_value<double>> difference = cmp_vector(*vector, vals2);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                vals2[difference->index - 1]));
    }
}


/** \file QuantumChannel.cc
 *  \todo clean Clean code when it is simple.
 *  \todo doc Write doxygen documentation.
 *  \authors takaakimatsuo
 *
 *  \brief QuantumChannel
 */
#include <omnetpp.h>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
//#include <Eigen/Dense>
#include <PhotonicQubit_m.h>

using namespace Eigen;
using namespace omnetpp;
using namespace quisp::messages;

namespace quisp {
namespace channels {

/*The sum of Z,X and Y error rate equates to pauli_error_rate. Value could potentially between 0 ~ 1. */
struct channel_error_model {
  double pauli_error_rate;  // Overall error rate
  double Z_error_rate;
  double X_error_rate;
  double Y_error_rate;
};

/** \class QuantumChannel QuantumChannel.cc
 *  \todo Documentation of the class header.
 *
 *  \brief QuantumChannel
 */
class QuantumChannel : public cDatarateChannel {
 public:
  channel_error_model err;
  double photon_loss_rate;
  double distance = 0;  // in km
  // int less = 0, more = 0;
 private:
  double No_error_ceil;
  double X_error_ceil;
  double Y_error_ceil;
  double Z_error_ceil;
  double Lost_ceil;
  int DEBUG_darkcount_count = 0;
  MatrixXd Q_to_the_distance;
  virtual void initialize();
  virtual void processMessage(cMessage *msg, simtime_t t, result_t &result);

 public:
  QuantumChannel();
};

Define_Channel(QuantumChannel)

    QuantumChannel::QuantumChannel() {}

void QuantumChannel::initialize() {
  cDatarateChannel::initialize();
  Q_to_the_distance(5, 5);
  distance = par("distance");  // in km

  /*double Z_error_ratio = par("Z_error_ratio");//par("name") will be read from .ini or .ned file
  double X_error_ratio = par("X_error_ratio");
  double Y_error_ratio = par("Y_error_ratio");
  double Loss_error_ratio = par("photon_loss_ratio");
  if(Z_error_ratio==0 && X_error_ratio==0 && Y_error_ratio==0 && Loss_error_ratio==0){
      Z_error_ratio=1;//To avoid bug.
      X_error_ratio=1;
      Y_error_ratio=1;
      Loss_error_ratio=1;
  }

  double ratio_sum = Z_error_ratio + X_error_ratio + Y_error_ratio + Loss_error_ratio;//Get the sum of x:y:z for normalization
  err.pauli_error_rate = par("channel_error_rate");//This is per km.
  err.X_error_rate = err.pauli_error_rate * (X_error_ratio/ratio_sum);
  err.Y_error_rate = err.pauli_error_rate * (Y_error_ratio/ratio_sum);
  err.Z_error_rate = err.pauli_error_rate * (Z_error_ratio/ratio_sum);
  photon_loss_rate = err.pauli_error_rate * (Loss_error_ratio/ratio_sum);//Photon Loss rate per km.
  */

  photon_loss_rate = par("channel_Loss_error_rate");
  err.X_error_rate = par("channel_X_error_rate");
  err.Y_error_rate = par("channel_Y_error_rate");
  err.Z_error_rate = par("channel_Z_error_rate");
  err.pauli_error_rate = err.X_error_rate + err.Y_error_rate + err.Z_error_rate + photon_loss_rate;

  /*
  int num_err_type = 0;
  if(err.X_error_rate !=0){
      num_err_type++;
  }
  if(err.Z_error_rate !=0){
      num_err_type++;
  }
  if(err.Y_error_rate !=0){
      num_err_type++;
  }

  if((1-err.pauli_error_rate) < double(1)/double(num_err_type)){
      //error("Error rate inaccurate.");
      std::cout<<"Inaccurate error rate \n";
  }*/

  // std::cout<<"Sum of errors must be ... = "<<err.X_error_rate+err.Y_error_rate+err.Z_error_rate+photon_loss_rate<<"\n";
  // std::cout<<"Channel err:"<<err.pauli_error_rate<<" X = " <<err.X_error_rate << "Y = "<< err.Y_error_rate << ", Z = "<< err.Z_error_rate<<",Loss"<<photon_loss_rate<<"\n";
  MatrixXd Transition_matrix(5, 5);

  Transition_matrix << 1 - err.pauli_error_rate, err.X_error_rate, err.Z_error_rate, err.Y_error_rate, photon_loss_rate, err.X_error_rate, 1 - err.pauli_error_rate,
      err.Y_error_rate, err.Z_error_rate, photon_loss_rate, err.Z_error_rate, err.Y_error_rate, 1 - err.pauli_error_rate, err.X_error_rate, photon_loss_rate, err.Y_error_rate,
      err.Z_error_rate, err.X_error_rate, 1 - err.pauli_error_rate, photon_loss_rate, 0, 0, 0, 0, 1;

  std::cout << "Transition mat per km = \n" << Transition_matrix << "\n";
  MatrixPower<MatrixXd> Apow(Transition_matrix);
  Q_to_the_distance = Apow(distance);
  std::cout << "Transition mat = " << Q_to_the_distance << "\n";

  // std::cout<<"\nNo_error_ceil = "<<No_error_ceil<<", X_error_ceil = "<< X_error_ceil << ", Z_error_ceil"<<Z_error_ceil<<", Y_error_ceil"<<Y_error_ceil<<" pauli err rate is
  // "<<err.pauli_error_rate<<"\n"; std::cout<<" 1-err.pauli_error_rate"
  // <<1-err.pauli_error_rate<<"err.X_error_rate"<<err.X_error_rate<<"err.Z_error_rate"<<err.Z_error_rate<<"err.Y_error_rate"<<err.Y_error_rate<<"photon_loss_rate"<<photon_loss_rate<<"\n";
}

void QuantumChannel::processMessage(cMessage *msg, simtime_t t, result_t &result) {
  cDatarateChannel::processMessage(msg, t, result);  // Call the original processMessage

  try {
    PhotonicQubit *q = check_and_cast<PhotonicQubit *>(msg);

    bool lost = q->getPhotonLost();
    bool Zerr = q->getPauliZerr();
    bool Xerr = q->getPauliXerr();

    // The photon may have an error when emitted.
    MatrixXd Initial_condition(1, 5);  // I, X, Z, Y, Photon Lost
    if (lost) {
      Initial_condition << 0, 0, 0, 0, 1;  // Photon already lost. Maybe by emission time. Not implemented though.
    } else if (Zerr && Xerr) {
      Initial_condition << 0, 0, 0, 1, 0;  // Has a Y error
    } else if (Zerr && !Xerr) {
      Initial_condition << 0, 0, 1, 0, 0;  // Has a Z error
    } else if (!Zerr && Xerr) {
      Initial_condition << 0, 1, 0, 0, 0;  // Has an X error
    } else {
      Initial_condition << 1, 0, 0, 0, 0;  // No error
    }
    MatrixXd Output_condition(1, 5);
    Output_condition = Initial_condition * Q_to_the_distance;

    // std::cout<<"Q_to_the_distance"<<Q_to_the_distance<<"\n";
    // std::cout<<"Output_condition = "<<Output_condition<<"\n";
    No_error_ceil = Output_condition(0, 0);
    X_error_ceil = No_error_ceil + Output_condition(0, 1);
    Z_error_ceil = X_error_ceil + Output_condition(0, 2);
    Y_error_ceil = Z_error_ceil + Output_condition(0, 3);
    Lost_ceil = Y_error_ceil + Output_condition(0, 4);

    // std::cout<<"NO error ceil = "<<No_error_ceil<<", X = "<<X_error_ceil<<"Z, "<<Z_error_ceil<<", Y = "<<Y_error_ceil<<", Lost = "<<Lost_ceil<<"\n";

    double rand = dblrand();  // Gives a random double between 0.0 ~ 1.0
    /* if(rand<0.5){
         less++;
     }else{
         more++;
     }*/
    // double rand = std::rand()/(RAND_MAX + 1.);
    if (rand < No_error_ceil) {
      // Qubit will end up with no error
    } else if (No_error_ceil <= rand && rand < X_error_ceil && (No_error_ceil != X_error_ceil)) {
      // X error
      bool xerr = q->getPauliXerr();
      q->setPauliXerr(!xerr);  // if xerr already was true, then another x error will make it false
    } else if (X_error_ceil <= rand && rand < Z_error_ceil && (X_error_ceil != Z_error_ceil)) {
      // Z error
      bool zerr = q->getPauliZerr();
      q->setPauliZerr(!zerr);
    } else if (Z_error_ceil <= rand && rand < Y_error_ceil && (Z_error_ceil != Y_error_ceil)) {
      // Y error
      bool xerr = q->getPauliXerr();
      q->setPauliXerr(!xerr);
      bool zerr = q->getPauliZerr();
      q->setPauliZerr(!zerr);
    } else {
      // Photon was lost
      DEBUG_darkcount_count++;
      // std::cout<<"less = "<<less<<", more = "<<more<<"\n";
      // std::cout<<"dbl="<<rand<<" count = "<<DEBUG_darkcount_count<<"\n";
      q->setPhotonLost(true);
    }
    q->setError_random_for_debug(rand);  // For debugging purpose
  } catch (std::exception &e) {
    // error("Only PhotonicQubit is allowed in quantum channel");
    EV << "Only PhotonicQubit is allowed in quantum channel";
  }
}

}  // namespace channels
}  // namespace quisp


/*=============================================================================

  LeastSquaresPackage: A software package for estimating the rotation and translation of rigid bodies.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include <boost/program_options.hpp>
#include "lsqLeastSquare3D.h"
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>

// A helper function to simplify the help command.
template<class T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
{
    std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
    return os;
}

/**
* \file lsqComputeRotTrans.cpp
* \brief End-user application for computing the rotation and translation connecting two sets of 3D points.
* \ingroup applications
*
* This command line application can read from two separate files, each of them containing the coordinates of many 3D points,
* and compute the rotation and translation connecting the two sets. The user can choose between two different methods to
* compute the rotation, and the methods can be selected with a specific option (see help). The two methods are based, respectively,
* on the singular value decomposition of a matrix obtained from the sets of points (K.S. Arun et al., IEEE PAMI-9, 698-700, 1987),
* and on the quaternionic representation of matrices (B.K.P. Horn, J. Opt. Soc. Am. A 4, 629-642, 1987). Rotation and translation
* are saved in file "rotation.dat" and "translation.dat" respectively.
*/
int main(int argc, char* argv[]) {

  try {

    std::string algorithm_strategy;
    std::string output_filename;

    // Allowed options, shown in the help.
    boost::program_options::options_description desc("Options");

    desc.add_options()
      ("help,h", "produce help message")
      ("version,v", "return the version of the application")
      ("method,m", boost::program_options::value< std::string >(&algorithm_strategy)->default_value("svd"),
        "set the algorithm for computing the rotation, either svd or quat")
      ("output,o", boost::program_options::value< std::string >(&output_filename)->default_value("output.dat"),
        "set the name of the output file where rotation and translation are saved")
    ;

    // Hidden options, are allowed on command line, but are not shown to the user.
    boost::program_options::options_description hidden("Hidden options");

    hidden.add_options()
      ("input-files", boost::program_options::value< std::vector<std::string> >(),
        "input files with the coordinates of the two sets of 3D points")
    ;

    boost::program_options::options_description cmdline_options;
    cmdline_options.add(desc).add(hidden);

    boost::program_options::positional_options_description p;
    p.add("input-files", -1);

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(p).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help")) {
      std::cout << "Usage: options_description [options]" << std::endl;
      std::cout << desc;  // The helper function is used by the help to print the list of commands.
      return 0;
    }

    if (vm.count("version")) {
      std::cout << "lsqComputeRotTrans version 0.0.1" << std::endl;
      return 0;
    }

    lsq::LeastSquare3D reference_frames;

    if (vm.count("input-files")) {

      auto input_filenames = vm["input-files"].as<std::vector<std::string>>();

      if (input_filenames.size() != 2) {
        std::cerr << "Provide two files with the sets of coordinates of the 3D points." << std::endl;
        return 1;
      }

      // ---------------------------- Read the coordinates and fill the two sets of 3D points ---------------------------
      std::ifstream filein;
      double x_value, y_value, z_value;
      Eigen::Vector3d point;

      for( int i = 0 ; i < 2 ;  ++i ) {

        filein.open( input_filenames[i] );

        if (!filein.is_open()) {
          std::cerr << "Error opening the input file " << input_filenames[i] << "." << std::endl;
          return 1;
        }

        std::cout << "Input file " << input_filenames[i] << " is read and points are saved..." << std::endl;

        while(!filein.eof()) {
          filein >> x_value >> y_value >> z_value;
          point = {x_value, y_value, z_value};
          if ( i == 0 )
            reference_frames.add_point_first_vector(point);
          else
            reference_frames.add_point_second_vector(point);
        }

        filein.close();
      }

      if ( !reference_frames.same_number_of_points() ) {
        std::cout << "warning: The two sets of points have different size. The additional points will be neglected." << std::endl;
      }

    }
    else {
      std::cerr << "No input files are provided. Provide two files with the sets of coordinates of the 3D points." << std::endl;
      return 1;
    }

    //  ----------------------------------- Set the algorithm to compute the rotation -----------------------------------
    std::unique_ptr<lsq::ComputeRotation> algorithm;

    if ( algorithm_strategy == std::string("svd") ) {
      std::cout << "The chosen algorithm is svd." << std::endl;
      algorithm = std::unique_ptr<lsq::ComputeRotation>( new lsq::SVDMethod() );
    }
    else if ( algorithm_strategy == std::string("quat") ) {
      std::cout << "The chosen algorithm is quat." << std::endl;
      algorithm = std::unique_ptr<lsq::ComputeRotation>( new lsq::QuaternionMethod() );
    }
    else {
      std::cerr << "The chosen method is not available. Refer to the --help for the available methods." << std::endl;
      return 1;
    }

    reference_frames.set_rotation_strategy( std::move(algorithm) );

    // ---------------------- Compute the centroids and the update the sets of points around them -----------------------
    reference_frames.centroid_first_vector();
    reference_frames.centroid_second_vector();
    reference_frames.update_first_points_around_centroid();
    reference_frames.update_second_points_around_centroid();

    // ---------------------------------------------- Compute the H matrix ----------------------------------------------
    reference_frames.compute_H_matrix();

    // ---------------------------------------- Compute rotation and translation ----------------------------------------
    reference_frames.compute_rotation_matrix();
    reference_frames.compute_translation_vector();

    // ----------------------------------------- Save rotation and translation -----------------------------------------
    Eigen::Matrix3d rotation = reference_frames.get_rotation_matrix();
    Eigen::Vector3d translation = reference_frames.get_translation_vector();

    std::ofstream fileout;

    fileout.open( output_filename );

    if (!fileout.is_open()) {
      std::cerr << "Error opening the output file " << output_filename << "." << std::endl;
      return 1;
    }

    std::cout << "Rotation and translation are saved in the output file " << output_filename << "." << std::endl;

    fileout << "The rotation matrix is:" << std::endl;
    fileout << rotation << std::endl;
    fileout << "The translation vector is:" << std::endl;
    fileout << translation << std::endl;

    fileout.close();

    }
    catch(std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        return 1;
    }
    return 0;
}


/*
Copyright <2017> <Benjamin Santos>

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.
*/

#include <iostream>
#include <fstream>
#include <utility>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include <boost/phoenix/core.hpp>

#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>

using namespace std;
using namespace boost::numeric::odeint;
namespace phoenix = boost::phoenix;

// vector type
typedef double value_type;
typedef vector< value_type > state_type;

// system definition
struct stiff_system{

  // constructor, sets a and b
  stiff_system(double a_=-101.0, double b_=-100.0): a(a_), b(b_){  }

  // rhs of ode system
  inline
  void operator()( const state_type &x, state_type &dxdt, const value_type t) const {
    dxdt[0] = a*x[0] + b*x[1];
    dxdt[1] = x[0];
  }
  double a;
  double b;
};

int main(int argc, char **argv) {

  state_type xini( 2.0 , 1.0 );

  // system
  //   stiff_system ssys();
  stiff_system* ssys = new stiff_system();

  auto stepper = make_controlled(1.0e-6, 1.0e-6,
                                runge_kutta_cash_karp54< state_type >());

  size_t num_of_steps = integrate_adaptive(stepper, *ssys, xini,
                                           0.01, 50.0, 0.01,
                        cout << phoenix::arg_names::arg2 << '\t'
                             << phoenix::arg_names::arg1[0] << '\t'
                             << phoenix::arg_names::arg1[1] << '\t'
                             << phoenix::arg_names::arg1[1]
                            *phoenix::arg_names::arg1[1] << "\n" );

  cerr << "\n[ii] Number of steps: " <<  num_of_steps << endl;

  // deallocate memory for ssys
  delete(ssys);

  return 0;
}


#ifndef FLEMU_UTILITY_HPP
#define FLEMU_UTILITY_HPP
#include <boost/ut.hpp>

#include <cmath>
#include <cstdint>
#include <cstring>

#include <concepts>

namespace flemu
{

// Both ends are included.
//                 7654 3210
// mask(1,3) == 0b'0000'1110;
//
template<std::unsigned_integral UInt>
constexpr UInt mask(const std::size_t x, const std::size_t y) noexcept
{
    const auto start = std::min(x, y);
    const auto stop  = std::max(x, y);

    const std::size_t width = stop - start + 1;
    if (width < 8 * sizeof(UInt))
    {
        const auto m = (UInt(1) << width) - 1;
        return m << start;
    }
    else
    {
        return std::numeric_limits<UInt>::max();
    }
}

template<std::unsigned_integral UInt>
constexpr int bit_at(const UInt x, const std::size_t i) noexcept
{
    if (i < 8 * sizeof(UInt))
    {
        return (x & mask<UInt>(i, i)) == 0 ? 0 : 1;
    }
    else
    {
        return 0;
    }
}

template<std::unsigned_integral UInt>
std::string as_bit(const UInt x) noexcept
{
    std::string str;
    const auto width = sizeof(UInt)*8;
    for(std::size_t i=0; i<width; ++i)
    {
        if(i % 4 == 0 && i != 0)
        {
            str += '\'';
        }
        str += bit_at(x, width-i-1) + '0';
    }
    return str;
}

// only g++11 supports std::bit_cast ...
template<typename T, typename U>
T bit_cast(const U& u)
{
    static_assert(sizeof(T) == sizeof(U));
    T t;
    std::memcpy(reinterpret_cast<char*>(std::addressof(t)),
                reinterpret_cast<const char*>(std::addressof(u)),
                sizeof(T));
    return t;
}

#ifdef FLEMU_ACTIVATE_UNIT_TESTS
inline boost::ut::suite tests_utility = [] {
    using namespace boost::ut::literals;

    "mask"_test = [] {
        boost::ut::expect(mask<std::uint32_t>( 1,  3) == 0b00001110);
        boost::ut::expect(mask<std::uint32_t>( 3,  1) == 0b00001110);
        boost::ut::expect(mask<std::uint32_t>( 3,  3) == 0b00001000);
        boost::ut::expect(mask<std::uint32_t>(31,  0) == 0xFFFFFFFF);
        boost::ut::expect(mask<std::uint32_t>( 0, 31) == 0xFFFFFFFF);
        boost::ut::expect(mask<std::uint32_t>(31, 31) == 0x80000000);

        boost::ut::expect(mask<std::uint64_t>(31,  0) == 0xFFFF'FFFF);
        boost::ut::expect(mask<std::uint64_t>(47, 32) == 0x0000'FFFF'0000'0000);
        boost::ut::expect(mask<std::uint64_t>(63,  0) == 0xFFFF'FFFF'FFFF'FFFFull);
        boost::ut::expect(mask<std::uint64_t>(63, 63) == 0x8000'0000'0000'0000ull);
    };
};
#endif

} // flemu
#endif// FLEMU_UTILITY_HPP


#ifndef MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP
#define MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP

#include <random>
#include <exception>
#include <sstream>
#include <string>
#include <cmath>
#include <cstddef>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/assert.hpp>

namespace mpp { namespace hamiltonian {

template<class real_scalar_type>
class multivariate_normal {
public:
    typedef boost::numeric::ublas::vector<real_scalar_type> real_vector_type;
    typedef std::normal_distribution<real_scalar_type> normal_distribution_type;

    explicit multivariate_normal(real_vector_type const & sigma_inv) throw()
    :m_sigma_inv(sigma_inv) {
        if( sigma_inv.size() == std::size_t(0)) {
            std::stringstream msg;
            msg << "The number of dimensions = "
                << m_sigma_inv.size()
                << " of the multivariate normal kinetic energy should be"
                << " greater than zero.";
            throw std::length_error(msg.str());
        }

        for(std::size_t i=0;i<sigma_inv.size();++i) {
            if( not std::isfinite(sigma_inv (i)) ) {
                std::stringstream msg;
                msg << i << "th value of sigma_inv is not finite";
                throw std::out_of_range(msg.str());
            }
        }
    }

    real_scalar_type log_posterior(real_vector_type const & p) const {
        BOOST_ASSERT_MSG( p.size() == m_sigma_inv.size(),
            "p should have the same dimensionality of the log_posterior.");
        real_scalar_type val(0);
        for(std::size_t i=0;i<p.size();++i) {
            val -= p(i)*p(i)*m_sigma_inv(i);
        }
        return real_scalar_type(0.5)*val;
    }

    real_vector_type grad_log_posterior(real_vector_type const & p) const {
        BOOST_ASSERT_MSG( p.size() == m_sigma_inv.size(),
            "p should have the same dimensionality of the log_posterior.");
        real_vector_type dp(p.size());
        for(std::size_t i=0;i<p.size();++i) {
            dp(i) = -m_sigma_inv(i)*p(i);
        }
        return dp;
    }

    template<class rng_type>
    real_vector_type generate_sample(rng_type & rng) {
        real_vector_type sample(m_sigma_inv.size());
        for(std::size_t i=0;i<m_sigma_inv.size();++i) {
            real_scalar_type scale
                = m_sigma_inv(i) > real_scalar_type(0) ?
                    std::sqrt( real_scalar_type(1)/m_sigma_inv(i) ) : 0 ;
            sample(i) = scale*m_norm_dist(rng);
        }
        return sample;
    }
private:
    real_vector_type m_sigma_inv;
    normal_distribution_type m_norm_dist;
};

}}

#endif //MPP_HAMILTONIAN_KINETIC_ENERGY_MULTIVAR_NORMAL_HPP


//  Copyright John Maddock 2007.
//  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 "required_defines.hpp"
#include "performance_measure.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include <boost/array.hpp>

#define T double
#include "../test/erf_data.ipp"
#include "../test/erf_large_data.ipp"
#include "../test/erf_small_data.ipp"

template <std::size_t N>
double erf_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::erf(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(erf_test, "erf")
{
   double result = erf_evaluate2(erf_data);
   result += erf_evaluate2(erf_large_data);
   result += erf_evaluate2(erf_small_data);

   consume_result(result);
   set_call_count((sizeof(erf_data) + sizeof(erf_large_data) + sizeof(erf_small_data)) / sizeof(erf_data[0]));
}

template <std::size_t N>
double erf_inv_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::erf_inv(data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(erf_inv_test, "erf_inv")
{
   double result = erf_inv_evaluate2(erf_data);
   result += erf_inv_evaluate2(erf_large_data);
   result += erf_inv_evaluate2(erf_small_data);

   consume_result(result);
   set_call_count((sizeof(erf_data) + sizeof(erf_large_data) + sizeof(erf_small_data)) / sizeof(erf_data[0]));
}

#ifdef TEST_CEPHES

extern "C" {

double erf(double);

}

template <std::size_t N>
double erf_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += erf(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(erf_test, "erf-cephes")
{
   double result = erf_evaluate_cephes(erf_data);
   result += erf_evaluate_cephes(erf_large_data);
   result += erf_evaluate_cephes(erf_small_data);

   consume_result(result);
   set_call_count((sizeof(erf_data) + sizeof(erf_large_data) + sizeof(erf_small_data)) / sizeof(erf_data[0]));
}

#endif

#ifdef TEST_GSL

#include <gsl/gsl_sf.h>

template <std::size_t N>
double erf_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_erf (data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(erf_test, "erf-gsl")
{
   double result = erf_evaluate_gsl(erf_data);
   result += erf_evaluate_gsl(erf_large_data);
   result += erf_evaluate_gsl(erf_small_data);

   consume_result(result);
   set_call_count((sizeof(erf_data) + sizeof(erf_large_data) + sizeof(erf_small_data)) / sizeof(erf_data[0]));
}

#endif


// Author: Dmitry Anisimov.
// In this test we compute mean value coordinates at some particular points,
// where the computation might break. The used polygon is a concave polygon with 7 vertices.
// We also use inexact kernel and epsilon = 1.0e-15.

// Works with an exact kernel, too.

#include <cmath>
#include <cassert>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Barycentric_coordinates_2/Mean_value_2.h>
#include <CGAL/Barycentric_coordinates_2/Generalized_barycentric_coordinates_2.h>
#include <boost/math/special_functions/fpclassify.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef Kernel::FT      Scalar;
typedef Kernel::Point_2 Point;

typedef std::vector<Scalar> Coordinate_vector;
typedef std::vector<Point>  Point_vector;

typedef std::back_insert_iterator<Coordinate_vector> Vector_insert_iterator;

typedef CGAL::Barycentric_coordinates::Mean_value_2<Kernel> Mean_value;
typedef CGAL::Barycentric_coordinates::Generalized_barycentric_coordinates_2<Mean_value, Kernel> Mean_value_coordinates;

typedef boost::optional<Vector_insert_iterator> Output_type;

using std::cout; using std::endl; using std::string;

int main()
{
    Point_vector vertices(7);

    vertices[0] = Point(0, 0);                                     vertices[1] = Point(1, 1);
    vertices[2] = Point(Scalar(7)/Scalar(4), Scalar(1)/Scalar(2)); vertices[3] = Point(Scalar(7)/Scalar(4), Scalar(5)/Scalar(2));
    vertices[4] = Point(1, 2);                                     vertices[5] = Point(0, 3);
    vertices[6] = Point(Scalar(1)/Scalar(2), Scalar(3)/Scalar(2));

    Mean_value_coordinates mean_value_coordinates(vertices.begin(), vertices.end());

    const Point query_points[11] = { Point(Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            , Scalar(2) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            ),
                                     Point(Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            , Scalar(1) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))            ),
                                     Point(1                                                                  , Scalar(3) / Scalar(2)                                              ),
                                     Point(Scalar(5) / Scalar(4)                                              , Scalar(5) / Scalar(4)                                              ),
                                     Point(Scalar(5) / Scalar(4)                                              , Scalar(7) / Scalar(4)                                              ),
                                     Point(Scalar(3) / Scalar(2)                                              , Scalar(3) / Scalar(2)                                              ),

                                     Point(Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(7) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(5) / Scalar(4) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),

                                     Point(Scalar(3) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(3) / Scalar(4) + (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(3) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(9) / Scalar(4) - (Scalar(1) / Scalar(std::pow(10.0, 300.0)))),
                                     Point(Scalar(1) / Scalar(2) + (Scalar(1) / Scalar(std::pow(10.0, 300.0))), Scalar(3) / Scalar(2)                                              )
                                   };

    Coordinate_vector coordinates;

    int count = 0;
    const Scalar epsilon = Scalar(1) / Scalar(std::pow(10.0, 15.0));

    for(int i = 0; i < 11; ++i) {
        const Output_type result = mean_value_coordinates(query_points[i], coordinates);

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 0])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 0])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 1])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 1])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 2])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 2])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 3])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 3])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 4])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 4])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 5])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 5])));

        assert(!boost::math::isnan(CGAL::to_double(coordinates[count + 6])));
        assert(!boost::math::isinf(CGAL::to_double(coordinates[count + 6])));

        const Scalar coordinate_sum = coordinates[count + 0] +
                                      coordinates[count + 1] +
                                      coordinates[count + 2] +
                                      coordinates[count + 3] +
                                      coordinates[count + 4] +
                                      coordinates[count + 5] +
                                      coordinates[count + 6] ;

        const Point linear_combination( vertices[0].x()*coordinates[count + 0] +
                                        vertices[1].x()*coordinates[count + 1] +
                                        vertices[2].x()*coordinates[count + 2] +
                                        vertices[3].x()*coordinates[count + 3] +
                                        vertices[4].x()*coordinates[count + 4] +
                                        vertices[5].x()*coordinates[count + 5] +
                                        vertices[6].x()*coordinates[count + 6] ,
                                        vertices[0].y()*coordinates[count + 0] +
                                        vertices[1].y()*coordinates[count + 1] +
                                        vertices[2].y()*coordinates[count + 2] +
                                        vertices[3].y()*coordinates[count + 3] +
                                        vertices[4].y()*coordinates[count + 4] +
                                        vertices[5].y()*coordinates[count + 5] +
                                        vertices[6].y()*coordinates[count + 6] );

        const Point difference(linear_combination.x() - query_points[i].x(), linear_combination.y() - query_points[i].y());

        assert( ((coordinate_sum - Scalar(1)) < epsilon) && difference.x() < epsilon && difference.y() < epsilon );

        if( ((coordinate_sum - Scalar(1)) > epsilon) || difference.x() > epsilon || difference.y() > epsilon )
        {
            cout << endl << "MV_special_points_test: FAILED." << endl << endl;
            exit(EXIT_FAILURE);
        }
        count += 7;
    }

    cout << endl << "MV_special_points_test: PASSED." << endl << endl;

    return EXIT_SUCCESS;
}